Examples with RotationMatrix3d maspack.matrix.RotationMatrix3d used on opensource projects

Example 41 with RotationMatrix3d

use of maspack.matrix.RotationMatrix3d in project artisynth_core by artisynth.

the class SpatialInertiaTest method test.

public void test() throws Exception {
    SpatialInertia M1 = new SpatialInertia();
    SpatialInertia M2 = new SpatialInertia();
    SpatialInertia M3 = new SpatialInertia();
    double mass;
    Point3d com = new Point3d();
    SymmetricMatrix3d J = new SymmetricMatrix3d();
    Matrix3d F = new Matrix3d();
    for (int i = 0; i < 100; i++) {
        M1.setRandom(-0.5, 0.5, randGen);
        String s = M1.toString();
        M2.scan(new ReaderTokenizer(new StringReader(s)));
        checkEqual("toString/scan MASS_INERTIA check", M2, M1, EPS);
        s = M1.toString("%g", SpatialInertia.MATRIX_STRING);
        M2.scan(new ReaderTokenizer(new StringReader(s)));
        checkEqual("toString/scan MATRIX_STRING check", M2, M1, EPS);
        MatrixNd MA = new MatrixNd(0, 0);
        MatrixNd MB = new MatrixNd(0, 0);
        MA.set(M1);
        M2.set(MA);
        mass = randGen.nextDouble();
        com.setRandom(-0.5, 0.5, randGen);
        F.setRandom(-0.5, 0.5, randGen);
        J.mulTransposeLeft(F);
        M1.setMass(mass);
        M1.setCenterOfMass(com);
        M1.setRotationalInertia(J);
        checkComponents("set component check", M1, mass, com, J);
        M2.set(M1);
        checkComponents("set check", M2, mass, com, J);
        M1.set(mass, J, com);
        checkComponents("set check", M1, mass, com, J);
        M1.set(mass, J);
        com.setZero();
        checkComponents("set check", M1, mass, com, J);
        M1.setRandom();
        MA.set(M1);
        MB.set(M2);
        // check addition
        M2.add(M1);
        MB.add(MA);
        M3.set(MB);
        checkEqual("add check", M2, M3, EPS);
        // check subtraction
        MB.sub(MA);
        M2.sub(M1);
        M3.set(MB);
        checkEqual("sub check", M2, M3, EPS);
        // check scaling
        MB.scale(3);
        M2.scale(3);
        M3.set(MB);
        checkEqual("scale check", M2, M3, EPS);
        MatrixNd invM2 = new MatrixNd(0, 0);
        MatrixNd invMB = new MatrixNd(0, 0);
        // check inversion
        MB.set(M2);
        M2.getInverse(invM2);
        invMB.invert(MB);
        double eps = invM2.frobeniusNorm() * EPS;
        checkEqual("inverse check", invM2, invMB, eps);
        Matrix6d invM6 = new Matrix6d();
        SpatialInertia.invert(invM6, M2);
        checkEqual("inverse check", invM6, invMB, eps);
        CholeskyDecomposition chol = new CholeskyDecomposition();
        chol.factor(MB);
        MatrixNd U = new MatrixNd(6, 6);
        MatrixNd L = new MatrixNd(6, 6);
        chol.get(L);
        U.transpose(L);
        MatrixNd invU = new MatrixNd(6, 6);
        MatrixNd invL = new MatrixNd(6, 6);
        invU.invert(U);
        invL.transpose(invU);
        VectorNd vec = new VectorNd(6);
        VectorNd res = new VectorNd(6);
        Twist tw1 = new Twist();
        Twist twr = new Twist();
        Twist twrCheck = new Twist();
        Wrench wr1 = new Wrench();
        Wrench wrr = new Wrench();
        Wrench wrrCheck = new Wrench();
        // check multiply by vector (twist)
        tw1.setRandom();
        M2.mul(wrr, tw1);
        vec.set(tw1);
        MB.mul(res, vec);
        wrrCheck.set(res);
        checkEqual("mul check", wrr, wrrCheck, EPS);
        // check inverse multiply by vector (wrench)
        wr1.setRandom();
        M2.mulInverse(twr, wr1);
        vec.set(wr1);
        invMB.mul(res, vec);
        twrCheck.set(res);
        checkEqual("inverse mul check", twr, twrCheck, eps);
        // check inverse multiply by vector (twist, twist)
        wr1.setRandom();
        twr.set(wr1);
        M2.mulInverse(twr, twr);
        vec.set(wr1);
        invMB.mul(res, vec);
        twrCheck.set(res);
        checkEqual("inverse mul check", twr, twrCheck, eps);
        // check right factor multiply by vector
        tw1.setRandom();
        M2.mulRightFactor(twr, tw1);
        vec.set(tw1);
        U.mul(res, vec);
        twrCheck.set(res);
        checkEqual("mul right factor", twr, twrCheck, EPS);
        M2.mulRightFactor(tw1, tw1);
        checkEqual("mul right factor (twr == tw1)", tw1, twrCheck, EPS);
        // check right factor inverse multiply by vector
        tw1.setRandom();
        M2.mulRightFactorInverse(twr, tw1);
        vec.set(tw1);
        invU.mul(res, vec);
        twrCheck.set(res);
        checkEqual("mul right factor inverse", twr, twrCheck, eps);
        M2.mulRightFactorInverse(tw1, tw1);
        checkEqual("mul right factor inverse (twr == tw1)", tw1, twrCheck, eps);
        // check left factor multiply by vector
        wr1.setRandom();
        M2.mulLeftFactor(wrr, wr1);
        vec.set(wr1);
        L.mul(res, vec);
        wrrCheck.set(res);
        checkEqual("mul left factor", wrr, wrrCheck, EPS);
        M2.mulLeftFactor(wr1, wr1);
        checkEqual("mul left factor (wrr == wr1)", wr1, wrrCheck, EPS);
        // check left factor inverse multiply by vector
        wr1.setRandom();
        M2.mulLeftFactorInverse(wrr, wr1);
        vec.set(wr1);
        invL.mul(res, vec);
        wrrCheck.set(res);
        checkEqual("mul left factor inverse", wrr, wrrCheck, eps);
        M2.mulLeftFactorInverse(wr1, wr1);
        checkEqual("mul left factor inverse (wrr == wr1)", wr1, wrrCheck, eps);
        // check addition of point mass
        Matrix6d M6 = new Matrix6d();
        SpatialInertia MCheck = new SpatialInertia();
        MCheck.set(M1);
        M6.set(M1);
        Point3d com1 = new Point3d();
        double mass1 = randGen.nextDouble();
        com1.setRandom(-0.5, 0.5, randGen);
        Point3d com2 = new Point3d();
        double mass2 = randGen.nextDouble();
        com2.setRandom(-0.5, 0.5, randGen);
        M3.setPointMass(mass1, com1);
        MCheck.add(M3);
        M3.setPointMass(mass2, com2);
        MCheck.add(M3);
        M1.addPointMass(mass1, com1);
        M1.addPointMass(mass2, com2);
        checkEqual("point mass addition", M1, MCheck, EPS);
        SpatialInertia.addPointMass(M6, mass1, com1);
        SpatialInertia.addPointMass(M6, mass2, com2);
        Matrix6d MM = new Matrix6d();
        MM.sub(MCheck, M6);
        checkEqual("point mass addition", M6, MCheck, EPS);
        // check rotation transforms
        RotationMatrix3d R = new RotationMatrix3d();
        SpatialInertia MSave = new SpatialInertia(M1);
        R.setRandom();
        M1.getRotated(M6, R);
        MCheck.set(M1);
        MCheck.transform(R);
        checkEqual("getRotated", M6, MCheck, EPS);
        MCheck.inverseTransform(R);
        checkEqual("inverseTransform", MCheck, MSave, EPS);
        R.transpose();
        M1.set(M6);
        M1.getRotated(M6, R);
        // should be back to original
        checkEqual("getRotated (back)", M6, MSave, EPS);
    }
// // test creation of inertia from a mesh
// double density = 20;
// double wx = 3.0;
// double wy = 1.0;
// double wz = 2.0;
// SpatialInertia boxInertia =
// SpatialInertia.createBoxInertia (20 * wx * wy * wz, wx, wy, wz);
// PolygonalMesh boxMesh = MeshFactory.createQuadBox (wx, wy, wz);
// SpatialInertia meshInertia =
// SpatialInertia.createClosedVolumeInertia (boxMesh, density);
// 
// checkMeshInertia (boxMesh, density, boxInertia);
// 
// SpatialInertia innerBoxInertia =
// SpatialInertia.createBoxInertia (1.0 * density, 1, 1, 1);
// boxInertia.setBox (20 * wx * wy * wz, wx, wy, wz);
// boxInertia.sub (innerBoxInertia);
// boxMesh = buildMeshWithHole (wx, wy, wz);
// 
// checkMeshInertia (boxMesh, density, boxInertia);
// 
// boxMesh.triangulate();
// checkMeshInertia (boxMesh, density, boxInertia);
}

