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

Example 1 with Quaternion

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

the class FixedMeshBody method scanItem.

protected boolean scanItem(ReaderTokenizer rtok, Deque<ScanToken> tokens) throws IOException {
    rtok.nextToken();
    if (scanAttributeName(rtok, "mesh")) {
        myMeshInfo.scan(rtok);
        return true;
    } else if (scanAttributeName(rtok, "position")) {
        Point3d pos = new Point3d();
        pos.scan(rtok);
        setPosition(pos);
        return true;
    } else if (scanAttributeName(rtok, "rotation")) {
        Quaternion q = new Quaternion();
        q.scan(rtok);
        setRotation(q);
        return true;
    }
    rtok.pushBack();
    return super.scanItem(rtok, tokens);
}

Example 2 with Quaternion

use of maspack.matrix.Quaternion 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 3 with Quaternion

use of maspack.matrix.Quaternion 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 4 with Quaternion

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

the class RigidTransformInputProbe method apply.

/**
 * Apply this probe at time t to model myModel.
 *
 * @param t time to interpolate vertex positions to.
 */
public void apply(double t) {
    if (myRigid == null) {
        return;
    }
    double tloc = (t - getStartTime()) / myScale;
    myTransAndQuaternParams.interpolate(myTmpVec, tloc);
    RigidTransform3d tx = new RigidTransform3d();
    Vector3d offset = new Vector3d();
    offset.x = myTmpVec.get(0);
    offset.y = myTmpVec.get(1);
    offset.z = myTmpVec.get(2);
    tx.setTranslation(offset);
    Quaternion q = new Quaternion();
    q.s = myTmpVec.get(3);
    q.u.x = myTmpVec.get(4);
    q.u.y = myTmpVec.get(5);
    q.u.z = myTmpVec.get(6);
    RotationMatrix3d rot = new RotationMatrix3d(q);
    RigidTransform3d rigidTx = new RigidTransform3d();
    rigidTx.setTranslation(offset);
    rigidTx.setRotation(rot);
    myRigid.setPose(rigidTx);
}

Example 5 with Quaternion

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

the class RigidBody method scanItem.

protected boolean scanItem(ReaderTokenizer rtok, Deque<ScanToken> tokens) throws IOException {
    rtok.nextToken();
    if (scanAttributeName(rtok, "mesh")) {
        myMeshInfo.scan(rtok);
        setMeshFromInfo();
        return true;
    } else if (scanAttributeName(rtok, "pose")) {
        RigidTransform3d X = new RigidTransform3d();
        X.scan(rtok);
        myState.setPose(X);
        return true;
    } else if (scanAttributeName(rtok, "position")) {
        Point3d pos = new Point3d();
        pos.scan(rtok);
        setPosition(pos);
        return true;
    } else if (scanAttributeName(rtok, "rotation")) {
        Quaternion q = new Quaternion();
        q.scan(rtok);
        setRotation(q);
        return true;
    } else if (scanAttributeName(rtok, "vel")) {
        rtok.scanToken('[');
        myState.vel.scan(rtok);
        rtok.scanToken(']');
        return true;
    } else if (scanAttributeName(rtok, "inertia")) {
        SpatialInertia M = new SpatialInertia();
        M.scan(rtok);
        setInertia(M);
        return true;
    } else if (scanAttributeName(rtok, "mass")) {
        double mass = rtok.scanNumber();
        setMass(mass);
        return true;
    } else if (scanAttributeName(rtok, "density")) {
        double density = rtok.scanNumber();
        setDensity(density);
        return true;
    }
    rtok.pushBack();
    return super.scanItem(rtok, tokens);
}