double dVector3Length(final DVector3 a) { //return dSqrt(a.v[0]*a.v[0]+a.v[1]*a.v[1]+a.v[2]*a.v[2]); return a.length(); }
private static double length (DVector3 a) { return a.length(); //return dSqrt (a.v[0]*a.v[0] + a.v[1]*a.v[1] + a.v[2]*a.v[2]); }
double dVector3Length(final DVector3 a) { //return dSqrt(a.v[0]*a.v[0]+a.v[1]*a.v[1]+a.v[2]*a.v[2]); return a.length(); }
private static void inspectJoints() { final double forcelimit = 2000.0; int i; for (i=0; i<SEGMCNT-1; i++) { if (hinges[i].getBody(0)!=null) { // This joint has not snapped already... inspect it. double l0 = jfeedbacks[i].f1.length();// dLENGTH(jfeedbacks[i].f1); double l1 = jfeedbacks[i].f2.length();// dLENGTH(jfeedbacks[i].f2); colours[i+0] = 0.95*colours[i+0] + 0.05 * l0/forcelimit; colours[i+1] = 0.95*colours[i+1] + 0.05 * l1/forcelimit; if (l0 > forcelimit || l1 > forcelimit) stress[i]++; else stress[i]=0; if (stress[i]>4) { // Low-pass filter the noisy feedback data. // Only after 4 consecutive timesteps with excessive load, snap. System.err.println("SNAP! (that was the sound of joint " + i + " breaking)"); hinges[i].attach (null, null); } } } }
private static void inspectJoints() { final double forcelimit = 2000.0; int i; for (i=0; i<SEGMCNT-1; i++) { if (dJointGetBody(hinges[i], 0)!=null) { // This joint has not snapped already... inspect it. double l0 = jfeedbacks[i].f1.length();// dLENGTH(jfeedbacks[i].f1); double l1 = jfeedbacks[i].f2.length();// dLENGTH(jfeedbacks[i].f2); colours[i+0] = 0.95*colours[i+0] + 0.05 * l0/forcelimit; colours[i+1] = 0.95*colours[i+1] + 0.05 * l1/forcelimit; if (l0 > forcelimit || l1 > forcelimit) stress[i]++; else stress[i]=0; if (stress[i]>4) { // Low-pass filter the noisy feedback data. // Only after 4 consecutive timesteps with excessive load, snap. fprintf(stderr,"SNAP! (that was the sound of joint %d breaking)\n", i); dJointAttach (hinges[i], null, null); } } } }
private DVector3 getDragTorque(double density, DBody odeBody, double area) { DVector3 dragTorque = new DVector3(odeBody.getAngularVel()); double avel = dragTorque.length(); dragTorque.safeNormalize(); dragTorque.scale(-0.5 * density * area * avel * avel * 0.5); return dragTorque; }
private DVector3 getDragForce(double density, DBody odeBody, double area) { DVector3 dragForce = new DVector3(odeBody.getLinearVel()); double lvel = dragForce.length(); dragForce.safeNormalize(); dragForce.scale(-0.5 * density * area * lvel * lvel * 0.5); return dragForce; }
public double dGeomSpherePointDepth (DVector3C xyz) { recomputePosr(); DVector3 pos = new DVector3(); pos.eqDiff(xyz, final_posr().pos()); return _radius - pos.length(); }
public double dGeomSpherePointDepth (DVector3C xyz) { recomputePosr(); DVector3 pos = new DVector3(); pos.eqDiff(xyz, final_posr().pos()); return _radius - pos.length(); }
public double dGeomSpherePointDepth (double x, double y, double z) { //dUASSERT (g && g.type == dSphereClass,"argument not a sphere"); recomputePosr(); //dxSphere s = (dxSphere) g; // double [] pos = final_posr.pos.v; // return _radius - dSqrt ((x-pos[0])*(x-pos[0]) + // (y-pos[1])*(y-pos[1]) + // (z-pos[2])*(z-pos[2])); DVector3 pos = new DVector3(x, y, z); pos.sub(final_posr().pos()); return _radius - pos.length(); } public double dGeomSpherePointDepth (DVector3C xyz)
a.scale(1./a.length()); return true;
public double dGeomSpherePointDepth (double x, double y, double z) { //dUASSERT (g && g.type == dSphereClass,"argument not a sphere"); recomputePosr(); //dxSphere s = (dxSphere) g; // double [] pos = final_posr.pos.v; // return _radius - dSqrt ((x-pos[0])*(x-pos[0]) + // (y-pos[1])*(y-pos[1]) + // (z-pos[2])*(z-pos[2])); DVector3 pos = new DVector3(x, y, z); pos.sub(final_posr().pos()); return _radius - pos.length(); } public double dGeomSpherePointDepth (DVector3C xyz)
scale(1./length()); return true;
scale(1./length()); return true;
a.scale(1./a.length()); return true;
/** * Function that computes ax1,ax2 = axis 1 and 2 in global coordinates (they are * relative to body 1 and 2 initially) and then computes the constrained * rotational axis as the cross product of ax1 and ax2. * the sin and cos of the angle between axis 1 and 2 is computed, this comes * from dot and cross product rules. * * @param ax1 Will contain the joint axis1 in world frame * @param ax2 Will contain the joint axis2 in world frame * @param axis Will contain the cross product of ax1 x ax2 * @param sin_angle * @param cos_angle */ private void getAxisInfo(DVector3 ax1, DVector3 ax2, DVector3 axCross, RefDouble sin_angle, RefDouble cos_angle) { dMultiply0_331 (ax1, node[0].body.posr().R(), _axis1); dMultiply0_331 (ax2, node[1].body.posr().R(), _axis2); dCalcVectorCross3 (axCross ,ax1, ax2); sin_angle.d = axCross.length();//dSqrt (axCross[0]*axCross[0] + axCross[1]*axCross[1] + axCross[2]*axCross[2]); cos_angle.d = ax1.dot(ax2);//dDOT (ax1,ax2); }
/** * Function that computes ax1,ax2 = axis 1 and 2 in global coordinates (they are * relative to body 1 and 2 initially) and then computes the constrained * rotational axis as the cross product of ax1 and ax2. * the sin and cos of the angle between axis 1 and 2 is computed, this comes * from dot and cross product rules. * * @param ax1 Will contain the joint axis1 in world frame * @param ax2 Will contain the joint axis2 in world frame * @param axis Will contain the cross product of ax1 x ax2 * @param sin_angle * @param cos_angle */ private void getAxisInfo(DVector3 ax1, DVector3 ax2, DVector3 axCross, RefDouble sin_angle, RefDouble cos_angle) { dMultiply0_331 (ax1, node[0].body.posr().R(), _axis1); dMultiply0_331 (ax2, node[1].body.posr().R(), _axis2); dCalcVectorCross3 (axCross ,ax1, ax2); sin_angle.d = axCross.length();//dSqrt (axCross[0]*axCross[0] + axCross[1]*axCross[1] + axCross[2]*axCross[2]); cos_angle.d = ax1.dot(ax2);//dDOT (ax1,ax2); }
double sep = sides.length(); noGyroBody.setPosition( -sep, 0, sep); expGyroBody.setPosition( 0, 0, sep);