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

Example 1 with Matrix3d

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

the class MeshBase method inverseTransform.

/**
 * Applies an inverse affine transformation to the vertices of this mesh, in
 * local mesh coordinates. The topology of the mesh remains unchanged.
 *
 * @param X
 * affine transformation
 */
public void inverseTransform(AffineTransform3dBase X) {
    for (Vertex3d vertex : myVertices) {
        vertex.pnt.inverseTransform(X);
    }
    if (myNormalsExplicitP) {
        Matrix3d A = new Matrix3d(X.getMatrix());
        A.transpose();
        for (int i = 0; i < myNormals.size(); i++) {
            Vector3d nrm = myNormals.get(i);
            A.mul(nrm, nrm);
            nrm.normalize();
        }
    } else {
        // auto normals will be regenerated
        clearNormals();
    }
    invalidateBoundingInfo();
    notifyModified();
}

Example 2 with Matrix3d

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

the class PolygonalMesh method computePrincipalAxes.

public static RigidTransform3d computePrincipalAxes(PolygonalMesh mesh) {
    Vector3d mov1 = new Vector3d();
    Vector3d mov2 = new Vector3d();
    Vector3d pov = new Vector3d();
    double vol = mesh.computeVolumeIntegrals(mov1, mov2, pov);
    double mass = vol;
    Point3d cov = new Point3d();
    // center of volume
    cov.scale(1.0 / vol, mov1);
    // [c], skew symmetric
    Matrix3d covMatrix = new Matrix3d(0, -cov.z, cov.y, cov.z, 0, -cov.x, -cov.y, cov.x, 0);
    // J
    Matrix3d J = new Matrix3d((mov2.y + mov2.z), -pov.z, -pov.y, -pov.z, (mov2.x + mov2.z), -pov.x, -pov.y, -pov.x, (mov2.x + mov2.y));
    // Jc = J + m[c][c]
    Matrix3d Jc = new Matrix3d();
    Jc.mul(covMatrix, covMatrix);
    Jc.scale(mass);
    Jc.add(J);
    // Compute eigenvectors and eigenvlaues of Jc
    SymmetricMatrix3d JcSymmetric = new SymmetricMatrix3d(Jc);
    Vector3d lambda = new Vector3d();
    Matrix3d U = new Matrix3d();
    JcSymmetric.getEigenValues(lambda, U);
    // Construct the rotation matrix
    RotationMatrix3d R = new RotationMatrix3d();
    R.set(U);
    lambda.absolute();
    if (lambda.x > lambda.y && lambda.z > lambda.y) {
        R.rotateZDirection(new Vector3d(R.m01, R.m11, R.m21));
    } else if (lambda.x > lambda.z && lambda.y > lambda.z) {
        R.rotateZDirection(new Vector3d(R.m00, R.m10, R.m20));
    }
    return (new RigidTransform3d(cov, R));
}

Example 3 with Matrix3d

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

the class OBB method setTransform.

/**
 * Sets the transform for this OBB from a covariance matrix and centroid.
 */
