public void dMassAdd (DMassC b) { //dAASSERT (b); double denom = dRecip (_mass + b.getMass()); //for (i=0; i<3; i++) a._c.v[i] = (a._c.v[i]*a._mass + b._c.v[i]*b._mass)*denom; _c.eqSum( _c, _mass, b.getC(), b.getMass() ).scale( denom ); _mass += b.getMass(); //for (i=0; i<12; i++) a._I.v[i] += b._I.v[i]; _I.add(b.getI()); }
public void dMassAdd (DMassC b) { //dAASSERT (b); double denom = dRecip (_mass + b.getMass()); //for (i=0; i<3; i++) a._c.v[i] = (a._c.v[i]*a._mass + b._c.v[i]*b._mass)*denom; _c.eqSum( _c, _mass, b.getC(), b.getMass() ).scale( denom ); _mass += b.getMass(); //for (i=0; i<12; i++) a._I.v[i] += b._I.v[i]; _I.add(b.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); }
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); }
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); }
I2.add(_I);
I2.add(_I);
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)) );
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)) );
Itild.add(I);
Itild.add(I);
Itild.add(I);
Itild.add(I);
m.setI( m.getI().clone().add(anchor_m.getI()) ); test_body.setMass (m);
m.setI( m.getI().clone().add(anchor_m.getI()) ); dBodySetMass (test_body,m);