Example 42 with RotationMatrix3d

use of maspack.matrix.RotationMatrix3d in project artisynth_core by artisynth.

the class SphericalCoupling method getRpy.

public void getRpy(Vector3d angs, RigidTransform3d TGD) {
    // on entry, TGD is set to TCD. It is then projected to TGD
    projectToConstraint(TGD, TGD);
    double[] rpy = new double[3];
    RotationMatrix3d RDC = new RotationMatrix3d();
    RDC.transpose(TGD.R);
    doGetRpy(rpy, RDC);
    angs.x = rpy[2];
    angs.y = rpy[1];
    angs.z = rpy[0];
}

Example 43 with RotationMatrix3d

use of maspack.matrix.RotationMatrix3d in project artisynth_core by artisynth.

the class RotatableScaler3d method mouseDragged.

public boolean mouseDragged(MouseRayEvent e) {
    if (mySelectedComponent != NONE) {
        // boolean constrained = dragIsConstrained (e);
        // boolean repositioning = dragIsRepositioning (e);
        boolean constrained = dragIsConstrained();
        boolean repositioning = dragIsRepositioning();
        if (mySelectedComponent == X_ROTATE || mySelectedComponent == Y_ROTATE || mySelectedComponent == Z_ROTATE) {
            RotationMatrix3d R = new RotationMatrix3d();
            findRotation(R, myRotPnt, e.getRay());
            updateRotation(R, constrained, repositioning);
        } else {
            Point3d pnt = new Point3d();
            intersectRayAndFixture(pnt, e.getRay());
            updatePosition(pnt, constrained, repositioning);
        }
        if (!repositioning) {
            fireDraggerMoveListeners(myTransform, myIncrementalTransform, e.getModifiersEx());
        }
        return true;
    }
    return false;
}

