void ODERigidObject::Create(dWorldID worldID,dSpaceID space,bool useBoundaryLayer) { Clear(); spaceID = space; bodyID = dBodyCreate(worldID); dMass mass; mass.mass = obj.mass; //NOTE: in ODE, COM must be zero vector! -- we take care of this by shifting geometry //CopyVector3(mass.c,obj.com); mass.c[0] = mass.c[1] = mass.c[2] = 0; mass.c[3] = 1.0; CopyMatrix(mass.I,obj.inertia); int res=dMassCheck(&mass); if(res != 1) { fprintf(stderr,"Uh... mass is not considered to be valid by ODE?\n"); std::cerr<<"Inertia: "<<obj.inertia<<std::endl; } dBodySetMass(bodyID,&mass); geometry = new ODEGeometry; geometry->Create(&obj.geometry,spaceID,-obj.com,useBoundaryLayer); dGeomSetBody(geometry->geom(),bodyID); dGeomSetData(geometry->geom(),(void*)-1); geometry->SetPadding(defaultPadding); geometry->surf().kRestitution = obj.kRestitution; geometry->surf().kFriction = obj.kFriction; geometry->surf().kStiffness = obj.kStiffness; geometry->surf().kDamping = obj.kDamping; SetTransform(obj.T); }
void dMassRotate (dMass *m, const dMatrix3 R) { // if the body is rotated by `R' relative to its point of reference, // the new inertia about the point of reference is: // // R * I * R' // // where I is the old inertia. dMatrix3 t1; dReal t2[3]; dAASSERT (m); // rotate inertia matrix dMultiply2_333 (t1,m->I,R); dMultiply0_333 (m->I,R,t1); // ensure perfect symmetry m->_I(1,0) = m->_I(0,1); m->_I(2,0) = m->_I(0,2); m->_I(2,1) = m->_I(1,2); // rotate center of mass dMultiply0_331 (t2,R,m->c); m->c[0] = t2[0]; m->c[1] = t2[1]; m->c[2] = t2[2]; # ifndef dNODEBUG dMassCheck (m); # endif }
void dMassAdjust (dMass *m, dReal newmass) { dAASSERT (m); dReal scale = newmass / m->mass; m->mass = newmass; for (int i=0; i<3; i++) for (int j=0; j<3; j++) m->_I(i,j) *= scale; # ifndef dNODEBUG dMassCheck (m); # endif }
void dMassSetBoxTotal (dMass *m, dReal total_mass, dReal lx, dReal ly, dReal lz) { dAASSERT (m); dMassSetZero (m); m->mass = total_mass; m->_I(0,0) = total_mass/REAL(12.0) * (ly*ly + lz*lz); m->_I(1,1) = total_mass/REAL(12.0) * (lx*lx + lz*lz); m->_I(2,2) = total_mass/REAL(12.0) * (lx*lx + ly*ly); # ifndef dNODEBUG dMassCheck (m); # endif }
void dMassSetSphereTotal (dMass *m, dReal total_mass, dReal radius) { dAASSERT (m); dMassSetZero (m); m->mass = total_mass; dReal II = REAL(0.4) * total_mass * radius*radius; m->_I(0,0) = II; m->_I(1,1) = II; m->_I(2,2) = II; # ifndef dNODEBUG dMassCheck (m); # endif }
void dMassSetCylinderTotal (dMass *m, dReal total_mass, int direction, dReal radius, dReal length) { dReal r2,I; dAASSERT (m); dUASSERT (direction >= 1 && direction <= 3,"bad direction number"); dMassSetZero (m); r2 = radius*radius; m->mass = total_mass; I = total_mass*(REAL(0.25)*r2 + (REAL(1.0)/REAL(12.0))*length*length); m->_I(0,0) = I; m->_I(1,1) = I; m->_I(2,2) = I; m->_I(direction-1,direction-1) = total_mass*REAL(0.5)*r2; # ifndef dNODEBUG dMassCheck (m); # endif }
void dMassTranslate (dMass *m, dReal x, dReal y, dReal z) { // if the body is translated by `a' relative to its point of reference, // the new inertia about the point of reference is: // // I + mass*(crossmat(c)^2 - crossmat(c+a)^2) // // where c is the existing center of mass and I is the old inertia. int i,j; dMatrix3 ahat,chat,t1,t2; dReal a[3]; dAASSERT (m); // adjust inertia matrix dSetZero (chat,12); dSetCrossMatrixPlus (chat,m->c,4); a[0] = x + m->c[0]; a[1] = y + m->c[1]; a[2] = z + m->c[2]; dSetZero (ahat,12); dSetCrossMatrixPlus (ahat,a,4); dMultiply0_333 (t1,ahat,ahat); dMultiply0_333 (t2,chat,chat); for (i=0; i<3; i++) for (j=0; j<3; j++) m->_I(i,j) += m->mass * (t2[i*4+j]-t1[i*4+j]); // ensure perfect symmetry m->_I(1,0) = m->_I(0,1); m->_I(2,0) = m->_I(0,2); m->_I(2,1) = m->_I(1,2); // adjust center of mass m->c[0] += x; m->c[1] += y; m->c[2] += z; # ifndef dNODEBUG dMassCheck (m); # endif }
void dMassSetParameters (dMass *m, dReal themass, dReal cgx, dReal cgy, dReal cgz, dReal I11, dReal I22, dReal I33, dReal I12, dReal I13, dReal I23) { dAASSERT (m); dMassSetZero (m); m->mass = themass; m->c[0] = cgx; m->c[1] = cgy; m->c[2] = cgz; m->_I(0,0) = I11; m->_I(1,1) = I22; m->_I(2,2) = I33; m->_I(0,1) = I12; m->_I(0,2) = I13; m->_I(1,2) = I23; m->_I(1,0) = I12; m->_I(2,0) = I13; m->_I(2,1) = I23; dMassCheck (m); }
void dMassSetCapsule (dMass *m, dReal density, int direction, dReal radius, dReal length) { dReal M1,M2,Ia,Ib; dAASSERT (m); dUASSERT (direction >= 1 && direction <= 3,"bad direction number"); dMassSetZero (m); M1 = (dReal) (M_PI*radius*radius*length*density); // cylinder mass M2 = (dReal) ((REAL(4.0)/REAL(3.0))*M_PI*radius*radius*radius*density); // total cap mass m->mass = M1+M2; Ia = M1*(REAL(0.25)*radius*radius + (REAL(1.0)/REAL(12.0))*length*length) + M2*(REAL(0.4)*radius*radius + REAL(0.375)*radius*length + REAL(0.25)*length*length); Ib = (M1*REAL(0.5) + M2*REAL(0.4))*radius*radius; m->_I(0,0) = Ia; m->_I(1,1) = Ia; m->_I(2,2) = Ia; m->_I(direction-1,direction-1) = Ib; # ifndef dNODEBUG dMassCheck (m); # endif }
/* * dMassSetTrimesh, implementation by Gero Mueller. * Based on Brian Mirtich, "Fast and Accurate Computation of * Polyhedral Mass Properties," journal of graphics tools, volume 1, * number 2, 1996. */ void dMassSetTrimesh( dMass *m, dReal density, dGeomID g ) { dAASSERT (m); dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); dMassSetZero (m); #if dTRIMESH_ENABLED dxTriMesh *TriMesh = (dxTriMesh *)g; unsigned int triangles = FetchTriangleCount( TriMesh ); dReal nx, ny, nz; unsigned int i, A, B, C; // face integrals dReal Fa, Fb, Fc, Faa, Fbb, Fcc, Faaa, Fbbb, Fccc, Faab, Fbbc, Fcca; // projection integrals dReal P1, Pa, Pb, Paa, Pab, Pbb, Paaa, Paab, Pabb, Pbbb; dReal T0 = 0; dReal T1[3] = {0., 0., 0.}; dReal T2[3] = {0., 0., 0.}; dReal TP[3] = {0., 0., 0.}; for( i = 0; i < triangles; i++ ) { dVector3 v[3]; FetchTransformedTriangle( TriMesh, i, v); dVector3 n, a, b; dSubtractVectors3( a, v[1], v[0] ); dSubtractVectors3( b, v[2], v[0] ); dCalcVectorCross3( n, b, a ); nx = fabs(n[0]); ny = fabs(n[1]); nz = fabs(n[2]); if( nx > ny && nx > nz ) C = 0; else C = (ny > nz) ? 1 : 2; // Even though all triangles might be initially valid, // a triangle may degenerate into a segment after applying // space transformation. if (n[C] != REAL(0.0)) { A = (C + 1) % 3; B = (A + 1) % 3; // calculate face integrals { dReal w; dReal k1, k2, k3, k4; //compProjectionIntegrals(f); { dReal a0=0, a1=0, da; dReal b0=0, b1=0, db; dReal a0_2, a0_3, a0_4, b0_2, b0_3, b0_4; dReal a1_2, a1_3, b1_2, b1_3; dReal C1, Ca, Caa, Caaa, Cb, Cbb, Cbbb; dReal Cab, Kab, Caab, Kaab, Cabb, Kabb; P1 = Pa = Pb = Paa = Pab = Pbb = Paaa = Paab = Pabb = Pbbb = 0.0; for( int j = 0; j < 3; j++) { switch(j) { case 0: a0 = v[0][A]; b0 = v[0][B]; a1 = v[1][A]; b1 = v[1][B]; break; case 1: a0 = v[1][A]; b0 = v[1][B]; a1 = v[2][A]; b1 = v[2][B]; break; case 2: a0 = v[2][A]; b0 = v[2][B]; a1 = v[0][A]; b1 = v[0][B]; break; } da = a1 - a0; db = b1 - b0; a0_2 = a0 * a0; a0_3 = a0_2 * a0; a0_4 = a0_3 * a0; b0_2 = b0 * b0; b0_3 = b0_2 * b0; b0_4 = b0_3 * b0; a1_2 = a1 * a1; a1_3 = a1_2 * a1; b1_2 = b1 * b1; b1_3 = b1_2 * b1; C1 = a1 + a0; Ca = a1*C1 + a0_2; Caa = a1*Ca + a0_3; Caaa = a1*Caa + a0_4; Cb = b1*(b1 + b0) + b0_2; Cbb = b1*Cb + b0_3; Cbbb = b1*Cbb + b0_4; Cab = 3*a1_2 + 2*a1*a0 + a0_2; Kab = a1_2 + 2*a1*a0 + 3*a0_2; Caab = a0*Cab + 4*a1_3; Kaab = a1*Kab + 4*a0_3; Cabb = 4*b1_3 + 3*b1_2*b0 + 2*b1*b0_2 + b0_3; Kabb = b1_3 + 2*b1_2*b0 + 3*b1*b0_2 + 4*b0_3; P1 += db*C1; Pa += db*Ca; Paa += db*Caa; Paaa += db*Caaa; Pb += da*Cb; Pbb += da*Cbb; Pbbb += da*Cbbb; Pab += db*(b1*Cab + b0*Kab); Paab += db*(b1*Caab + b0*Kaab); Pabb += da*(a1*Cabb + a0*Kabb); } P1 /= 2.0; Pa /= 6.0; Paa /= 12.0; Paaa /= 20.0; Pb /= -6.0; Pbb /= -12.0; Pbbb /= -20.0; Pab /= 24.0; Paab /= 60.0; Pabb /= -60.0; } w = - dCalcVectorDot3(n, v[0]); k1 = 1 / n[C]; k2 = k1 * k1; k3 = k2 * k1; k4 = k3 * k1; Fa = k1 * Pa; Fb = k1 * Pb; Fc = -k2 * (n[A]*Pa + n[B]*Pb + w*P1); Faa = k1 * Paa; Fbb = k1 * Pbb; Fcc = k3 * (SQR(n[A])*Paa + 2*n[A]*n[B]*Pab + SQR(n[B])*Pbb + w*(2*(n[A]*Pa + n[B]*Pb) + w*P1)); Faaa = k1 * Paaa; Fbbb = k1 * Pbbb; Fccc = -k4 * (CUBE(n[A])*Paaa + 3*SQR(n[A])*n[B]*Paab + 3*n[A]*SQR(n[B])*Pabb + CUBE(n[B])*Pbbb + 3*w*(SQR(n[A])*Paa + 2*n[A]*n[B]*Pab + SQR(n[B])*Pbb) + w*w*(3*(n[A]*Pa + n[B]*Pb) + w*P1)); Faab = k1 * Paab; Fbbc = -k2 * (n[A]*Pabb + n[B]*Pbbb + w*Pbb); Fcca = k3 * (SQR(n[A])*Paaa + 2*n[A]*n[B]*Paab + SQR(n[B])*Pabb + w*(2*(n[A]*Paa + n[B]*Pab) + w*Pa)); } T0 += n[0] * ((A == 0) ? Fa : ((B == 0) ? Fb : Fc)); T1[A] += n[A] * Faa; T1[B] += n[B] * Fbb; T1[C] += n[C] * Fcc; T2[A] += n[A] * Faaa; T2[B] += n[B] * Fbbb; T2[C] += n[C] * Fccc; TP[A] += n[A] * Faab; TP[B] += n[B] * Fbbc; TP[C] += n[C] * Fcca; } } T1[0] /= 2; T1[1] /= 2; T1[2] /= 2; T2[0] /= 3; T2[1] /= 3; T2[2] /= 3; TP[0] /= 2; TP[1] /= 2; TP[2] /= 2; m->mass = density * T0; m->_I(0,0) = density * (T2[1] + T2[2]); m->_I(1,1) = density * (T2[2] + T2[0]); m->_I(2,2) = density * (T2[0] + T2[1]); m->_I(0,1) = - density * TP[0]; m->_I(1,0) = - density * TP[0]; m->_I(2,1) = - density * TP[1]; m->_I(1,2) = - density * TP[1]; m->_I(2,0) = - density * TP[2]; m->_I(0,2) = - density * TP[2]; // Added to address SF bug 1729095 dMassTranslate( m, T1[0] / T0, T1[1] / T0, T1[2] / T0 ); # ifndef dNODEBUG dMassCheck (m); # endif #endif // dTRIMESH_ENABLED }