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

Example 1 with Twist

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

the class LemkeContactSolver method addRestitution.

private void addRestitution(Twist vel, double restitution) {
    // compute the velocity impulse that was applied along all the
    // normals
    Wrench fnorm = new Wrench();
    Twist vnorm = new Twist();
    int kz = 0;
    for (ContactVariable the = thetaAlpha.head; the != null; the = the.next) {
        double value = zk.get(kz++);
        fnorm.scaledAdd(value, the.nd, fnorm);
    }
    vnorm.set(fnorm);
    if (myInertia != null) {
        myInertia.mulRightFactorInverse(vnorm, vnorm);
    }
    vel.scaledAdd(restitution, vnorm, vel);
}

Example 2 with Twist

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

the class DantzigLCPSolverTest method testMultiPointContact.

/**
 * Create a test case involving multi-point contact of a box on a plane. The
 * angle of the plane normal relative to the horizontal is ang, and the
 * friction coefficient is mu.
 */
public void testMultiPointContact(double ang, double mu) {
    double mass = 4.0;
    SpatialInertia Inertia = SpatialInertia.createBoxInertia(mass, 2.0, 1.0, 2.0);
    Point3d[] pnts = new Point3d[] { new Point3d(1.0, -0.5, 1.0), new Point3d(1.0, -0.5, -1.0), new Point3d(-1.0, -0.5, -1.0) // new Point3d (-1.0, -0.5, 1.0),
    };
    int nump = pnts.length;
    int numc = 3 * pnts.length;
    Wrench[] constraints = new Wrench[numc];
    for (int i = 0; i < pnts.length; i++) {
        Wrench NT = new Wrench();
        NT.f.set(Vector3d.Y_UNIT);
        NT.m.cross(pnts[i], NT.f);
        constraints[i] = NT;
        Wrench DT = new Wrench();
        DT.f.set(Vector3d.X_UNIT);
        DT.m.cross(pnts[i], DT.f);
        constraints[pnts.length + 2 * i] = DT;
        DT = new Wrench();
        DT.f.set(Vector3d.Z_UNIT);
        DT.m.cross(pnts[i], DT.f);
        constraints[pnts.length + 2 * i + 1] = DT;
    }
    Twist vel0 = new Twist(Math.sin(ang), -Math.cos(ang), 0, 0, 0, 0);
    vel0.scale(9.8);
    MatrixNd A = new MatrixNd(nump, nump);
    VectorNd b = new VectorNd(nump);
    VectorNd theEst = new VectorNd(nump);
    Twist tw = new Twist();
    Wrench wr = new Wrench();
    for (int i = 0; i < nump; i++) {
        for (int j = 0; j < nump; j++) {
            Inertia.mulInverse(tw, constraints[j]);
            A.set(i, j, constraints[i].dot(tw));
        }
        b.set(i, -constraints[i].dot(vel0));
    // System.out.println ("D=" + constraints[i].toString ("%8.3f"));
    }
    CholeskyDecomposition Chol = new CholeskyDecomposition();
    Chol.factor(A);
    Chol.solve(theEst, b);
    // System.out.println ("vel0=" + vel0.toString ("%8.3f"));
    // System.out.println ("theEst=" + theEst.toString ("%8.3f"));
    // for (int i=0; i<nump; i++)
    // { wr.scale (theEst.get(i), constraints[i]);
    // Inertia.mulInverse (tw, wr);
    // vel0.add (tw);
    // }
    // System.out.println ("vel0=" + vel0.toString("%8.3f"));
    MatrixNd M = new MatrixNd(numc, numc);
    VectorNd q = new VectorNd(numc);
    VectorNd z = new VectorNd(numc);
    VectorNd w = new VectorNd(numc);
    VectorNd lo = new VectorNd(numc);
    VectorNd hi = new VectorNd(numc);
    for (int i = 0; i < numc; i++) {
        for (int j = 0; j < numc; j++) {
            Inertia.mulInverse(tw, constraints[j]);
            M.set(i, j, constraints[i].dot(tw));
        }
        q.set(i, constraints[i].dot(vel0));
    }
    for (int i = 0; i < pnts.length; i++) {
        lo.set(i, 0);
        hi.set(i, inf);
        lo.set(pnts.length + 2 * i, -theEst.get(i) * mu);
        hi.set(pnts.length + 2 * i, theEst.get(i) * mu);
        lo.set(pnts.length + 2 * i + 1, -theEst.get(i) * mu);
        hi.set(pnts.length + 2 * i + 1, theEst.get(i) * mu);
    }
    // System.out.println ("ang=" + Math.toDegrees (ang));
    testSolver(z, w, M, q, lo, hi, 0, numc, DantzigLCPSolver.Status.SOLVED);
    Twist vel = new Twist(vel0);
    for (int i = 0; i < numc; i++) {
        wr.scale(z.get(i), constraints[i]);
        Inertia.mulInverse(tw, wr);
        vel.add(tw);
    }
    // }
    if (ang <= Math.atan(mu)) {
        if (!vel.epsilonEquals(Twist.ZERO, 1e-8)) {
            throw new TestException("velocity should be 0 with ang=" + Math.toDegrees(ang));
        }
    } else {
        if (vel.epsilonEquals(Twist.ZERO, 1e-8)) {
            throw new TestException("velocity should be non-zero with ang=" + Math.toDegrees(ang));
        }
    // if (Math.abs(Math.abs(z.get(1)) - theEst*mu) > 1e-8)
    // { throw new TestException (
    // "friction force not on the cone");
    // }
    }
}

