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

Example 31 with Matrix3d

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

the class GLViewer method restoreDefaultState.

public void restoreDefaultState(boolean strictChecking) {
    // check draw mode
    if (myMappingsSet) {
        if (myColorMapProps != null) {
            setColorMap(null);
        }
        if (myNormalMapProps != null) {
            setNormalMap(null);
        }
        if (myBumpMapProps != null) {
            setBumpMap(null);
        }
        myMappingsSet = false;
    }
    if (myNonDefaultColorSettings != 0) {
        if (myBackColor != null) {
            setBackColor(null);
        }
        if ((myNonDefaultColorSettings & EMISSION_BIT) != 0) {
            setEmission(DEFAULT_MATERIAL_EMISSION);
        }
        if ((myNonDefaultColorSettings & SPECULAR_BIT) != 0) {
            setSpecular(DEFAULT_MATERIAL_SPECULAR);
        }
        if ((myNonDefaultColorSettings & SHININESS_BIT) != 0) {
            setShininess(DEFAULT_MATERIAL_SHININESS);
        }
        if (getColorInterpolation() != DEFAULT_COLOR_INTERPOLATION) {
            setColorInterpolation(DEFAULT_COLOR_INTERPOLATION);
        }
        if (myActiveColor == ActiveColor.HIGHLIGHT) {
            setHighlighting(false);
        }
        myNonDefaultColorSettings = 0;
    }
    if (myNonDefaultGeneralSettings != 0) {
        if (myViewerState.faceMode != DEFAULT_FACE_STYLE) {
            setFaceStyle(DEFAULT_FACE_STYLE);
        }
        if ((myNonDefaultGeneralSettings & LINE_WIDTH_BIT) != 0) {
            setLineWidth(DEFAULT_LINE_WIDTH);
        }
        if ((myNonDefaultGeneralSettings & POINT_SIZE_BIT) != 0) {
            setPointSize(DEFAULT_POINT_SIZE);
        }
        if (myViewerState.shading != DEFAULT_SHADING) {
            setShading(DEFAULT_SHADING);
        }
        // }
        if (myViewerState.colorMixing != DEFAULT_COLOR_MIXING) {
            setVertexColorMixing(DEFAULT_COLOR_MIXING);
        }
        if (getDepthOffset() != DEFAULT_DEPTH_OFFSET) {
            setDepthOffset(DEFAULT_DEPTH_OFFSET);
        }
        myNonDefaultGeneralSettings = 0;
    }
    if (myModelMatrixSet) {
        int mmsize = modelMatrixStack.size();
        if (rendering2d) {
            mmsize -= 1;
        }
        if (mmsize > 0) {
            if (strictChecking) {
                throw new IllegalStateException("render() method exited with model matrix stack size of " + mmsize);
            } else {
                while (mmsize > 0) {
                    modelMatrixStack.pop();
                    mmsize--;
                }
            }
        }
        if (rendering2d) {
            synchronized (modelMatrix) {
                if (!modelMatrix.equals(myDefaultModelMatrix2d)) {
                    // reset to default
                    modelMatrix.set(myDefaultModelMatrix2d);
                    invalidateModelMatrix();
                }
            }
        } else {
            synchronized (modelMatrix) {
                if (!modelMatrix.isIdentity()) {
                    // reset to identity
                    modelMatrix = new RigidTransform3d();
                    modelNormalMatrix = new Matrix3d();
                    invalidateModelMatrix();
                }
            }
        }
        myModelMatrixSet = false;
    }
    if (myDrawMode != null) {
        if (strictChecking) {
            throw new IllegalStateException("render() method exited while still in draw mode: " + myDrawMode);
        } else {
            resetDraw();
        }
    }
    // set front alpha if not one
    if (myCurrentMaterial.isTransparent()) {
        setFrontAlpha(1.0f);
    }
}

Example 32 with Matrix3d

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

the class GLViewer 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 33 with Matrix3d

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

the class SpatialInertia method invert.

/**
 * Computes the inverse of a spatial inertia, stored in M, and
 * return the result in MI.
 *
 * @param MI Matrix returning the spatial inertia
 * @param M Inertia to invert
 */
public static void invert(Matrix6d MI, Matrix6d M) {
    double m = M.m00;
    double mcx = +M.m15;
    double mcy = -M.m05;
    double mcz = +M.m04;
    double cx = mcx / m;
    double cy = mcy / m;
    double cz = mcz / m;
    Vector3d c = new Vector3d(cx, cy, cz);
    Matrix3d X = new Matrix3d();
    // start by setting X to J
    X.m00 = M.m33 - mcy * cy - mcz * cz;
    X.m11 = M.m44 - mcx * cx - mcz * cz;
    X.m22 = M.m55 - mcx * cx - mcy * cy;
    X.m01 = M.m34 + mcx * cy;
    X.m02 = M.m35 + mcx * cz;
    X.m12 = M.m45 + mcy * cz;
    X.m10 = X.m01;
    X.m20 = X.m02;
    X.m21 = X.m12;
    // invert to get J^{-1}
    X.invert();
    // set lower right block to J^{-1}
    MI.m33 = X.m00;
    MI.m44 = X.m11;
    MI.m55 = X.m22;
    MI.m34 = X.m01;
    MI.m35 = X.m02;
    MI.m45 = X.m12;
    MI.m43 = X.m01;
    MI.m53 = X.m02;
    MI.m54 = X.m12;
    // set upper right block to [c] J^{-1}
    X.crossProduct(c, X);
    MI.m03 = X.m00;
    MI.m04 = X.m01;
    MI.m05 = X.m02;
    MI.m13 = X.m10;
    MI.m14 = X.m11;
    MI.m15 = X.m12;
    MI.m23 = X.m20;
    MI.m24 = X.m21;
    MI.m25 = X.m22;
    // set lower left block to ([c] J^{-1})^T
    MI.m30 = X.m00;
    MI.m40 = X.m01;
    MI.m50 = X.m02;
    MI.m31 = X.m10;
    MI.m41 = X.m11;
    MI.m51 = X.m12;
    MI.m32 = X.m20;
    MI.m42 = X.m21;
    MI.m52 = X.m22;
    // set upper left block to 1/m I - [c] J^{-1} [c]
    X.crossProduct(X, c);
    MI.m00 = -X.m00 + 1 / m;
    MI.m01 = -X.m01;
    MI.m02 = -X.m02;
    MI.m10 = -X.m10;
    MI.m11 = -X.m11 + 1 / m;
    MI.m12 = -X.m12;
    MI.m20 = -X.m20;
    MI.m21 = -X.m21;
    MI.m22 = -X.m22 + 1 / m;
}

Example 34 with Matrix3d

use of maspack.matrix.Matrix3d 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 35 with Matrix3d

use of maspack.matrix.Matrix3d 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;
}