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

Example 11 with Twist

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

the class RigidBody method getEffectiveMassForces.

public static int getEffectiveMassForces(VectorNd f, double t, FrameState state, SpatialInertia effectiveInertia, int idx) {
    Wrench coriolisForce = new Wrench();
    Twist bodyVel = new Twist();
    bodyVel.inverseTransform(state.XFrameToWorld.R, state.vel);
    SpatialInertia S = effectiveInertia;
    S.coriolisForce(coriolisForce, bodyVel);
    if (dynamicVelInWorldCoords) {
        coriolisForce.transform(state.XFrameToWorld.R);
    }
    double[] buf = f.getBuffer();
    buf[idx++] = -coriolisForce.f.x;
    buf[idx++] = -coriolisForce.f.y;
    buf[idx++] = -coriolisForce.f.z;
    buf[idx++] = -coriolisForce.m.x;
    buf[idx++] = -coriolisForce.m.y;
    buf[idx++] = -coriolisForce.m.z;
    return idx;
}

Example 12 with Twist

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

the class RigidBody method copy.

@Override
public RigidBody copy(int flags, Map<ModelComponent, ModelComponent> copyMap) {
    RigidBody comp = (RigidBody) super.copy(flags, copyMap);
    // comp.myAttachments = new ArrayList<PointFrameAttachment>();
    // comp.myComponents = new ArrayList<ModelComponent>();
    // comp.indicesValidP = false;
    comp.setDynamic(true);
    comp.mySpatialInertia = new SpatialInertia(mySpatialInertia);
    // comp.myEffectiveInertia = new SpatialInertia (mySpatialInertia);
    PolygonalMesh mesh = getMesh();
    comp.myMeshInfo = new MeshInfo();
    if (mesh != null) {
        PolygonalMesh meshCopy = mesh.copy();
        comp.setMesh(meshCopy, getMeshFileName(), getMeshFileTransform());
        comp.myMeshInfo.myFlippedP = myMeshInfo.myFlippedP;
    } else {
        comp.setMesh(null, null, null);
    }
    // comp.myMarkers =
    // new PointList<FrameMarker> (
    // FrameMarker.class, "markers", "k");
    // comp.add (comp.myMarkers);
    comp.myBodyForce = new Wrench();
    comp.myCoriolisForce = new Wrench();
    comp.myBodyVel = new Twist();
    comp.myBodyAcc = new Twist();
    comp.myQvel = new Quaternion();
    comp.myTmpPos = new Point3d();
    comp.myConnectors = null;
    return comp;
}

Example 13 with Twist

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

the class MotionTargetTerm method updateTargetVelocityVec.

private void updateTargetVelocityVec() {
    if (myTargetVel == null || myTargetVel.size() != myTargetVelSize)
        myTargetVel = new VectorNd(myTargetVelSize);
    double[] buf = myTargetVel.getBuffer();
    int idx = 0;
    for (int i = 0; i < myTargets.size(); i++) {
        MotionTargetComponent target = myTargets.get(i);
        if (target instanceof Point) {
            Vector3d vel = ((Point) target).getVelocity();
            buf[idx++] = vel.x;
            buf[idx++] = vel.y;
            buf[idx++] = vel.z;
        } else if (target instanceof Frame) {
            Twist vel = ((Frame) target).getVelocity();
            buf[idx++] = vel.v.x;
            buf[idx++] = vel.v.y;
            buf[idx++] = vel.v.z;
            buf[idx++] = vel.w.x;
            buf[idx++] = vel.w.y;
            buf[idx++] = vel.w.z;
        }
    }
}