Example 3 with Twist

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

the class FemModel3d method mulInverseMass.

@Override
public void mulInverseMass(SparseBlockMatrix M, VectorNd a, VectorNd f) {
    double[] abuf = a.getBuffer();
    double[] fbuf = f.getBuffer();
    int asize = a.size();
    if (M.getAlignedBlockRow(asize) == -1) {
        throw new IllegalArgumentException("size of 'a' not block aligned with 'M'");
    }
    if (f.size() < asize) {
        throw new IllegalArgumentException("size of 'f' is less than the size of 'a'");
    }
    int bf = myFrame.getSolveIndex();
    int bk;
    if (myFrameRelativeP && bf < asize) {
        int fidx = M.getBlockRowOffset(bf);
        Wrench wtmp = new Wrench();
        double[] tmp6 = new double[6];
        for (int i = 0; i < myNodes.size(); i++) {
            FemNode3d n = myNodes.get(i);
            if ((bk = n.getSolveIndex()) != -1) {
                int idx = M.getBlockRowOffset(bk);
                if (idx < asize) {
                    MatrixBlock Mfk = M.getBlock(bf, bk);
                    zero6Vector(tmp6);
                    Mfk.mulAdd(tmp6, 0, fbuf, idx);
                    Matrix3x3Block Mkk = (Matrix3x3Block) M.getBlock(bk, bk);
                    // XXX do we want to use n.mulInverseEffectiveMass?
                    scaledAdd(wtmp, -1 / Mkk.m00, tmp6, 0);
                }
            }
        }
        scaledAdd(wtmp, 1, fbuf, fidx);
        // XXX do we need to use getBlock (bf, bf)???
        Matrix6dBlock Mff = (Matrix6dBlock) M.getBlock(bf, bf);
        SpatialInertia S = new SpatialInertia();
        S.set(Mff);
        Twist ttmp = new Twist();
        S.mulInverse(ttmp, wtmp);
        setFromTwist(abuf, fidx, ttmp);
        double[] tmp3 = new double[3];
        for (int i = 0; i < myNodes.size(); i++) {
            FemNode3d n = myNodes.get(i);
            if ((bk = n.getSolveIndex()) != -1) {
                int idx = M.getBlockRowOffset(bk);
                if (idx < asize) {
                    MatrixBlock Mkf = M.getBlock(bk, bf);
                    Matrix3x3Block Mkk = (Matrix3x3Block) M.getBlock(bk, bk);
                    double minv = 1 / Mkk.m00;
                    zero3Vector(tmp3);
                    Mkf.mulAdd(tmp3, 0, abuf, fidx);
                    abuf[idx++] = minv * (fbuf[idx] - tmp3[0]);
                    abuf[idx++] = minv * (fbuf[idx] - tmp3[1]);
                    abuf[idx++] = minv * (fbuf[idx] - tmp3[2]);
                }
            }
        }
    } else {
        for (int i = 0; i < myNodes.size(); i++) {
            FemNode3d n = myNodes.get(i);
            if ((bk = n.getSolveIndex()) != -1) {
                int idx = M.getBlockRowOffset(bk);
                if (idx < asize) {
                    n.mulInverseEffectiveMass(M.getBlock(bk, bk), abuf, fbuf, idx);
                }
            }
        }
    }
}

