/** * Compares two matrices for equality. * This is marginally faster than <tt>equals(Object o)</tt>. */ @Override public final boolean isEq(DMatrix3C m) { for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { if (get(i, j) != m.get(i, j)) return false; } } return true; }
/** * Compares two matrices for equality. * This is marginally faster than <tt>equals(Object o)</tt>. */ @Override public final boolean isEq(DMatrix3C m) { for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { if (get(i, j) != m.get(i, j)) return false; } } return true; }
dMultiply0 (R4,R2,R3); double[] cpos = {CANNON_X,CANNON_Y,1}; for (int i=0; i<3; i++) cpos[i] += 3*R4.get(i, 2);//[i*4+2]; dBodySetPosition (cannon_ball_body,cpos[0],cpos[1],cpos[2]); double force = 10; dBodySetLinearVel (cannon_ball_body,force*R4.get(0, 2),force*R4.get(1,2),force*R4.get(2,2)); dBodySetAngularVel (cannon_ball_body,0,0,0); break;
dMultiply0 (R4,R2,R3); double[] cpos = {CANNON_X,CANNON_Y,1}; for (int i=0; i<3; i++) cpos[i] += 3*R4.get(i, 2);//[i*4+2]; cannon_ball_body.setPosition (cpos[0],cpos[1],cpos[2]); double force = 10; cannon_ball_body.setLinearVel (force*R4.get(0, 2),force*R4.get(1,2),force*R4.get(2,2)); cannon_ball_body.setAngularVel (0,0,0); break;
private void testInvertPDMatrixM3() { int i,j,ok; DMatrix3 A = new DMatrix3(), Ainv = new DMatrix3(); DMatrix3 I = new DMatrix3(); HEADER(); dMakeRandomMatrix (A,1.0); dMultiply2 (Ainv,A,A); //System.arraycopy(Ainv, 0, A, 0, MSIZE4*MSIZE); A.set(Ainv); Ainv.setZero();//dSetZero (Ainv,MSIZE4*MSIZE); if (dInvertPDMatrix (A,Ainv)) println ("\tpassed (1)"); else println ("\tFAILED (1)"); dMultiply0 (I,A,Ainv); // compare with identity ok = 1; for (i=0; i<3; i++) { for (j=0; j<3; j++) { if (i != j) if (cmp (I.get(i, j),0.0)==false) ok = 0; } } for (i=0; i<3; i++) { if (cmp (I.get(i, i),1.0)==false) ok = 0; } if (ok != 0) println ("\tpassed (2)"); else println ("\tFAILED (2)"); }
y = r*Math.cos(beta); beta = dRandReal()-0.5; for (j=0; j<3; j++) a.set(j, p.get(j) + x*R.get(j,0) + y*R.get(j,1) + l*beta*R.get(j,2) ); if (dFabs(ccyl.getPointDepth (a)) >= tol) if (testFAILED()) return false; a.normalize(); if (dCalcVectorDot3_14(a,R,2) > 0) { for (j=0; j<3; j++) a.set(j, p.get(j) + a.get(j)*r + l*0.5*R.get(j,2) ); for (j=0; j<3; j++) a.set(j, p.get(j) + a.get(j)*r - l*0.5*R.get(j,2) ); a.normalize(); beta = dRandReal()-0.5; for (j=0; j<3; j++) a.set(j, p.get(j) + a.get(j)*r*0.99 + l*beta*R.get(j,2) ); if (ccyl.getPointDepth (a) < 0) if (testFAILED()) return false; y = (r-d)*Math.cos(beta); beta = dRandReal()-0.5; for (j=0; j<3; j++) a.set(j, p.get(j) + x*R.get(j,0) + y*R.get(j,1) + l*beta*R.get(j,2) ); if (dFabs(ccyl.getPointDepth (a) - d) >= tol) if (testFAILED()) return false; a.normalize(); if (dCalcVectorDot3_14(a,R,2) > 0) { for (j=0; j<3; j++) a.set(j, p.get(j) + a.get(j)*(r-d) + l*0.5*R.get(j,2) ); for (j=0; j<3; j++) a.set(j, p.get(j) + a.get(j)*(r-d) - l*0.5*R.get(j,2) );
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); }
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); }
for (j=0; j<3; j++) a.set(j, p.get(j) + l*beta*R.get(j, 2) );//[j*4+2] ); if (dFabs(dGeomCapsulePointDepth (ccyl,a.get0(),a.get1(),a.get2()) - r) >= tol) if (testFAILED()) return false; y = r*cos(beta); beta = dRandReal()-0.5; for (j=0; j<3; j++) a.set(j, p.get(j) + x*R.get(j, 0) + y*R.get(j, 1) + l*beta*R.get(j, 2) ); if (dFabs(dGeomCapsulePointDepth (ccyl,a.get0(),a.get1(),a.get2())) >= tol) if (testFAILED()) return false; dNormalize3 (a); if (dCalcVectorDot3_14(a,R,2) > 0) { for (j=0; j<3; j++) a.set(j, p.get(j) + a.get(j)*r + l*0.5*R.get(j, 2) );//v[j*4+2] ); for (j=0; j<3; j++) a.set(j, p.get(j) + a.get(j)*r - l*0.5*R.get(j, 2) );//v[j*4+2] ); dNormalize3 (a); beta = dRandReal()-0.5; for (j=0; j<3; j++) a.set(j, p.get(j) + a.get(j)*r*0.99 + l*beta*R.get(j, 2) );//v[j*4+2] ); if (dGeomCapsulePointDepth (ccyl,a.get0(),a.get1(),a.get2()) < 0) if (testFAILED()) return false; y = (r-d)*cos(beta); beta = dRandReal()-0.5; for (j=0; j<3; j++) a.set(j, p.get(j) + x*R.get(j, 0) + y*R.get(j, 1) + l*beta*R.get(j, 2) ); if (dFabs(dGeomCapsulePointDepth (ccyl,a.get0(),a.get1(),a.get2()) - d) >= tol) if (testFAILED()) return false; dNormalize3 (a); if (dCalcVectorDot3_14(a,R,2) > 0) { for (j=0; j<3; j++) a.set(j, p.get(j) + a.get(j)*(r-d) + l*0.5*R.get(j, 2) );//v[j*4+2] );
private void computeMassParams (DMass m, DVector3[] q, double[] pm) { //assertTrue(q.length==NUMP && q[0].length==3 && pm.length==NUMP); dIASSERT(q.length==NUMP && pm.length==NUMP); int i; m.setZero (); for (i=0; i<NUMP; i++) { m.setMass( m.getMass() + pm[i]);// += pm[i]; //for (j=0; j<3; j++) m.getC().v[j] += pm[i]*q[i][j]; DVector3 cTmp = new DVector3(m.getC()); m.setC( cTmp.add(pm[i]*q[i].get0(), pm[i]*q[i].get1(), pm[i]*q[i].get2()) ); DMatrix3 I = new DMatrix3(m.getI()); I.add(0,0, pm[i]*(q[i].get1()*q[i].get1() + q[i].get2()*q[i].get2()) ); I.add(1,1, pm[i]*(q[i].get0()*q[i].get0() + q[i].get2()*q[i].get2()) ); I.add(2,2, pm[i]*(q[i].get0()*q[i].get0() + q[i].get1()*q[i].get1()) ); I.add(0,1, -pm[i]*(q[i].get0()*q[i].get1()) ); I.add(0,2, -pm[i]*(q[i].get0()*q[i].get2()) ); I.add(1,2, -pm[i]*(q[i].get1()*q[i].get2()) ); m.setI(I); } //for (j=0; j<3; j++) m.getC().v[j] /= m.getMass(); DVector3 cTmp = new DVector3(m.getC()); m.setC( cTmp.scale(1./m.getMass()) ); DMatrix3 I = new DMatrix3(m.getI()); 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.setI(I); }
void computeMassParams (DMass m, DVector3[] q, double[] pm) { //TODO assertTrue(q.length==NUMP && q[0].length==3 && pm.length==NUMP); dIASSERT(q.length==NUMP && pm.length==NUMP); int i; dMassSetZero (m); for (i=0; i<NUMP; i++) { m.setMass( m.getMass() + pm[i]);// += pm[i]; //for (j=0; j<3; j++) m.getC().v[j] += pm[i]*q[i][j]; DVector3 cTmp = new DVector3(m.getC()); m.setC( cTmp.add(pm[i]*q[i].get0(), pm[i]*q[i].get1(), pm[i]*q[i].get2()) ); DMatrix3 I = new DMatrix3(m.getI()); I.add(0,0, pm[i]*(q[i].get1()*q[i].get1() + q[i].get2()*q[i].get2()) ); I.add(1,1, pm[i]*(q[i].get0()*q[i].get0() + q[i].get2()*q[i].get2()) ); I.add(2,2, pm[i]*(q[i].get0()*q[i].get0() + q[i].get1()*q[i].get1()) ); I.add(0,1, -pm[i]*(q[i].get0()*q[i].get1()) ); I.add(0,2, -pm[i]*(q[i].get0()*q[i].get2()) ); I.add(1,2, -pm[i]*(q[i].get1()*q[i].get2()) ); m.setI(I); } //for (j=0; j<3; j++) m.getC().v[j] /= m.getMass(); DVector3 cTmp = new DVector3(m.getC()); m.setC( cTmp.scale(1./m.getMass()) ); DMatrix3 I = new DMatrix3(m.getI()); 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.setI(I); }
a.normalize (); k = (dRandReal()-0.5)*l; for (j=0; j<3; j++) a.set(j, p.get(j) + r*0.99*a.get(j) + k*0.99*R.get(j,2) ); for (j=0; j<3; j++) b.set(j, dRandReal()-0.5 ); b.normalize (); k = (dRandReal()-0.5)*l; for (j=0; j<3; j++) b.set(j, p.get(j) + r*0.99*b.get(j) + k*0.99*R.get(j,2) ); ray.setLength (a.distance(b)); b.sub( a ); x = Math.sin(k); y = Math.cos(k); for (j=0; j<3; j++) a.set(j, x*R.get(j,0) + y*R.get(j,1) ); k = (dRandReal()-0.5)*l; for (j=0; j<3; j++) b.set(j, -a.get(j)*r*2 + k*R.get(j,2) + p.get(j) ); ray.set (b, a); ray.setLength (r*0.99); a.normalize (); if (dCalcVectorDot3_14(a,R,2) < 0) { for (j=0; j<3; j++) b.set(j, p.get(j) - a.get(j)*2*r + l*0.5*R.get(j,2) ); for (j=0; j<3; j++) b.set(j, p.get(j) - a.get(j)*2*r - l*0.5*R.get(j,2) );
dNormalize3 (a); k = (dRandReal()-0.5)*l; for (j=0; j<3; j++) a.set(j, p.get(j) + r*0.99*a.get(j) + k*0.99*R.get(j, 2) );//v[j*4+2] ); for (j=0; j<3; j++) b.set(j, dRandReal()-0.5 ); dNormalize3 (b); k = (dRandReal()-0.5)*l; for (j=0; j<3; j++) b.set(j, p.get(j) + r*0.99*b.get(j) + k*0.99*R.get(j, 2) );//v[j*4+2] ); dGeomRaySetLength (ray,a.distance(b)); for (j=0; j<3; j++) b.add(j,-a.get(j) ); x = sin(k); y = cos(k); for (j=0; j<3; j++) a.set(j, x*R.get(j, 0) + y*R.get(j, 1) ); k = (dRandReal()-0.5)*l; for (j=0; j<3; j++) b.set(j, -a.get(j)*r*2 + k*R.get(j, 2) + p.get(j) ); dGeomRaySet (ray,b.get0(),b.get1(),b.get2(),a.get0(),a.get1(),a.get2()); dGeomRaySetLength (ray,r*0.99); dNormalize3 (a); if (dCalcVectorDot3_14(a,R,2) < 0) { for (j=0; j<3; j++) b.set(j, p.get(j) - a.get(j)*2*r + l*0.5*R.get(j, 2) );//v[j*4+2] ); for (j=0; j<3; j++) b.set(j, p.get(j) - a.get(j)*2*r - l*0.5*R.get(j, 2) );//v[j*4+2] );
DVector3 csides = new DVector3(2,2,2); dsDrawBox (cpos,R2,csides); for (i=0; i<3; i++) cpos.add(i, 1.5*R4.get(i, 2));//[i*4+2]); dsDrawCylinder (cpos,R4,3f,0.5f);
DVector3 csides = new DVector3(2,2,2); dsDrawBox (cpos,R2,csides); for (i=0; i<3; i++) cpos.add(i, 1.5*R4.get(i, 2));//[i*4+2]); dsDrawCylinder (cpos,R4,3f,0.5f);