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

Example 1 with RotationMatrix3d

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

the class CPDTest method main.

public static void main(String[] args) {
    AffineTransform3d trans = new AffineTransform3d();
    RotationMatrix3d R = new RotationMatrix3d(0.7605, -0.6307, 0.1541, 0.6485, 0.7263, -0.2279, 0.0318, 0.2733, 0.9614);
    double s = 2.7;
    trans.setA(R, new Vector3d(s, s, s), new Vector3d(0, 0, 0));
    trans.setTranslation(new Vector3d(1, 2, 3));
    Point3d[] X = get3DFish();
    int N = X.length;
    int M = N - 20;
    Point3d[] Y = new Point3d[M];
    Point3d[] out = new Point3d[M];
    for (int i = 0; i < N; i++) {
        if (i < M) {
            Y[i] = new Point3d(X[i]);
            out[i] = new Point3d();
        }
        X[i].transform(trans);
    }
    double w = 0.01;
    double lambda = 0.1;
    double beta2 = 3.5;
    CPD.verbose = true;
    ScaledRigidTransform3d rigidT = CPD.rigid(X, Y, w, 1e-10, 100, true, out);
    AffineTransform3d affT = CPD.affine(X, Y, w, 1e-10, 100, out);
    CPD.coherent(X, Y, lambda, beta2, w, 1e-10, 100, out);
    System.out.println("Original: \n" + trans.toString());
    System.out.println("Rigid Computed: \n" + rigidT.toString());
    System.out.println("Affine Computed: \n" + affT.toString());
    for (int i = 0; i < M; i++) {
        System.out.println(X[i].toString() + "\n" + out[i].toString() + "\n");
    }
}

Example 2 with RotationMatrix3d

use of maspack.matrix.RotationMatrix3d 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 RotationMatrix3d

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

the class OBB method updateForPoint.

// public void update () {
// update (defaultTolerance);
// }
/**
 * Update a set of half-widths to accommodate a single point <code>p</code>
 * within a prescribed margin for coordinate frame of this box.
 *
 * @param hw half-widths to update
 * @param p point to accommodate
 * @param margin extra space margin
 */
private void updateForPoint(Vector3d hw, Point3d p, double margin) {
    // transform x, y, z of p into coordinates bx, by, bz of the coordinate
    // frame of this box.
    RotationMatrix3d R = myX.R;
    double x = p.x - myX.p.x;
    double y = p.y - myX.p.y;
    double z = p.z - myX.p.z;
    double bx = R.m00 * x + R.m10 * y + R.m20 * z;
    double by = R.m01 * x + R.m11 * y + R.m21 * z;
    double bz = R.m02 * x + R.m12 * y + R.m22 * z;
    double del;
    // now use bx, by, bz to update the bounds
    if ((del = bx + margin - hw.x) > 0) {
        hw.x += del;
    } else if ((del = -bx + margin - hw.x) > 0) {
        hw.x += del;
    }
    if ((del = by + margin - hw.y) > 0) {
        hw.y += del;
    } else if ((del = -by + margin - hw.y) > 0) {
        hw.y += del;
    }
    if ((del = bz + margin - hw.z) > 0) {
        hw.z += del;
    } else if ((del = -bz + margin - hw.z) > 0) {
        hw.z += del;
    }
}

Example 4 with RotationMatrix3d

use of maspack.matrix.RotationMatrix3d 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 5 with RotationMatrix3d

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

the class GLViewer method setEyeToWorld.

/**
 * Sets the eyeToWorld transform for this viewer, using the canonical
 * parameters used by the GL <code>lookat</code> method.
 *
 * @param eye
 * position of the eye, in world coordinates
 * @param center
 * point that the eye is looking at, in world coordinates
 * @param up
 * up direction, in world coordinates
 */
public void setEyeToWorld(Point3d eye, Point3d center, Vector3d up) {
    Vector3d zaxis = new Vector3d();
    Vector3d yaxis = new Vector3d();
    Vector3d xaxis = new Vector3d();
    zaxis.sub(eye, center);
    double n = zaxis.norm();
    if (n > 1e-12) {
        zaxis.scale(1.0 / n);
    } else {
        RotationMatrix3d R = new RotationMatrix3d();
        R.rotateZDirection(up);
        R.getColumn(0, zaxis);
        R.getColumn(1, xaxis);
        R.getColumn(2, yaxis);
    }
    xaxis.cross(up, zaxis);
    n = xaxis.norm();
    if (n > 1e-6) {
        xaxis.scale(1.0 / n);
        yaxis.cross(zaxis, xaxis);
        yaxis.normalize();
    } else {
        RotationMatrix3d R = new RotationMatrix3d();
        R.rotateZDirection(zaxis);
        R.getColumn(1, yaxis);
        R.getColumn(0, xaxis);
    }
    synchronized (viewMatrix) {
        viewMatrix.set(new double[] { xaxis.x, xaxis.y, xaxis.z, -xaxis.dot(eye), yaxis.x, yaxis.y, yaxis.z, -yaxis.dot(eye), zaxis.x, zaxis.y, zaxis.z, -zaxis.dot(eye) });
    }
    // XXX
    if (Math.abs(viewMatrix.getOffset().norm()) < 1e-5) {
    // System.err.println ("bang"); Thread.dumpStack();
    }
    invalidateViewMatrix();
}