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

Example 31 with RotationMatrix3d

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

the class PlanarConnector method computeRenderVtxs.

protected void computeRenderVtxs(RigidTransform3d TDW) {
    RotationMatrix3d RPD = new RotationMatrix3d();
    RPD.setZDirection(((PlanarCoupling) myCoupling).getNormal());
    myRenderVtxs[0].set(myPlaneSize / 2, myPlaneSize / 2, 0);
    myRenderVtxs[1].set(-myPlaneSize / 2, myPlaneSize / 2, 0);
    myRenderVtxs[2].set(-myPlaneSize / 2, -myPlaneSize / 2, 0);
    myRenderVtxs[3].set(myPlaneSize / 2, -myPlaneSize / 2, 0);
    for (int i = 0; i < myRenderVtxs.length; i++) {
        myRenderVtxs[i].transform(RPD);
        myRenderVtxs[i].transform(TDW);
    }
}

Example 32 with RotationMatrix3d

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

the class RigidTransformInputProbe method addTransform.

public void addTransform(double t, RigidTransform3d tx) {
    VectorNd posVector = new VectorNd();
    Vector3d offset = tx.getOffset();
    posVector.append(offset.x);
    posVector.append(offset.y);
    posVector.append(offset.z);
    RotationMatrix3d rot = new RotationMatrix3d();
    Vector3d scale = new Vector3d();
    Matrix3d shear = new Matrix3d();
    tx.getMatrixComponents(rot, scale, shear);
    Quaternion q = new Quaternion(rot.getAxisAngle());
    posVector.append(q.s);
    posVector.append(q.u.x);
    posVector.append(q.u.y);
    posVector.append(q.u.z);
    NumericListKnot knot = new NumericListKnot(myVectorSize);
    knot.t = t;
    knot.v.set(posVector);
    myTransAndQuaternParams.add(knot);
    if (t < getStartTime()) {
        setStartTime(t);
    }
    if (t > getStopTime()) {
        setStopTime(t);
    }
}

Example 33 with RotationMatrix3d

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

the class PointDistributor method getSphericalMidpointCubature.

public static CubaturePoint3d[] getSphericalMidpointCubature(Point3d center, double radius, int nR, int nPhi, int nTheta, Vector3d axis) {
    CubaturePoint3d[] pnts = new CubaturePoint3d[nR * nPhi * nTheta];
    double dr = radius / nR;
    double dphi = 2 * Math.PI / nPhi;
    double dtheta = Math.PI / nTheta;
    RigidTransform3d trans = new RigidTransform3d();
    if (axis != null) {
        RotationMatrix3d R = new RotationMatrix3d();
        R.rotateZDirection(axis);
        trans.setRotation(R);
    }
    trans.setTranslation(center);
    double r, phi, theta;
    int idx = 0;
    for (int i = 0; i < nR; i++) {
        for (int j = 0; j < nPhi; j++) {
            for (int k = 0; k < nTheta; k++) {
                r = i * dr + dr / 2;
                phi = j * dphi + (i + k) * dphi / 2;
                theta = k * dtheta + dtheta / 2;
                pnts[idx] = new CubaturePoint3d();
                pnts[idx].x = r * Math.cos(phi) * Math.sin(theta);
                pnts[idx].y = r * Math.sin(phi) * Math.sin(theta);
                pnts[idx].z = r * Math.cos(theta);
                pnts[idx].w = spherePartialVolume(radius, r, dr, phi, dphi, theta, dtheta);
                // transform
                pnts[idx].transform(trans);
                idx++;
            }
        }
    }
    return pnts;
}

Example 34 with RotationMatrix3d

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

the class PointDistributor method getSphericalCapMidpointCubature.

public static CubaturePoint3d[] getSphericalCapMidpointCubature(Point3d center, double radius, double height, int nR, int nTheta, int nH, Vector3d axis) {
    CubaturePoint3d[] pnts = new CubaturePoint3d[nR * nTheta * nH];
    double dr = radius / nR;
    double dtheta = 2 * Math.PI / nTheta;
    double dh = height / nH;
    RigidTransform3d trans = new RigidTransform3d();
    if (axis != null) {
        RotationMatrix3d R = new RotationMatrix3d();
        R.rotateZDirection(axis);
        trans.setRotation(R);
    }
    Vector3d t = new Vector3d(axis);
    t.normalize();
    t.scale(radius - height);
    t.add(center);
    trans.setTranslation(t);
    double r, theta, h, s;
    int idx = 0;
    for (int i = 0; i < nR; i++) {
        for (int j = 0; j < nTheta; j++) {
            for (int k = 0; k < nH; k++) {
                h = k * dh + dh / 2;
                theta = j * dtheta + (i + k) * dtheta / 2;
                r = i * dr + dr / 2;
                s = Math.sqrt(-h * h + 2 * h * height - 2 * h * radius - height * height + 2 * height * radius) / radius;
                pnts[idx] = new CubaturePoint3d();
                pnts[idx].x = r * s * Math.cos(theta);
                pnts[idx].y = r * s * Math.sin(theta);
                pnts[idx].z = h;
                pnts[idx].w = sphereCapPartialVolume(height, radius, r, dr, h, dh, theta, dtheta);
                pnts[idx].transform(trans);
                idx++;
            }
        }
    }
    return pnts;
}

Example 35 with RotationMatrix3d

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

the class HydrostatDemo method addContactBlock.

public void addContactBlock(MechModel mech) {
    RigidBody block = new RigidBody();
    block.setName("block");
    block.setMesh(MeshFactory.createQuadBox(100, 100, 20), null);
    block.setInertia(SpatialInertia.createBoxInertia(1, 100, 100, 20));
    block.setPose(new RigidTransform3d(new Vector3d(-50, 0, 40), new RotationMatrix3d()));
    mech.addRigidBody(block);
}