@SuppressWarnings("unused") private void printMassParams (DMass m) { println ("mass = " + m.getMass()); println ("com = " + m.getC()); println ("I = " + m.getI()); }
private void compareMassParams (DMass m1, DMass m2, final String msg) { int i,j; boolean ok = true; if (!(cmp(m1.getMass(),m2.getMass()) && cmp(m1.getC().get0(),m2.getC().get0()) && cmp(m1.getC().get1(),m2.getC().get1()) && cmp(m1.getC().get2(),m2.getC().get2()))) ok = false; for (i=0; i<3; i++) for (j=0; j<3; j++) if (cmp (m1.getI().get(i,j),m2.getI().get(i,j))==false) ok = false; if (ok) println ("\tpassed (", msg, ")"); else println ("\tFAILED (", msg, ")"); }
void compareMassParams (DMass m1, DMass m2, final String msg) { int i,j; boolean ok = true; if (!(cmp(m1.getMass(),m2.getMass()) && cmp(m1.getC().get0(),m2.getC().get0()) && cmp(m1.getC().get1(),m2.getC().get1()) && cmp(m1.getC().get2(),m2.getC().get2()))) ok = false; for (i=0; i<3; i++) for (j=0; j<3; j++) if (cmp (m1.getI().get(i,j),m2.getI().get(i,j))==false) ok = false; if (ok) printf ("\tpassed (%s)\n",msg); else printf ("\tFAILED (%s)\n",msg); }
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); }
void printMassParams (DMass m) { printf ("mass = %.4f\n",m.getMass()); printf ("com = (%.4f,%.4f,%.4f)\n",m.getC().get0(),m.getC().get1(),m.getC().get2()); DMatrix3C I = m.getI(); printf ("I = [ %10.4f %10.4f %10.4f ]\n" + " [ %10.4f %10.4f %10.4f ]\n" + " [ %10.4f %10.4f %10.4f ]\n", I.get00(),I.get01(),I.get02(), I.get10(),I.get11(),I.get12(), I.get20(),I.get21(),I.get22()); }
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); }
totalMass += m.getMass(); DxRagdollBody bone = new DxRagdollBody(body, cyllen, radius, body.getPosition(), body.getQuaternion()); bones.add(bone);
totalMass += m.getMass(); DxRagdollBody bone = new DxRagdollBody(body, cyllen, radius, body.getPosition(), body.getQuaternion()); bones.add(bone);
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); }
DMass m = dMassCreate(); dBodyGetMass(obj[selected].body, m); printf("Object G=%f\n", GRAVITY*m.getMass());
" passed (2)\n" , " FAILED (2)\n"); DMatrix3 I =(DMatrix3) m.getI(); if (m.getMass()==10 && m.getC().get0()==0.1 && m.getC().get1()==0.2 && m.getC().get2()==0.15 && I.get00()==3 && I.get11()==5 && I.get22()==14 && I.get01()==3.1 && I.get02()==3.2 && I.get12()==4 && m.setSphere (1.4, 0.86); I = (DMatrix3)m.getI(); if (cmp(m.getMass(),3.73002719949386) && m.getC().get0()==0 && m.getC().get1()==0 && m.getC().get2()==0 && cmp(I.get00(),1.10349124669826) && cmp(I.get11(),1.10349124669826) && m.setCapsule (1.3,1,0.76,1.53); I = (DMatrix3)m.getI(); if (cmp(m.getMass(),5.99961928996029) && m.getC().get0()==0 && m.getC().get1()==0 && m.getC().get2()==0 && cmp(I.get00(),1.59461986077384) && cmp(I.get11(),4.21878433864904) && m.setBox (0.27,3,4,5); I = (DMatrix3)m.getI(); if (cmp(m.getMass(),16.2) && m.getC().get0()==0 && m.getC().get1()==0 && m.getC().get2()==0 && cmp(I.get00(),55.35) && cmp(I.get11(),45.9) && cmp(I.get22(),33.75) && I.get01()==0 && I.get02()==0 && I.get12()==0 && m2.setMass( m1.getMass() ); m2.translate (1,2,-3); for (i=0; i<NUMP; i++) {
" passed (2)\n" , " FAILED (2)\n"); DMatrix3 I =(DMatrix3) m.getI(); if (m.getMass()==10 && m.getC().get0()==0.1 && m.getC().get1()==0.2 && m.getC().get2()==0.15 && I.get00()==3 && I.get11()==5 && I.get22()==14 && I.get01()==3.1 && I.get02()==3.2 && I.get12()==4 && dMassSetSphere (m,1.4, 0.86); I = (DMatrix3)m.getI(); if (cmp(m.getMass(),3.73002719949386) && m.getC().get0()==0 && m.getC().get1()==0 && m.getC().get2()==0 && cmp(I.get00(),1.10349124669826) && cmp(I.get11(),1.10349124669826) && dMassSetCapsule (m,1.3,1,0.76,1.53); I = (DMatrix3)m.getI(); if (cmp(m.getMass(),5.99961928996029) && m.getC().get0()==0 && m.getC().get1()==0 && m.getC().get2()==0 && cmp(I.get00(),1.59461986077384) && cmp(I.get11(),4.21878433864904) && dMassSetBox (m,0.27,3,4,5); I = (DMatrix3)m.getI(); if (cmp(m.getMass(),16.2) && m.getC().get0()==0 && m.getC().get1()==0 && m.getC().get2()==0 && cmp(I.get00(),55.35) && cmp(I.get11(),45.9) && cmp(I.get22(),33.75) && I.get01()==0 && I.get02()==0 && I.get12()==0 && m2.setMass( m1.getMass() ); dMassTranslate (m2,1,2,-3); for (i=0; i<NUMP; i++) {