/** * Instead, please use a.dot(b). * @param a a * @param b b * @return dot product */ public static double dCalcVectorDot3(DVector3C a, DVector3C b) { return a.dot(b); } public static double dCalcVectorDot3(double[] a, int o1, DVector3C b) { return dDOTpq(a,o1,b); }
public static void dQFromAxisAndAngle (DQuaternion q, DVector3C axyz, double angle) { //dAASSERT (q); double l = axyz.lengthSquared();//ax*ax + ay*ay + az*az; if (l > 0.0) { angle *= 0.5; l = dSin(angle) * dRecipSqrt(l); // q.v[0] = dCos (angle); // q.v[1] = ax*l; // q.v[2] = ay*l; // q.v[3] = az*l; q.set( Math.cos(angle), axyz.get0()*l, axyz.get1()*l, axyz.get2()*l ); } else { q.set( 1, 0, 0, 0); } }
double lspeed = lvel.get0()*lvel.get0()+lvel.get(1)*lvel.get1()+lvel.get2()*lvel.get2(); if (lspeed > DISABLE_THRESHOLD) disable = false; final DVector3C avel = dBodyGetAngularVel(b); double aspeed = avel.get0()*avel.get0()+avel.get1()*avel.get1()+avel.get2()*avel.get2(); if (aspeed > DISABLE_THRESHOLD) disable = false;
RefInt BlocksP = new RefInt(1); double MinX = Center.get(AXIS0) - Extents.get(AXIS0); double MaxX = dNextAfter((Center.get(AXIS0) + Extents.get(AXIS0)), dInfinity); double MinZ = Center.get(AXIS1) - Extents.get(AXIS1); double MaxZ = dNextAfter((Center.get(AXIS1) + Extents.get(AXIS1)), dInfinity); this.Blocks[0].Create(MinX, MaxX, MinZ, MaxZ, null, Depth, BlocksA, BlocksP);
private static void computeMassParams (DMass m, DVector3[] q, DVectorN pm) { double pmi, q0, q1, q2; m.setZero (); DVector3 C = m.getC().clone(); DMatrix3 I = m.getI().clone(); for (int i=0; i<NUM; i++) { pmi = pm.get(i); m.setMass(m.getMass() + pmi);// += pmi; C.eqSum( C, q[i], pmi); q0 = q[i].get0(); q1 = q[i].get1(); q2 = q[i].get2(); I.add(0,0, pmi*(q1*q1 + q2*q2)); I.add(1,1, pmi*(q0*q0 + q2*q2)); I.add(2,2, pmi*(q0*q0 + q1*q1)); I.sub(0,1, pmi*(q0*q1)); I.sub(0,2, pmi*(q0*q2)); I.sub(1,2, pmi*(q1*q2)); } //for (j=0; j<3; j++) m.c.v[j] /= m.mass; C.scale(1./m.getMass()); I.set(1,0, I.get(0,1)); I.set(2,0, I.get(0,2)); I.set(2,1, I.get(1,2)); m.setC(C); m.setI(I); }
double lspeed = lvel.lengthSquared(); if (lspeed > DISABLE_THRESHOLD) disable = false; DVector3C avel = b.getAngularVel(); double aspeed = avel.lengthSquared(); if (aspeed > DISABLE_THRESHOLD) disable = false;
public static void dQFromAxisAndAngle (DQuaternion q, DVector3C axyz, double angle) { //dAASSERT (q); double l = axyz.lengthSquared();//ax*ax + ay*ay + az*az; if (l > 0.0) { angle *= 0.5; l = dSin(angle) * dRecipSqrt(l); // q.v[0] = dCos (angle); // q.v[1] = ax*l; // q.v[2] = ay*l; // q.v[3] = az*l; q.set( Math.cos(angle), axyz.get0()*l, axyz.get1()*l, axyz.get2()*l ); } else { q.set( 1, 0, 0, 0); } }
RefInt BlocksP = new RefInt(1); double MinX = Center.get(AXIS0) - Extents.get(AXIS0); double MaxX = dNextAfter((Center.get(AXIS0) + Extents.get(AXIS0)), dInfinity); double MinZ = Center.get(AXIS1) - Extents.get(AXIS1); double MaxZ = dNextAfter((Center.get(AXIS1) + Extents.get(AXIS1)), dInfinity); this.Blocks[0].Create(MinX, MaxX, MinZ, MaxZ, null, Depth, BlocksA, BlocksP);
private static void computeMassParams (DMass m, DVector3[] q, DVectorN pm) { int i,j; double pmi, q0, q1, q2; dMassSetZero (m); DVector3 C = m.getC().clone(); DMatrix3 I = m.getI().clone(); for (i=0; i<NUM; i++) { pmi = pm.get(i); m.setMass(m.getMass() + pmi);// += pmi; for (j=0; j<3; j++) C.add(j, pmi*q[i].get(j)); q0 = q[i].get0(); q1 = q[i].get1(); q2 = q[i].get2(); I.add(0,0, pmi*(q1*q1 + q2*q2)); I.add(1,1, pmi*(q0*q0 + q2*q2)); I.add(2,2, pmi*(q0*q0 + q1*q1)); I.sub(0,1, pmi*(q0*q1)); I.sub(0,2, pmi*(q0*q2)); I.sub(1,2, pmi*(q1*q2)); } //for (j=0; j<3; j++) m.c.v[j] /= m.mass; C.scale(1./m.getMass()); I.set(1,0, I.get(0,1)); I.set(2,0, I.get(0,2)); I.set(2,1, I.get(1,2)); m.setC(C); m.setI(I); }
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); }
/** * Instead, please use a.dot(b). * @param a a * @param b b * @return dot product */ public static double dCalcVectorDot3(DVector3C a, DVector3C b) { return a.dot(b); } public static double dCalcVectorDot3(double[] a, int o1, DVector3C b) { return dDOTpq(a,o1,b); }
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); }
double dVector3Dot(final DVector3C a,final DVector3C b) { return a.dot(b); }
m.setBox (1, boxDim.get(X), boxDim.get(Y), boxDim.get(Z)); m.adjust (Mass1); geomW = OdeHelper.createBox (space, boxDim.get(X), boxDim.get(Y), boxDim.get(Z)); geomW.setBody (bodyW); geomW.setCategoryBits (catBits[W]); m.setBox (1, boxDim.get(X), boxDim.get(Y), boxDim.get(Z)); m.adjust (Mass1); geomD = OdeHelper.createBox (space, boxDim.get(X), boxDim.get(Y), boxDim.get(Z)); geomD.setBody (bodyD); geomD.setCategoryBits (catBits[D]); geomEXT = OdeHelper.createBox (null, extDim.get(X), extDim.get(Y), extDim.get(Z)); geomEXT.setCategoryBits (catBits[EXT]); geomEXT.setCollideBits ( geomINT = OdeHelper.createBox (null, INT_EXT_RATIO*extDim.get(X), INT_EXT_RATIO*extDim.get(Y), INT_EXT_RATIO*extDim.get(Z)); geomINT.setCategoryBits (catBits[INT]); geomINT.setCollideBits ( geomANCHOR = OdeHelper.createBox (null, ancDim.get(X), ancDim.get(Y), ancDim.get(Z)); geomANCHOR.setCategoryBits (catBits[ANCHOR]); geomANCHOR.setCollideBits (