private void setTransform(Matrix3d C, Point3d cent) {
    SymmetricMatrix3d Csym = new SymmetricMatrix3d(C.m00, C.m11, C.m22, C.m01, C.m02, C.m12);
    Matrix3d U = new Matrix3d();
    Vector3d sig = new Vector3d();
    Matrix3d V = new Matrix3d();
    // Might want to do this factorization with SVDecomposition3d, but
    // SymmetricMatrix3d.getSVD() is a bit faster
    Csym.getSVD(U, sig, V);
    if (U.determinant() < 0) {
        U.m02 = -U.m02;
        U.m12 = -U.m12;
        U.m22 = -U.m22;
    }
    // Code to handle degenetrate eigenvalues
    // Handle degeneracies corresponding to equal-length axes in the
    // inertia ellipsoid. If any two axes have similar lengths, then
    // U is redefined solely using the direction of the remaining
    // axis.
    // 
    // In determining whether axes have similar lengths, we use
    // the fact that the moment of inertia of an ellipsoid
    // with unit mass is given by
    // 
    // sig.x = 1/5 (b^2 + c^2)
    // sig.y = 1/5 (a^2 + c^2)
    // sig.z = 1/5 (a^2 + b^2)
    // 
    // along with the fact that if a and b are close together, then
    // 
    // a^2 - b^2 ~= 2 b (a-b)
    // 
    // a^2 + b^2 ~= 2 a^2 ~= 2 b^2
    // 
    // and so
    // 
    // (a - b) ~= (a^2 - b^2) / 2 b
    // ~= (a^2 - b^2) / (sqrt(2) * sqrt(2 b^2))
    // ~= (a^2 - b^2) / (sqrt(2) * sqrt(a^2 + b^2))
    boolean xout = false, yout = false, zout = false;
    sig.scale(5);
    double tol = sig.infinityNorm() * 1e-6;
    if (Math.abs(sig.x - sig.y) / Math.sqrt(2 * sig.z) < tol) {
        // eliminate x and y
        xout = yout = true;
    }
    if (Math.abs(sig.x - sig.z) / Math.sqrt(2 * sig.y) < tol) {
        // eliminate x and z
        xout = zout = true;
    }
    if (Math.abs(sig.z - sig.y) / Math.sqrt(2 * sig.x) < tol) {
        // eliminate z and y
        zout = yout = true;
    }
    if (zout && xout && yout) {
        U.setIdentity();
    } else if (zout || yout || xout) {
        Vector3d zdir = new Vector3d();
        RotationMatrix3d R = new RotationMatrix3d();
        if (xout && yout) {
            U.getColumn(2, zdir);
        } else if (xout && zout) {
            U.getColumn(1, zdir);
        } else if (zout && yout) {
            U.getColumn(0, zdir);
        }
        R.setZDirection(zdir);
        U.set(R);
    }
    myX.R.set(U);
    myX.p.set(cent);
}

Example 4 with Matrix3d

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

the class OBB method set.

public void set(Boundable[] elems, int num, double margin, Method method) {
    Matrix3d C = new Matrix3d();
    Point3d cent = new Point3d();
    Point3d max = new Point3d(-INF, -INF, -INF);
    Point3d min = new Point3d(INF, INF, INF);
    switch(method) {
        case ConvexHull:
            {
                HashedPointSet pointSet = createPointSetForOBB(elems, num);
                quickhull3d.Point3d[] hullPnts = computeConvexHullAndCovariance(C, cent, pointSet.getPointsAsDoubleArray(), pointSet.size());
                setTransform(C, cent);
                computeBoundsFromConvexHullPoints(min, max, hullPnts, hullPnts.length);
                break;
            }
        case Covariance:
            {
                computeCovarianceFromElements(C, cent, elems, num);
                setTransform(C, cent);
                computeBoundsFromElements(min, max, elems, num);
                break;
            }
        case Points:
            {
                HashedPointSet pointSet = createPointSetForOBB(elems, num);
                Point3d[] pnts = pointSet.getPoints();
                computeCovarianceFromPoints(C, cent, pnts);
                setTransform(C, cent);
                computeBoundsFromPoints(min, max, pnts);
                break;
            }
        default:
            throw new InternalErrorException("Unimplemented method " + method);
    }
    setWidthsAndCenter(min, max, margin);
}

Example 5 with Matrix3d

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

the class OBB method computeCovarianceFromElements.

private void computeCovarianceFromElements(Matrix3d C, Point3d cent, Boundable[] elems, int num) {
    Matrix3d Celem = new Matrix3d();
    Point3d centElem = new Point3d();
    double size = 0;
    for (int i = 0; i < num; i++) {
        Boundable elem = elems[i];
        double s = elem.computeCovariance(Celem);
        if (s < 0) {
            throw new IllegalArgumentException("Cannot create OBB: boundable type " + elem.getClass() + " does not implement computeCovariance()");
        }
        elem.computeCentroid(centElem);
        C.add(Celem);
        size += s;
        cent.scaledAdd(s, centElem);
    }
    cent.scale(1.0 / size);
    C.addScaledOuterProduct(-size, cent, cent);
}