void dBodySetKinematic () { //dSetZero (b->invI,4*3); invI.setZero(); invMass = 0; }
void dBodySetKinematic () { //dSetZero (b->invI,4*3); invI.setZero(); invMass = 0; }
public void dMassSetZero () { //dAASSERT (m); _mass = 0.0; _c.setZero();//dSetZero (_c.v);//,sizeof(m.c) / sizeof(double)); _I.setZero();//dSetZero (_I.v);//,sizeof(m.I) / sizeof(double)); }
public void dMassSetZero () { //dAASSERT (m); _mass = 0.0; _c.setZero();//dSetZero (_c.v);//,sizeof(m.c) / sizeof(double)); _I.setZero();//dSetZero (_I.v);//,sizeof(m.I) / sizeof(double)); }
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); }
return false; Ainv.setZero(); for (i = 0; i < 3; i++) {
return false; Ainv.setZero(); for (i = 0; i < 3; 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)"); }