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

Example 6 with Wrench

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

the class Frame method copy.

public Frame copy(int flags, Map<ModelComponent, ModelComponent> copyMap) {
    Frame comp = (Frame) super.copy(flags, copyMap);
    comp.myState = new FrameState();
    comp.myForce = new Wrench();
    comp.myExternalForce = new Wrench();
    comp.myRenderFrame = new RigidTransform3d();
    comp.myAxisLength = myAxisLength;
    comp.mySolveBlock = null;
    comp.mySolveBlockNum = -1;
    comp.mySolveBlockValidP = false;
    comp.setPose(getPose());
    return comp;
}

Example 7 with Wrench

use of maspack.spatialmotion.Wrench 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 Wrench

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

the class LemkeContactSolver method activateFriction.

private void activateFriction(int ci) {
    // boolean z0isBasic = z0Var.isBasic;
    // System.out.println ("activating friction " + ci);
    zVars[numVars + numd + 1].set(z0Var);
    z0Var = zVars[numVars + numd + 1];
    int di = numVars;
    if (frictionStatus[ci] == FRICTION_INACTIVE) {
        // first time friction
        // activated, so compute
        // friction directions
        Point3d pnt = contactInfo[ci].pnt;
        // set x direction to be parallel to current tangent velocity
        // current body
        computeVelocity(currentVel, wtotalAdjusted);
        // velocity
        // compute tangent velocity
        vtmp.cross(currentVel.w, pnt);
        vtmp.add(currentVel.v);
        ydir.cross(vtmp, contactInfo[ci].nrm);
        double mag = ydir.norm();
        if (smartDirections && mag >= WORKING_PREC) {
            ydir.scale(1 / mag);
            xdir.cross(ydir, contactInfo[ci].nrm);
            xdir.normalize();
        }
        // else
        {
            R.setIdentity();
            R.setZDirection(contactInfo[ci].nrm);
            R.getColumn(0, xdir);
            R.getColumn(1, ydir);
        }
        for (int j = 0; j < numd; j++) {
            double cos = Math.cos(2 * Math.PI * j / (double) numd);
            double sin = Math.sin(2 * Math.PI * j / (double) numd);
            Wrench D = NDBuffer[numc + ci * numd + j];
            D.f.combine(cos, xdir, sin, ydir);
            D.m.cross(pnt, D.f);
            bq[di + j] = D.dot(contactInfo[ci].otherBodyVelocity);
            if (myInertia != null) {
                myInertia.mulLeftFactorInverse(D, D);
            }
        }
    }
    for (int j = 0; j < numd; j++) {
        Wrench D = NDBuffer[numc + ci * numd + j];
        int idx = di + j;
        zVars[idx].init(PHI, ci, di, wVars[idx], D);
        wVars[idx].init(SIGMA, ci, di, zVars[idx], D);
        c[idx] = 1;
    }
    int idx = di + numd;
    zVars[idx].init(LAMBDA, ci, di, wVars[idx], null);
    wVars[idx].init(GAMMA, ci, di, zVars[idx], null);
    c[idx] = 1;
    bq[idx] = 0;
    zVars[ci].di = di;
    wVars[ci].di = di;
    for (int j = 0; j < numd + 1; j++) {
        varToActive[numVars + j] = numActiveVars + j;
        activeToVar[numActiveVars + j] = numVars + j;
    }
    frictionStatus[ci] = FRICTION_ACTIVE;
    numVars += (numd + 1);
    numActiveVars += (numd + 1);
}

Example 9 with Wrench

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

the class LemkeContactSolver method solve.

// private void printStepInfo (
// String msg, double[] mv, double[] qv, ContactVariable driveVar,
// ContactVariable blockingVar, int[] candidates, int numCand)
// {
// int[] indices = new int[numActiveVars];
// Variable[] vars = new Variable[numActiveVars];
// // int dec = 0;
// // for (int i=0; i<numVars; i++)
// // { if (wVars[i].di == i && !frictionActive(wVars[i]))
// // { dec += numd+1;
// // i += numd;
// // }
// // else if (wVars[i].isBasic)
// // { vars[i-dec] = wVars[i];
// // }
// // else if (zVars[i].isBasic)
// // { vars[i-dec] = zVars[i];
// // }
// // else
// // { vars[i-dec] = z0Var;
// // }
// // }
// // System.out.println ("dec=" + dec);
// for (int i=0; i<numActiveVars; i++)
// { int vari = activeToVar[i];
// if (wVars[vari].isBasic)
// { vars[i] = wVars[vari];
// }
// else if (zVars[vari].isBasic)
// { vars[i] = zVars[vari];
// }
// else
// { vars[i] = z0Var;
// }
// }
// int i = 0;
// // ContactVariable[] zAlpha = getZAlphaVars();
// // int i = 0;
// // for (int k=0; k<zAlpha.length; k++)
// // { indices[i++] = zAlpha[k] == z0Var ? wzVar.idx : zAlpha[k].idx;
// // }
// // for (ContactVariable wvar=wBeta.head; wvar!=null; wvar=wvar.next)
// // { indices[i++] = wvar.idx;
// // }
// for (i=0; i<numActiveVars; i++)
// { indices[i] = i;
// }
// // if (numVars != numActiveVars)
// // { for (i=0; i<numActiveVars; i++)
// // { indices[i] = compressIndex (indices[i]);
// // System.out.println (indices[i]);
// // }
// // }
// printStepInfo (msg, vars, mv, qv, numActiveVars, driveVar,
// blockingVar, candidates, numCand, indices);
// }
// public int solve (Twist vel, Point3d[] pnts, Vector3d[] nrms,
// double[] mus, int ncontacts,
// SpatialInertia inertia, Twist tw)
// {
// ContactInfo[] cinfo = new ContactInfo[ncontacts];
// for (int i=0; i<ncontacts; i++)
// { ContactInfo c = new ContactInfo();
// c.mu = mus[i];
// c.pnt = pnts[i];
// c.nrm = nrms[i];
// cinfo[i] = c;
// }
// Wrench wapplied = new Wrench();
// inertia.mul (wapplied, tw);
// return solve (vel, cinfo, ncontacts, inertia,
// wapplied, numFrictionDirs);
// }
/**
 * Applies an impulse to a rigid body velocity to adjust for normal and
 * frictional contact forces.
 *
 * @param tr
 * velocity with contact impulse added
 * @param contacts
 * provides information about the each contact
 * @param ncontacts
 * number of contacts
 * @param inertia
 * spatial inertia of the rigid body
 * @param tw
 * initial velocity of the rigid body
 * @param restitution
 * coefficent of restitution
 */
public int solve(Twist tr, ContactInfo[] contacts, int ncontacts, SpatialInertia inertia, Twist tw, double restitution) {
    if (ncontacts == 0) {
        tr.set(tw);
        return SOLVED;
    }
    Wrench wapplied = new Wrench();
    inertia.mul(wapplied, tw);
    return solve(tr, contacts, ncontacts, inertia, wapplied, restitution);
}

Example 10 with Wrench

use of maspack.spatialmotion.Wrench 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;
}