void dVector3Add(final DVector3 a,final DVector3 b,DVector3 c) { c.eqSum(a, b); // c.v[0] = a.v[0] + b.v[0]; // c.v[1] = a.v[1] + b.v[1]; // c.v[2] = a.v[2] + b.v[2]; }
void Vector3Add( DVector3C left, DVector3C right, DVector3 result ) { // result[0] = left[0] + right[0]; // result[1] = left[1] + right[1]; // result[2] = left[2] + right[2]; // result[3] = REAL(0.0); result.eqSum(left, right); }
void dVector3Add(final DVector3 a,final DVector3 b,DVector3 c) { c.eqSum(a, b); // c.v[0] = a.v[0] + b.v[0]; // c.v[1] = a.v[1] + b.v[1]; // c.v[2] = a.v[2] + b.v[2]; }
void Vector3Add( DVector3C left, DVector3C right, DVector3 result ) { // result[0] = left[0] + right[0]; // result[1] = left[1] + right[1]; // result[2] = left[2] + right[2]; // result[3] = REAL(0.0); result.eqSum(left, right); }
private void ADD(DVector3C a, DVector3C b, DVector3 r) { r.eqSum(a, b); }
private void ADD(DVector3C a, DVector3C b, DVector3 r) { r.eqSum(a, b); }
private void ADD(DVector3C a, DVector3C b, DVector3 r) { r.eqSum(a, b); }
private void ADD(DVector3C a, DVector3C b, DVector3 r) { r.eqSum(a, b); }
private final void FOO2(int i, int j, int op, DContactGeomBuffer contacts, int skip, DVector3 p, DMatrix3C R, DVector3 side) { contacts.get(i*skip).pos.eqSum(p, R.viewCol(j), op*side.get(j)); }
private final void FOO2(int i, int j, int op, DContactGeomBuffer contacts, int skip, DVector3 p, DMatrix3C R, DVector3 side) { contacts.get(i*skip).pos.eqSum(p, R.viewCol(j), op*side.get(j)); }
void dGeomGetRelPointPos (double px, double py, double pz, DVector3 result) { if ((_gflags & GEOM_PLACEABLE) == 0) { result.set(px, py, pz); return; } recomputePosr(); DVector3 prel,p = new DVector3(); prel = new DVector3(px, py, pz); dMultiply0_331 (p,_final_posr.R(),prel); result.eqSum(p, _final_posr.pos()); }
void dGeomGetRelPointPos (double px, double py, double pz, DVector3 result) { if ((_gflags & GEOM_PLACEABLE) == 0) { result.set(px, py, pz); return; } recomputePosr(); DVector3 prel,p = new DVector3(); prel = new DVector3(px, py, pz); dMultiply0_331 (p,_final_posr.R(),prel); result.eqSum(p, _final_posr.pos()); }
private static void answer(final double t, DVector3 tmp, DVector3C sign, DVector3C p1, DVector3 s, DMatrix3C R, DVector3 lret, DVector3C h, DVector3C c, DVector3C v, DVector3 bret) { //got_answer: // compute closest point on the line //for (int i=0; i<3; i++) lret.v[i] = p1.v[i] + t*tmp.v[i]; // note: tmp=p2-p1 lret.eqSum(p1, tmp, t); // compute closest point on the box for (int i=0; i<3; i++) { tmp.set(i, sign.get(i) * (s.get(i) + t*v.get(i)) ); if (tmp.get(i) < -h.get(i)) tmp.set(i, -h.get(i) ); else if (tmp.get(i) > h.get(i)) tmp.set(i, h.get(i) ); } dMultiply0_331 (s,R,tmp); //for (int i=0; i<3; i++) bret.v[i] = s.v[i] + c.v[i]; bret.eqSum(s, c); }
private static void answer(final double t, DVector3 tmp, DVector3C sign, DVector3C p1, DVector3 s, DMatrix3C R, DVector3 lret, DVector3C h, DVector3C c, DVector3C v, DVector3 bret) { //got_answer: // compute closest point on the line //for (int i=0; i<3; i++) lret.v[i] = p1.v[i] + t*tmp.v[i]; // note: tmp=p2-p1 lret.eqSum(p1, tmp, t); // compute closest point on the box for (int i=0; i<3; i++) { tmp.set(i, sign.get(i) * (s.get(i) + t*v.get(i)) ); if (tmp.get(i) < -h.get(i)) tmp.set(i, -h.get(i) ); else if (tmp.get(i) > h.get(i)) tmp.set(i, h.get(i) ); } dMultiply0_331 (s,R,tmp); //for (int i=0; i<3; i++) bret.v[i] = s.v[i] + c.v[i]; bret.eqSum(s, c); }
public void dMassAdd (DMassC b) { //dAASSERT (b); double denom = dRecip (_mass + b.getMass()); //for (i=0; i<3; i++) a._c.v[i] = (a._c.v[i]*a._mass + b._c.v[i]*b._mass)*denom; _c.eqSum( _c, _mass, b.getC(), b.getMass() ).scale( denom ); _mass += b.getMass(); //for (i=0; i<12; i++) a._I.v[i] += b._I.v[i]; _I.add(b.getI()); }
public void dMassAdd (DMassC b) { //dAASSERT (b); double denom = dRecip (_mass + b.getMass()); //for (i=0; i<3; i++) a._c.v[i] = (a._c.v[i]*a._mass + b._c.v[i]*b._mass)*denom; _c.eqSum( _c, _mass, b.getC(), b.getMass() ).scale( denom ); _mass += b.getMass(); //for (i=0; i<12; i++) a._I.v[i] += b._I.v[i]; _I.add(b.getI()); }
private void dJointAddUniversalTorques( double torque1, double torque2 ) { DVector3 axis1 = new DVector3(), axis2 = new DVector3(); if ( isFlagsReverse() ) { double temp = torque1; torque1 = - torque2; torque2 = - temp; } getAxis( axis1, _axis1 ); getAxis2( axis2, _axis2 ); // axis1.v[0] = axis1.v[0] * torque1 + axis2.v[0] * torque2; // axis1.v[1] = axis1.v[1] * torque1 + axis2.v[1] * torque2; // axis1.v[2] = axis1.v[2] * torque1 + axis2.v[2] * torque2; axis1.eqSum( axis1, torque1, axis2, torque2 ); if ( node[0].body != null ) node[0].body.dBodyAddTorque(axis1); if ( node[1].body != null ) node[1].body.dBodyAddTorque( axis1.scale(-1) ); }
private void dJointAddUniversalTorques( double torque1, double torque2 ) { DVector3 axis1 = new DVector3(), axis2 = new DVector3(); if ( isFlagsReverse() ) { double temp = torque1; torque1 = - torque2; torque2 = - temp; } getAxis( axis1, _axis1 ); getAxis2( axis2, _axis2 ); // axis1.v[0] = axis1.v[0] * torque1 + axis2.v[0] * torque2; // axis1.v[1] = axis1.v[1] * torque1 + axis2.v[1] * torque2; // axis1.v[2] = axis1.v[2] * torque1 + axis2.v[2] * torque2; axis1.eqSum( axis1, torque1, axis2, torque2 ); if ( node[0].body != null ) node[0].body.dBodyAddTorque(axis1); if ( node[1].body != null ) node[1].body.dBodyAddTorque( axis1.scale(-1) ); }
private void dJointAddHinge2Torques( double torque1, double torque2 ) { if ( node[0].body != null && node[1].body != null) { DVector3 axis1 = new DVector3(), axis2 = new DVector3(); dMultiply0_331( axis1, node[0].body.posr().R(), _axis1 ); dMultiply0_331( axis2, node[1].body.posr().R(), _axis2 ); // axis1.v[0] = axis1.v[0] * torque1 + axis2.v[0] * torque2; // axis1.v[1] = axis1.v[1] * torque1 + axis2.v[1] * torque2; // axis1.v[2] = axis1.v[2] * torque1 + axis2.v[2] * torque2; axis1.eqSum(axis1, torque1, axis2, torque2); node[0].body.dBodyAddTorque( axis1 ); node[1].body.dBodyAddTorque( axis1.reScale(-1) ); } }
private void dJointAddHinge2Torques( double torque1, double torque2 ) { if ( node[0].body != null && node[1].body != null) { DVector3 axis1 = new DVector3(), axis2 = new DVector3(); dMultiply0_331( axis1, node[0].body.posr().R(), _axis1 ); dMultiply0_331( axis2, node[1].body.posr().R(), _axis2 ); // axis1.v[0] = axis1.v[0] * torque1 + axis2.v[0] * torque2; // axis1.v[1] = axis1.v[1] * torque1 + axis2.v[1] * torque2; // axis1.v[2] = axis1.v[2] * torque1 + axis2.v[2] * torque2; axis1.eqSum(axis1, torque1, axis2, torque2); node[0].body.dBodyAddTorque( axis1 ); node[1].body.dBodyAddTorque( axis1.reScale(-1) ); } }