Example 4 with Twist

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

the class PointFem3dAttachment method computeVelDerivative.

private boolean computeVelDerivative(Vector3d dvel) {
    Frame pframe = myPoint.getPointFrame();
    boolean isNonZero = false;
    // can we optimize this? If the attachment has been enforced, then can we
    // compute lp2 and lv2 directly from the point itself?
    Vector3d lv2 = new Vector3d();
    Vector3d lp2 = new Vector3d();
    double[] coords = myCoords.getBuffer();
    for (int i = 0; i < myNodes.length; i++) {
        FemNode node = myNodes[i];
        double w = coords[i];
        lv2.scaledAdd(w, node.getLocalVelocity());
        lp2.scaledAdd(w, node.getLocalPosition());
    }
    if (myFemFrame != null) {
        RotationMatrix3d R2 = myFemFrame.getPose().R;
        Twist vel2 = myFemFrame.getVelocity();
        Vector3d tmp1 = new Vector3d();
        Vector3d tmp2 = new Vector3d();
        // R2*lp2
        tmp1.transform(R2, lp2);
        // R2*lv2
        tmp2.transform(R2, lv2);
        // 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);
        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);
        }
        isNonZero = true;
    } else if (pframe != null) {
        RotationMatrix3d R1 = pframe.getPose().R;
        Twist vel1 = pframe.getVelocity();
        // dvel = R1^T (w1 X (u1 - R1*lv1 - lv2))
        // R1*lv1
        dvel.transform(R1, myPoint.getLocalVelocity());
        dvel.negate();
        // since Fem has no frame, lv2 and world velocity are the same
        dvel.sub(lv2);
        dvel.add(vel1.v);
        dvel.cross(vel1.w, dvel);
        dvel.inverseTransform(R1);
        isNonZero = true;
    }
    return isNonZero;
}

Example 5 with Twist

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

the class FrameSpring method copy.

@Override
public ModelComponent copy(int flags, Map<ModelComponent, ModelComponent> copyMap) {
    FrameSpring comp = (FrameSpring) super.copy(flags, copyMap);
    if (myRenderProps != null) {
        comp.setRenderProps(myRenderProps);
    }
    if (myX1A != RigidTransform3d.IDENTITY) {
        comp.myX1A = new RigidTransform3d(myX1A);
    }
    if (myX2B != RigidTransform3d.IDENTITY) {
        comp.myX2B = new RigidTransform3d(myX2B);
    }
    comp.myF = new Wrench();
    comp.myFTmp = new Wrench();
    comp.myVel1 = new Twist();
    comp.myVel2 = new Twist();
    comp.myVel21 = new Twist();
    comp.myTmp00 = new Matrix6d();
    comp.myTmp01 = new Matrix6d();
    comp.myTmp10 = new Matrix6d();
    comp.myTmp11 = new Matrix6d();
    comp.myRenderFrame = new RigidTransform3d();
    comp.myRenderPnt1 = new float[3];
    comp.myRenderPnt2 = new float[3];
    comp.myTmpM = new Matrix3d();
    // comp.myRBA = new RotationMatrix3d();
    comp.myX21 = new RigidTransform3d();
    comp.myMaterial = (FrameMaterial) myMaterial.clone();
    return comp;
}