Example 44 with RotationMatrix3d

use of maspack.matrix.RotationMatrix3d in project artisynth_core by artisynth.

the class StlRenderer method computeInverseTranspose.

protected Matrix3d computeInverseTranspose(Matrix3dBase M) {
    Matrix3d out = new Matrix3d(M);
    if (!(M instanceof RotationMatrix3d)) {
        boolean success = out.invert();
        if (!success) {
            SVDecomposition3d svd3 = new SVDecomposition3d(M);
            svd3.pseudoInverse(out);
        }
        out.transpose();
    }
    return out;
}

Example 45 with RotationMatrix3d

use of maspack.matrix.RotationMatrix3d in project artisynth_core by artisynth.

the class CustomSphericalCoupling method getConstraintInfo.

@Override
public void getConstraintInfo(ConstraintInfo[] info, RigidTransform3d TGD, RigidTransform3d TCD, RigidTransform3d XERR, boolean setEngaged) {
    // projectToConstraint(TGD, TCD);
    // info[0].bilateral = true;
    // info[1].bilateral = true;
    // info[2].bilateral = true;
    // info[3].bilateral = false;
    // info[4].bilateral = false;
    // info[5].bilateral = false;
    // myXFC.mulInverseLeft(TGD, TCD);
    myErr.set(XERR);
    setDistancesAndZeroDerivatives(info, 3, myErr);
    if (myRangeType == TILT_LIMIT) {
        Vector3d utilt = new Vector3d();
        // Tilt axis is is z(C) x z(D).
        // In C coordinates, z(C) = (0,0,1) and z(D) = last row of TGD.R.
        // In D coordinates, z(D) = (0,0,1) and z(C) = last col of TGD.R.
        // D coordinates
        utilt.set(TGD.R.m12, -TGD.R.m02, 0);
        // utilt.set (-TGD.R.m21, TGD.R.m20, 0); // in C coordinates
        double ulen = utilt.norm();
        double theta = 0;
        if (ulen > 1e-8) {
            theta = Math.atan2(ulen, TGD.R.m22);
            utilt.scale(1 / ulen);
        }
        if (setEngaged) {
            if (theta > myMaxTilt) {
                info[3].engaged = 1;
            }
        }
        if (info[3].engaged != 0) {
            info[3].distance = myMaxTilt - theta;
            utilt.inverseTransform(TGD.R);
            info[3].wrenchC.set(0, 0, 0, utilt.x, utilt.y, utilt.z);
            info[3].dotWrenchC.setZero();
        }
    } else if (myRangeType == ROTATION_LIMIT) {
        Vector3d u = new Vector3d();
        double ang = TGD.R.getAxisAngle(u);
        // paranoid
        u.normalize();
        Vector3d a = new Vector3d(u.x * myMaxRotX, u.y * myMaxRotY, u.z * myMaxRotZ);
        double maxAng = a.norm();
        if (setEngaged) {
            if (ang > 0 && ang > maxAng) {
                info[3].engaged = 1;
            }
        }
        if (info[3].engaged != 0) {
            info[3].distance = maxAng - ang;
            u.x /= myMaxRotX;
            u.y /= myMaxRotY;
            u.z /= myMaxRotZ;
            u.normalize();
            info[3].wrenchC.set(0, 0, 0, -u.x, -u.y, -u.z);
            // TODO set this
            info[3].dotWrenchC.setZero();
        }
    } else if (myRangeType == CUSTOM_LIMIT) {
        // get relative z axis
        double[] z = new double[3];
        // z(C) in D coordinates
        TGD.R.getColumn(2, z);
        // check if X inside curve, and project to boundary if outside
        boolean engage = false;
        double[] zNew = { z[0], z[1], z[2] };
        Vector3d vAxis = new Vector3d(1, 0, 0);
        if (!myCurve.isWithin(z)) {
            double theta2 = myCurve.findClosestPoint(z, zNew);
            if (theta2 > 1e-5) {
                engage = true;
            }
        }
        Vector3d v1 = new Vector3d(z);
        Vector3d v2 = new Vector3d(zNew);
        double theta = Math.acos(v1.dot(v2));
        if (Math.toDegrees(theta) > 20) {
            System.out.printf("Z: (%f,%f,%f), Zn: (%f,%f,%f),theta=%f\n", z[0], z[1], z[2], zNew[0], zNew[1], zNew[2], Math.toDegrees(theta));
        }
        if (theta > 1e-7) {
            vAxis.cross(v1, v2);
            // transform to C coordinates?
            vAxis.inverseTransform(TGD);
        } else {
            theta = 0;
            engage = false;
        }
        // maybe set engaged
        if (setEngaged) {
            if (engage) {
                info[3].engaged = 1;
            } else {
                info[3].engaged = 0;
            }
        }
        if (info[3].engaged != 0) {
            vAxis.normalize();
            info[3].distance = -theta;
            info[3].wrenchC.set(0, 0, 0, vAxis.x, vAxis.y, vAxis.z);
            info[3].dotWrenchC.setZero();
        }
    } else if (myRangeType == RPY_LIMIT) {
        double roll, pitch, yaw;
        double[] rpy = new double[3];
        double[] rpyOrig = new double[3];
        for (int i = 0; i < 3; i++) {
            rpyOrig[i] = info[i + 3].coordinate;
        }
        RotationMatrix3d RDC = new RotationMatrix3d();
        Vector3d wBA = new Vector3d();
        RDC.transpose(TGD.R);
        doGetRpy(rpy, RDC);
        roll = rpy[0];
        pitch = rpy[1];
        yaw = rpy[2];
        double cr = Math.cos(roll);
        double sr = Math.sin(roll);
        double cp = Math.cos(pitch);
        double sp = Math.sin(pitch);
        double denom = cp;
        // the singularity at cp = 0
        if (Math.abs(denom) < 0.0001) {
            denom = (denom >= 0 ? 0.0001 : -0.0001);
        }
        double tp = sp / denom;
        // Don't need to transform because vel is now in Frame C
        // get angular velocity of B with respect to A in frame C
        // if (!myComputeVelInFrameC) {
        // wBA.transform(RDC, myVelBA.w);
        // }
        info[3].distance = 0;
        info[4].distance = 0;
        info[5].distance = 0;
        double dotp = -sr * wBA.x + cr * wBA.y;
        double doty = (cr * wBA.x + sr * wBA.y) / denom;
        double dotr = wBA.z + sp * doty;
        if (setEngaged) {
            maybeSetEngaged(info[3], roll, myMinRoll, myMaxRoll);
            maybeSetEngaged(info[4], pitch, myMinPitch, myMaxPitch);
            maybeSetEngaged(info[5], yaw, myMinYaw, myMaxYaw);
        }
        if (info[3].engaged != 0) {
            info[3].distance = getDistance(roll, myMinRoll, myMaxRoll);
            info[3].wrenchC.set(0, 0, 0, sp * cr / denom, sp * sr / denom, 1);
            info[3].dotWrenchC.f.setZero();
            double tt = (1 + tp * tp);
            info[3].dotWrenchC.m.set(-sr * tp * dotr + tt * cr * dotp, cr * tp * dotr + tt * sr * dotp, 0);
            // checkDeriv ("roll", info[3], conR);
            if (info[3].engaged == -1) {
                info[3].wrenchC.negate();
                info[3].dotWrenchC.negate();
            }
        }
        if (info[4].engaged != 0) {
            info[4].distance = getDistance(pitch, myMinPitch, myMaxPitch);
            info[4].wrenchC.set(0, 0, 0, -sr, cr, 0);
            info[4].dotWrenchC.f.setZero();
            info[4].dotWrenchC.m.set(-cr * dotr, -sr * dotr, 0);
            // checkDeriv ("pitch", info[4], conP);
            if (info[4].engaged == -1) {
                info[4].wrenchC.negate();
                info[4].dotWrenchC.negate();
            }
        }
        if (info[5].engaged != 0) {
            info[5].distance = getDistance(yaw, myMinYaw, myMaxYaw);
            info[5].wrenchC.set(0, 0, 0, cr / denom, sr / denom, 0);
            info[5].dotWrenchC.f.setZero();
            info[5].dotWrenchC.m.set((-sr * dotr + cr * tp * dotp) / denom, (cr * dotr + sr * tp * dotp) / denom, 0);
            // checkDeriv ("yaw", info[5], conY);
            if (info[5].engaged == -1) {
                info[5].wrenchC.negate();
                info[5].dotWrenchC.negate();
            }
        }
        boolean bigshot = false;
        for (int i = 3; i < 6; i++) {
            if (info[i].engaged != 0) {
                if (info[i].distance < -MAX_DISTANCE) {
                    bigshot = true;
                    break;
                }
            }
        }
        if (bigshot) {
            System.out.println("Large jumps..");
            System.out.printf("Angles: %f, %f, %f\n", Math.toDegrees(rpy[0]), Math.toDegrees(rpy[1]), Math.toDegrees(rpy[2]));
            System.out.printf("Min   : %f, %f, %f\n", Math.toDegrees(myMinRoll), Math.toDegrees(myMinPitch), Math.toDegrees(myMinYaw));
            System.out.printf("Max   : %f, %f, %f\n", Math.toDegrees(myMaxRoll), Math.toDegrees(myMaxPitch), Math.toDegrees(myMaxYaw));
            System.out.printf("On    : %d, %d, %d\n", info[3].engaged, info[4].engaged, info[5].engaged);
            System.out.printf("Dist  : %f, %f, %f\n", Math.toDegrees(info[3].distance), Math.toDegrees(info[4].distance), Math.toDegrees(info[5].distance));
        }
    } else if (myRangeType != 0) {
        throw new InternalErrorException("Unimplemented range limits " + NumberFormat.formatHex(myRangeType));
    }
}