/** * TZ */ @Override public void setI(DMatrix3C I) { _I.set(I); }
void dMatrix3Inv( final DMatrix3 ma, DMatrix3 dst ) { double det = dMatrix3Det( ma ); if ( dFabs( det ) < (0.0005) ) { dst.setIdentity(); return; } double detRecip = 1.0 / det; dst.set00( (ma.get11()*ma.get22() - ma.get12()*ma.get21()) * detRecip ); dst.set01( (ma.get21()*ma.get02() - ma.get01()*ma.get22()) * detRecip ); dst.set02( (ma.get01()*ma.get12() - ma.get11()*ma.get02()) * detRecip ); dst.set10( (ma.get12()*ma.get20() - ma.get10()*ma.get22()) * detRecip ); dst.set11( (ma.get00()*ma.get22() - ma.get20()*ma.get02()) * detRecip ); dst.set12( (ma.get10()*ma.get02() - ma.get00()*ma.get12()) * detRecip ); dst.set20( (ma.get10()*ma.get21() - ma.get20()*ma.get11()) * detRecip ); dst.set21( (ma.get20()*ma.get01() - ma.get00()*ma.get21()) * detRecip ); dst.set22( (ma.get00()*ma.get11() - ma.get01()*ma.get10()) * detRecip ); }
DMatrix3 ahat = new DMatrix3(),chat = new DMatrix3(); DMatrix3 t1 = new DMatrix3(),t2 = new DMatrix3(); dMultiply0_333 (t2,chat,chat); for (i=0; i<3; i++) for (j=0; j<3; j++) _I.add(i, j, _mass * (t2.get(i, j)-t1.get(i, j)) ); _I.set10( _I.get01() );//v[_I(1,0)] = _I.v[_I(0,1)]; _I.set20( _I.get02() );//v[_I(2,0)] = _I.v[_I(0,2)]; _I.set21( _I.get12() );//v[_I(2,1)] = _I.v[_I(1,2)];
while ((bodyIndex = Atomics.ThrsafeIncrementIntUpToLimit(callContext.m_inertiaBodyIndex, nb)) != nb) { int invIrowP = bodyIndex * IIE__MAX; DMatrix3 tmp = new DMatrix3(); DxBody b = bodyP[bodyIndex + bodyOfs]; DMatrix3 I = new DMatrix3(); DMatrix3 Itild= new DMatrix3();//{0}; Itild.scale(h); Itild.add(I); DMatrix3 itInv = new DMatrix3(); Itild.sub(0, 0, 1); Itild.sub(1, 1, 1); Itild.sub(2, 2, 1);
private void drawAABB(DGeom g) { if (show_aabb) { // draw the bounding box for this geom DAABBC aabb = g.getAABB(); DVector3 bbpos = aabb.getCenter(); DVector3 bbsides = aabb.getLengths(); DMatrix3 RI = new DMatrix3(); RI.setIdentity (); dsSetColorAlpha (1,0,0,0.5f); dsDrawBox (bbpos,RI,bbsides); } }
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); }
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 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)"); }
break; case 'x': case 'X': { DMatrix3 R2 = new DMatrix3(), R3 = new DMatrix3(), R4 = new DMatrix3(); dRFromAxisAndAngle (R2,0,0,1,cannon_angle); dRFromAxisAndAngle (R3,0,1,0,cannon_elevation); 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;
private double cmpIdentity (final DMatrix3C A) { DMatrix3 I = new DMatrix3(); I.setZero();//dSetZero (I,12); // I.v[0] = 1; // I.v[5] = 1; // I.v[10] = 1; I.eqIdentity(); return dMaxDifference (A,I); }
public static void dRSetIdentity (DMatrix3 R) { R.setIdentity(); }
/** * Returns a clone of this Matrix. */ @Override public DMatrix3 clone() { return new DMatrix3(this); }
private double cmpIdentity (final DMatrix3C A) { DMatrix3 I = new DMatrix3(); I.eqIdentity(); return dMaxDifference (A,I); }