public static void dMassSetZero (DMass m) { m.setZero(); }
public static void dMassSetZero (DMass m) { m.setZero(); }
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); }
m.setZero (); m.setZero (); println ("\tpassed (3)"); else println ("\tFAILED (3)"); m.setZero (); m.setSphere (1.4, 0.86); I = (DMatrix3)m.getI(); println ("\tpassed (4)"); else println ("\tFAILED (4)"); m.setZero (); m.setCapsule (1.3,1,0.76,1.53); I = (DMatrix3)m.getI(); m.setZero (); m.setBox (0.27,3,4,5); I = (DMatrix3)m.getI();
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); }