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

Example 61 with RotationMatrix3d

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

the class CSG method doObbTest.

private static void doObbTest() {
    OBB obb1 = new OBB();
    RigidTransform3d trans = new RigidTransform3d(new Point3d(0.5, -0.25, -0.25), new RotationMatrix3d(0.81649658092772603, 1.1102230246251565e-016, 0.57735026918962573, 0.40824829046386307, -0.70710678118654757, -0.57735026918962573, 0.40824829046386302, 0.70710678118654746, -0.57735026918962573));
    obb1.setTransform(trans);
    obb1.myHalfWidths.set(0.61237243579579459, 0.70710678128654769, 1.0000004163336343e-010);
    OBB obb2 = new OBB();
    trans = new RigidTransform3d(new Point3d(-0.099999999999999978, -0.25000000000000011, -0.24999999999999994), new RotationMatrix3d(0.81649658092772603, -1.1102230246251565e-016, -0.57735026918962573, -0.40824829046386307, -0.70710678118654757, -0.57735026918962573, -0.40824829046386302, 0.70710678118654746, -0.57735026918962573));
    obb2.setTransform(trans);
    obb2.myHalfWidths.set(0.61237243579579459, 0.70710678128654758, 1.0000005551115123e-010);
    boolean disjoint = BVBoxNodeTester.isDisjoint(obb1, obb2);
    System.out.printf("%s\n", disjoint ? "no" : "yes");
}

Example 62 with RotationMatrix3d

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

the class HeuerOffLinFM 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;
    double absx = Math.abs(Math.toDegrees(sx));
    double absy = Math.abs(Math.toDegrees(sy));
    double absz = Math.abs(Math.toDegrees(sz));
    double fp1 = 0.0312;
    double fp2 = -0.1818;
    double fp3 = 1.1181;
    double fp4 = 0;
    double ep1 = 0.0288;
    double ep2 = 0.0192;
    double ep3 = 1.248;
    double ep4 = 0;
    double lbp1 = 0.0081;
    double lbp2 = 0.1417;
    double lbp3 = 0.4741;
    double lbp4 = 0;
    double arp1 = 0.0291;
    double arp2 = -0.0044;
    double arp3 = 2.5695;
    double arp4 = 0;
    Jq.m00 = myK.x;
    Jq.m11 = myK.y;
    Jq.m22 = myK.z;
    Jq.m33 = (3 * lbp1 * Math.pow(absx, 2) + 2 * lbp2 * Math.pow(absx, 1) + lbp3) * Math.toDegrees(1) * R.m11;
    Jq.m44 = (3 * arp1 * Math.pow(absy, 2) + 2 * arp2 * Math.pow(absy, 1) + arp3) * Math.toDegrees(1) * R.m00;
    if (sz < 0)
        Jq.m55 = (3 * fp1 * Math.pow(absz, 2) + 2 * fp2 * Math.pow(absz, 1) + fp3) * Math.toDegrees(1) * R.m00;
    else
        Jq.m55 = (3 * ep1 * Math.pow(absz, 2) + 2 * ep2 * Math.pow(absz, 1) + ep3) * Math.toDegrees(1) * R.m00;
    if (!symmetric) {
        Jq.m34 = -(3 * lbp1 * Math.pow(absx, 2) + 2 * lbp2 * Math.pow(absx, 1) + lbp3) * Math.toDegrees(1) * R.m01;
        Jq.m43 = -(3 * arp1 * Math.pow(absy, 2) + 2 * arp2 * Math.pow(absy, 1) + arp3) * Math.toDegrees(1) * R.m10;
        if (sz < 0)
            Jq.m53 = -(3 * fp1 * Math.pow(absz, 2) + 2 * fp2 * Math.pow(absz, 1) + fp3) * Math.toDegrees(1) * R.m20;
        else
            Jq.m53 = -(3 * ep1 * Math.pow(absz, 2) + 2 * ep2 * Math.pow(absz, 1) + ep3) * Math.toDegrees(1) * R.m20;
    }
    // added for validaiton, and therefore should be removed
    Jq.scale(1);
}

Example 63 with RotationMatrix3d

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

the class NonlinearlyStiffFrameMaterial method computeDFdq.

@Override
public void computeDFdq(Matrix6d Jq, RigidTransform3d X21, Twist vel21, RigidTransform3d initialX21, boolean symmetric) {
    // TODO Auto-generated method stub
    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;
    double absx = Math.abs(Math.toDegrees(sx));
    double absy = Math.abs(Math.toDegrees(sy));
    double absz = Math.abs(Math.toDegrees(sz));
    // double sx = R.m21/R.m22;
    // sx = Math.atan(sx);
    // double sy = -R.m20;
    // sy = Math.asin(sy);
    // double sz = R.m10/R.m00;
    // sz = Math.atan(sz);
    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;
    Jq.m33 = -(-0.0087 * 3 * Math.pow(absx, 2) - 0.6989 * 2 * Math.pow(absx, 1)) * Math.toDegrees(1) * R.m11;
    Jq.m44 = -(-0.0061 * 4 * Math.pow(absy, 3) - 1.0191 * 2 * Math.pow(absy, 1)) * Math.toDegrees(1) * R.m00;
    Jq.m55 = -(-0.002 * 4 * Math.pow(absz, 3) + 0.0141 * 3 * Math.pow(absz, 2) - 0.4726 * 2 * Math.pow(absz, 1)) * Math.toDegrees(1) * R.m00;
    if (!symmetric) {
        // Jq.m34 = -myRotK.x * R.m01;
        // Jq.m43 = -myRotK.y * R.m10;
        // Jq.m53 = -myRotK.z* R.m20;
        Jq.m34 = (-0.0087 * 3 * Math.pow(absx, 2) - 0.6989 * 2 * Math.pow(absx, 1)) * Math.toDegrees(1) * R.m01;
        Jq.m43 = (-0.0061 * 4 * Math.pow(absy, 3) - 1.0191 * 2 * Math.pow(absy, 1)) * Math.toDegrees(1) * R.m10;
        Jq.m53 = ((-0.002 * 4 * Math.pow(absz, 3) + 0.0141 * 3 * Math.pow(absz, 2) - 0.4726 * 2 * Math.pow(absz, 1)) * Math.toDegrees(1)) * R.m20;
    }
}

Example 64 with RotationMatrix3d

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

the class OffsetLinearFrameMaterial 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;
    }
}

Example 65 with RotationMatrix3d

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

the class LinearMaterialBase method computeTangent.

@Override
public void computeTangent(Matrix6d D, SymmetricMatrix3d stress, SolidDeformation def, Matrix3d Q, FemMaterial baseMat) {
    // get spatial stiffness tensor
    getC(D);
    // rotate if corotated
    if (isCorotated()) {
        // need to rotate this tensor from linear frame into material one
        RotationMatrix3d R = def.getR();
        if (R == null) {
            Matrix3d F = def.getF();
            R = computeRotation(F, null);
        }
        // R rotates from material frame to the spatial one. Transpose
        // of R rotates from spatial frame to material one.
        TensorUtils.unrotateTangent(D, D, R);
    }
}