Examples with Twist maspack.spatialmotion.Twist used on opensource projects

Example 6 with Twist

use of maspack.spatialmotion.Twist in project artisynth_core by artisynth.

the class Frame method addPosImpulse.

public void addPosImpulse(double[] xbuf, int xidx, double h, double[] vbuf, int vidx) {
    Twist vel = new Twist();
    vel.v.x = vbuf[vidx++];
    vel.v.y = vbuf[vidx++];
    vel.v.z = vbuf[vidx++];
    vel.w.x = vbuf[vidx++];
    vel.w.y = vbuf[vidx++];
    vel.w.z = vbuf[vidx++];
    // XXX streamline this. This is only in the form it is to preserve exact
    // numeric compatibility with older code.
    RigidTransform3d X = new RigidTransform3d();
    X.p.set(xbuf[xidx], xbuf[xidx + 1], xbuf[xidx + 2]);
    Quaternion rot = new Quaternion(xbuf[xidx + 3], xbuf[xidx + 4], xbuf[xidx + 5], xbuf[xidx + 6]);
    rot.normalize();
    X.R.set(rot);
    if (dynamicVelInWorldCoords) {
        vel.extrapolateTransformWorld(X, h);
    } else {
        vel.extrapolateTransform(X, h);
    }
    rot.set(X.R);
    xbuf[xidx++] = X.p.x;
    xbuf[xidx++] = X.p.y;
    xbuf[xidx++] = X.p.z;
    xbuf[xidx++] = rot.s;
    xbuf[xidx++] = rot.u.x;
    xbuf[xidx++] = rot.u.y;
    xbuf[xidx++] = rot.u.z;
}

Example 7 with Twist

use of maspack.spatialmotion.Twist in project artisynth_core by artisynth.

the class RigidBody method mulInverseEffectiveMass.

public static int mulInverseEffectiveMass(SpatialInertia S, double[] a, double[] f, int idx) {
    Twist tw = new Twist();
    Wrench wr = new Wrench();
    wr.f.x = f[idx];
    wr.f.y = f[idx + 1];
    wr.f.z = f[idx + 2];
    wr.m.x = f[idx + 3];
    wr.m.y = f[idx + 4];
    wr.m.z = f[idx + 5];
    S.mulInverse(tw, wr);
    a[idx++] = tw.v.x;
    a[idx++] = tw.v.y;
    a[idx++] = tw.v.z;
    a[idx++] = tw.w.x;
    a[idx++] = tw.w.y;
    a[idx++] = tw.w.z;
    return idx;
}

Example 8 with Twist

use of maspack.spatialmotion.Twist in project artisynth_core by artisynth.

the class PointFrameAttachment method computeVelDerivative.

private void computeVelDerivative(Vector3d dvel) {
    Frame pframe = myPoint.getPointFrame();
    RotationMatrix3d R2 = myFrame.getPose().R;
    Twist vel2 = myFrame.getVelocity();
    Vector3d tmp1 = new Vector3d();
    Vector3d tmp2 = new Vector3d();
    if (myFrame instanceof DeformableBody) {
        // XXX shouldn't have to check explicitly for deformable body;
        // Frame should have the necessary methods
        DeformableBody defb = (DeformableBody) myFrame;
        defb.computeDeformedPos(tmp1, myLoc);
        // R2*lp2
        tmp1.transform(R2);
        defb.computeDeformedVel(tmp2, myLoc);
        // R2*lv2
        tmp2.transform(R2);
        // 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);
    } else {
        // tmp1 = w2 X R2*lp2
        // R2*lp2
        tmp1.transform(R2, myLoc);
        tmp1.cross(vel2.w, tmp1);
        // dvel = w2 X tmp1
        dvel.cross(vel2.w, tmp1);
    }
    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);
    }
}

Example 9 with Twist

use of maspack.spatialmotion.Twist in project artisynth_core by artisynth.

the class Frame method computePointVelocity.

/**
 * Computes the velocity, in world coordinates, of a point attached to this
 * frame.
 *
 * @param vel
 * returns the point velocity
 * @param loc
 * position of the point, in body coordinates
 * @param pvel
 * independent velocity for the point, in body coordinates
 */
public void computePointVelocity(Vector3d vel, Point3d loc, Vector3d pvel) {
    // use vel to store loc transformed into world coords
    Twist frameVel = myState.getVelocity();
    vel.transform(myState.XFrameToWorld.R, loc);
    vel.crossAdd(frameVel.w, vel, frameVel.v);
    Vector3d tmp = new Vector3d();
    tmp.transform(myState.XFrameToWorld.R, pvel);
    vel.add(tmp);
}

Example 10 with Twist

use of maspack.spatialmotion.Twist in project artisynth_core by artisynth.

the class Frame method computePointCoriolis.

/**
 * Computes the velocity derivative of a point attached to this frame
 * that is due to the current velocity of the frame.
 *
 * @param cor
 * returns the point Coriolis term (in world coordinates)
 * @param loc
 * position of the point, in body coordinates
 */
public void computePointCoriolis(Vector3d cor, Vector3d loc) {
    Twist tw = getVelocity();
    cor.transform(getPose().R, loc);
    cor.cross(tw.w, cor);
    cor.cross(tw.w, cor);
}