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

Example 21 with RotationMatrix3d

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

the class PointFem3dAttachment method computeVelDerivative.

private boolean computeVelDerivative(Vector3d dvel) {
    Frame pframe = myPoint.getPointFrame();
    boolean isNonZero = false;
    // can we optimize this? If the attachment has been enforced, then can we
    // compute lp2 and lv2 directly from the point itself?
    Vector3d lv2 = new Vector3d();
    Vector3d lp2 = new Vector3d();
    double[] coords = myCoords.getBuffer();
    for (int i = 0; i < myNodes.length; i++) {
        FemNode node = myNodes[i];
        double w = coords[i];
        lv2.scaledAdd(w, node.getLocalVelocity());
        lp2.scaledAdd(w, node.getLocalPosition());
    }
    if (myFemFrame != null) {
        RotationMatrix3d R2 = myFemFrame.getPose().R;
        Twist vel2 = myFemFrame.getVelocity();
        Vector3d tmp1 = new Vector3d();
        Vector3d tmp2 = new Vector3d();
        // R2*lp2
        tmp1.transform(R2, lp2);
        // R2*lv2
        tmp2.transform(R2, lv2);
        // tmp1 = w2 X R2*lp2 + R2*lv2
        tmp1.crossAdd(vel2.w, tmp1, tmp2);
        // dvel = w2 X R2*lv2 + w2 X tmp1
        dvel.cross(vel2.w, tmp2);
        dvel.crossAdd(vel2.w, tmp1, dvel);
        if (pframe != null) {
            RotationMatrix3d R1 = pframe.getPose().R;
            Twist vel1 = pframe.getVelocity();
            // R1*lv1
            tmp2.transform(R1, myPoint.getLocalVelocity());
            tmp2.negate();
            // tmp2 = -R1*lv1 - u2 + u1 - tmp1
            tmp2.sub(vel2.v);
            tmp2.add(vel1.v);
            tmp2.sub(tmp1);
            // dvel = R1^T (w1 X tmp2 + dvel)
            dvel.crossAdd(vel1.w, tmp2, dvel);
            dvel.inverseTransform(R1);
        }
        isNonZero = true;
    } else if (pframe != null) {
        RotationMatrix3d R1 = pframe.getPose().R;
        Twist vel1 = pframe.getVelocity();
        // dvel = R1^T (w1 X (u1 - R1*lv1 - lv2))
        // R1*lv1
        dvel.transform(R1, myPoint.getLocalVelocity());
        dvel.negate();
        // since Fem has no frame, lv2 and world velocity are the same
        dvel.sub(lv2);
        dvel.add(vel1.v);
        dvel.cross(vel1.w, dvel);
        dvel.inverseTransform(R1);
        isNonZero = true;
    }
    return isNonZero;
}

Example 22 with RotationMatrix3d

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

the class StiffnessWarper3d method computeRotation.

/**
 * Computes a corotated rotation based on the deformation gradient
 * @param F deformation gradient
 * @param P symmetric part of gradient after rotation
 */
protected void computeRotation(Matrix3d F, SymmetricMatrix3d P) {
    if (R == null) {
        R = new RotationMatrix3d();
    }
    SVDecomposition3d SVD = new SVDecomposition3d();
    SVD.polarDecomposition(R, P, F);
}

Example 23 with RotationMatrix3d

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

the class EditingAgent method intersectViewPlane.

/**
 * Intersect a ray with a view plane defined by the current eye direction and
 * a reference point.
 */
public Point3d intersectViewPlane(Line ray, Point3d ref, GLViewer viewer) {
    Point3d res = new Point3d();
    RotationMatrix3d R = viewer.getCenterToWorld().R;
    Plane plane = new Plane(new Vector3d(R.m02, R.m12, R.m22), ref);
    plane.intersectRay(res, ray.getDirection(), ray.getOrigin());
    return res;
}

Example 24 with RotationMatrix3d

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

the class FungMaterial method main.

public static void main(String[] args) {
    FungMaterial mat = new FungMaterial();
    RotationMatrix3d R = new RotationMatrix3d();
    R.setRpy(1, 2, 3);
    SolidDeformation def = new SolidDeformation();
    Matrix3d Q = new Matrix3d();
    def.setF(new Matrix3d(1, 3, 5, 2, 1, 4, 6, 1, 2));
    Q.set(R);
    Matrix6d D = new Matrix6d();
    SymmetricMatrix3d sig = new SymmetricMatrix3d();
    // mat.computeStress (sig, pt, dt, null);
    // System.out.println ("sig=\n" + sig.toString ("%12.6f"));
    // mat.computeTangent (D, sig, pt, dt, null);
    // System.out.println ("D=\n" + D.toString ("%12.6f"));
    mat.computeStress(sig, def, Q, null);
    System.out.println("sig=\n" + sig.toString("%12.6f"));
    mat.computeTangent(D, sig, def, Q, null);
    System.out.println("D=\n" + D.toString("%12.6f"));
}

Example 25 with RotationMatrix3d

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

the class LinearFrameMaterial method computeDFdq.

public void computeDFdq(Matrix6d Jq, RigidTransform3d X21, Twist vel21, RigidTransform3d initialX21, boolean symmetric) {
    Jq.setZero();
    RotationMatrix3d R = X21.R;
    // use these matrix entries as small angle approximations to
    // the rotations about x, y, and z
    // double sx =  R.m21;
    // double sy = -R.m20;
    // double sz =  R.m10;
    Jq.m00 = myK.x;
    Jq.m11 = myK.y;
    Jq.m22 = myK.z;
    Jq.m33 = myRotK.x * R.m11;
    Jq.m44 = myRotK.y * R.m00;
    Jq.m55 = myRotK.z * R.m00;
    if (!symmetric) {
        Jq.m34 = -myRotK.x * R.m01;
        Jq.m43 = -myRotK.y * R.m10;
        Jq.m53 = -myRotK.z * R.m20;
    }
}