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;
}
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;
}
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);
}
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);
}
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;
}
Aggregations