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 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 drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb) { if (!g) return; if (!pos) pos = dGeomGetPosition (g); if (!R) R = dGeomGetRotation (g); int type = dGeomGetClass (g); if (type == dBoxClass) { dVector3 sides; dGeomBoxGetLengths (g,sides); dsDrawBox (pos,R,sides); } else if (type == dSphereClass) { dsDrawSphere (pos,R,dGeomSphereGetRadius (g)); } else if (type == dCapsuleClass) { dReal radius,length; dGeomCapsuleGetParams (g,&radius,&length); dsDrawCapsule (pos,R,length,radius); } else if (type == dCylinderClass) { dReal radius,length; dGeomCylinderGetParams (g,&radius,&length); dsDrawCylinder (pos,R,length,radius); } else if (type == dGeomTransformClass) { dGeomID g2 = dGeomTransformGetGeom (g); const dReal *pos2 = dGeomGetPosition (g2); const dReal *R2 = dGeomGetRotation (g2); dVector3 actual_pos; dMatrix3 actual_R; dMultiply0_331 (actual_pos,R,pos2); actual_pos[0] += pos[0]; actual_pos[1] += pos[1]; actual_pos[2] += pos[2]; dMultiply0_333 (actual_R,R,R2); drawGeom (g2,actual_pos,actual_R,0); } if (show_aabb) { // draw the bounding box for this geom dReal aabb[6]; dGeomGetAABB (g,aabb); dVector3 bbpos; for (int i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]); dVector3 bbsides; for (int j=0; j<3; j++) bbsides[j] = aabb[j*2+1] - aabb[j*2]; dMatrix3 RI; dRSetIdentity (RI); dsSetColorAlpha (1,0,0,0.5); dsDrawBox (bbpos,RI,bbsides); } }
/******************************************************************************* Function to draw a geometry object. *******************************************************************************/ void GOdeObject::drawGeom( dGeomID g, const dReal *position, const dReal *orientation ) const { if( !g ) //If the geometry object is missing, end the function. return; if( !position ) //Position was not passed? position = dGeomGetPosition( g ); //Then, get the geometry position. if( !orientation ) //Orientation was not given? orientation = dGeomGetRotation( g ); //And get existing geometry orientation. int type = dGeomGetClass( g ); //Get the type of geometry. if( type == dBoxClass ) //Is it a box? { dReal sides[3]; dGeomBoxGetLengths( g, sides ); //Get length of sides. renderBox( sides, position, orientation ); //Render the actual box in environment. } if( type == dSphereClass ) //Is it a sphere? { dReal radius; radius = dGeomSphereGetRadius( g ); //Get the radius. renderSphere( radius, position, orientation ); //Render sphere in environment. } if( type == dCapsuleClass ) { dReal radius; dReal length; dGeomCapsuleGetParams( g, &radius, &length ); //Get both radius and length. renderCapsule( radius, length, position, orientation ); //Render capsule in environment. } if( type == dGeomTransformClass ) //Is it an embeded geom in a composite body. { dGeomID g2 = dGeomTransformGetGeom( g ); //Get the actual geometry inside the wrapper. const dReal *position2 = dGeomGetPosition( g2 ); //Get position and orientation of wrapped geometry. const dReal *orientation2 = dGeomGetRotation( g2 ); dVector3 actualPosition; //Real world coordinated position and orientation dMatrix3 actualOrientation; //of the wrapped geometry. dMultiply0_331( actualPosition, orientation, position2 ); //Get world coordinates of geometry position. actualPosition[0] += position[0]; actualPosition[1] += position[1]; actualPosition[2] += position[2]; dMultiply0_333( actualOrientation, orientation, orientation2 ); //Get world coordinates of geom orientation. drawGeom( g2, actualPosition, actualOrientation ); //Draw embeded geometry. } }
void dxGeom::computePosr() { // should only be recalced if we need to - ie offset from a body dIASSERT(offset_posr); dIASSERT(body); dMultiply0_331 (final_posr->pos,body->posr.R,offset_posr->pos); final_posr->pos[0] += body->posr.pos[0]; final_posr->pos[1] += body->posr.pos[1]; final_posr->pos[2] += body->posr.pos[2]; dMultiply0_333 (final_posr->R,body->posr.R,offset_posr->R); }
void getWorldOffsetPosr(const dxPosR& body_posr, const dxPosR& world_posr, dxPosR& offset_posr) { dMatrix3 inv_body; matrixInvert(body_posr.R, inv_body); dMultiply0_333(offset_posr.R, inv_body, world_posr.R); dVector3 world_offset; world_offset[0] = world_posr.pos[0] - body_posr.pos[0]; world_offset[1] = world_posr.pos[1] - body_posr.pos[1]; world_offset[2] = world_posr.pos[2] - body_posr.pos[2]; dMultiply0_331(offset_posr.pos, inv_body, world_offset); }
void getBodyPosr(const dxPosR& offset_posr, const dxPosR& final_posr, dxPosR& body_posr) { dMatrix3 inv_offset; matrixInvert(offset_posr.R, inv_offset); dMultiply0_333(body_posr.R, final_posr.R, inv_offset); dVector3 world_offset; dMultiply0_331(world_offset, body_posr.R, offset_posr.pos); body_posr.pos[0] = final_posr.pos[0] - world_offset[0]; body_posr.pos[1] = final_posr.pos[1] - world_offset[1]; body_posr.pos[2] = final_posr.pos[2] - world_offset[2]; }
void testSmallMatrixMultiply() { dMatrix3 A,B,C,A2; dVector3 a,a2,x; HEADER; dMakeRandomMatrix (A,3,3,1.0); dMakeRandomMatrix (B,3,3,1.0); dMakeRandomMatrix (C,3,3,1.0); dMakeRandomMatrix (x,3,1,1.0); // dMultiply0_331() dMultiply0_331 (a,B,x); dMultiply0 (a2,B,x,3,3,1); printf ("\t%s (1)\n",(dMaxDifference (a,a2,3,1) > tol) ? "FAILED" : "passed"); // dMultiply1_331() dMultiply1_331 (a,B,x); dMultiply1 (a2,B,x,3,3,1); printf ("\t%s (2)\n",(dMaxDifference (a,a2,3,1) > tol) ? "FAILED" : "passed"); // dMultiply0_133 dMultiply0_133 (a,x,B); dMultiply0 (a2,x,B,1,3,3); printf ("\t%s (3)\n",(dMaxDifference (a,a2,1,3) > tol) ? "FAILED" : "passed"); // dMultiply0_333() dMultiply0_333 (A,B,C); dMultiply0 (A2,B,C,3,3,3); printf ("\t%s (4)\n",(dMaxDifference (A,A2,3,3) > tol) ? "FAILED" : "passed"); // dMultiply1_333() dMultiply1_333 (A,B,C); dMultiply1 (A2,B,C,3,3,3); printf ("\t%s (5)\n",(dMaxDifference (A,A2,3,3) > tol) ? "FAILED" : "passed"); // dMultiply2_333() dMultiply2_333 (A,B,C); dMultiply2 (A2,B,C,3,3,3); printf ("\t%s (6)\n",(dMaxDifference (A,A2,3,3) > tol) ? "FAILED" : "passed"); }
int dMassCheck (const dMass *m) { int i; if (m->mass <= 0) { // dDEBUGMSG ("mass must be > 0"); return 0; } if (!dIsPositiveDefinite (m->I,3,NULL)) { dDEBUGMSG ("inertia must be positive definite"); return 0; } // verify that the center of mass position is consistent with the mass // and inertia matrix. this is done by checking that the inertia around // the center of mass is also positive definite. from the comment in // dMassTranslate(), if the body is translated so that its center of mass // is at the point of reference, then the new inertia is: // I + mass*crossmat(c)^2 // note that requiring this to be positive definite is exactly equivalent // to requiring that the spatial inertia matrix // [ mass*eye(3,3) M*crossmat(c)^T ] // [ M*crossmat(c) I ] // is positive definite, given that I is PD and mass>0. see the theorem // about partitioned PD matrices for proof. dMatrix3 I2,chat; dSetZero (chat,12); dSetCrossMatrixPlus (chat,m->c,4); dMultiply0_333 (I2,chat,chat); for (i=0; i<3; i++) I2[i] = m->I[i] + m->mass*I2[i]; for (i=4; i<7; i++) I2[i] = m->I[i] + m->mass*I2[i]; for (i=8; i<11; i++) I2[i] = m->I[i] + m->mass*I2[i]; if (!dIsPositiveDefinite (I2,3,NULL)) { dDEBUGMSG ("center of mass inconsistent with mass parameters"); return 0; } return 1; }
void start() { world = dWorldCreate(); dWorldSetGravity (world,0,0,-9.8); contact_group = dJointGroupCreate(0); space = dSimpleSpaceCreate (0); // first, the ground plane // it has to coincide with the plane we have in drawstuff ground = dCreatePlane(space, 0, 0, 1, 0); // now a ball dMass m; dMassSetSphere(&m, 0.1, ball_radius); ball1_geom = dCreateSphere(space, ball_radius); ball1_body = dBodyCreate(world); dGeomSetBody(ball1_geom, ball1_body); dBodySetMass(ball1_body, &m); ball2_geom = dCreateSphere(space, ball_radius); ball2_body = dBodyCreate(world); dGeomSetBody(ball2_geom, ball2_body); dBodySetMass(ball2_body, &m); // tracks made out of boxes dGeomID trk; dMatrix3 r1, r2, r3; dVector3 ro = {0, -(0.5*track_gauge + 0.5*track_width), track_elevation}; dMatrix3 s1, s2, s3; dVector3 so = {0, 0.5*track_gauge + 0.5*track_width, track_elevation}; dRFromAxisAndAngle(r1, 1, 0, 0, track_angle); dRFromAxisAndAngle(r2, 0, 1, 0, -track_incl); dMultiply0_333(r3, r2, r1); dRFromAxisAndAngle(s1, 1, 0, 0, -track_angle); dRFromAxisAndAngle(s2, 0, 1, 0, -track_incl); dMultiply0_333(s3, s2, s1); trk = dCreateBox(space, track_len, track_width, track_height); dGeomSetPosition(trk, ro[0], ro[1] + balls_sep, ro[2]); dGeomSetRotation(trk, r3); trk = dCreateBox(space, track_len, track_width, track_height); dGeomSetPosition(trk, so[0], so[1] + balls_sep, so[2]); dGeomSetRotation(trk, s3); // tracks made out of trimesh for (unsigned i=0; i<n_box_verts; ++i) { dVector3 p; dMultiply0_331(p, s3, box_verts[i]); dAddVectors3(p, p, so); dCopyVector3(track_verts[i], p); } // trimesh tracks 2, transform all vertices by s3 for (unsigned i=0; i<n_box_verts; ++i) { dVector3 p; dMultiply0_331(p, r3, box_verts[i]); dAddVectors3(p, p, ro); dCopyVector3(track_verts[n_box_verts + i], p); } // copy face indices for (unsigned i=0; i<n_box_faces; ++i) for (unsigned j=0; j<3; ++j) // each face index track_faces[3*i+j] = box_faces[3*i+j]; for (unsigned i=0; i<n_box_faces; ++i) for (unsigned j=0; j<3; ++j) // each face index track_faces[3*(i + n_box_faces)+j] = box_faces[3*i+j] + n_box_verts; mesh_data = dGeomTriMeshDataCreate(); dGeomTriMeshDataBuildSimple(mesh_data, track_verts[0], n_track_verts, track_faces, 3*n_track_faces); mesh_geom = dCreateTriMesh(space, mesh_data, 0, 0, 0); resetSim(); // initial camera position static float xyz[3] = {-5.9414,-0.4804,2.9800}; static float hpr[3] = {32.5000,-10.0000,0.0000}; dsSetViewpoint (xyz,hpr); dsSetSphereQuality(3); }
btVector3 btRigidBody::computeGyroscopicImpulseImplicit_Cooper(btScalar step) const { #if 0 dReal h = callContext->m_stepperCallContext->m_stepSize; // Step size dVector3 L; // Compute angular momentum dMultiply0_331(L, I, b->avel); #endif btVector3 inertiaLocal = getLocalInertia(); btMatrix3x3 inertiaTensorWorld = getWorldTransform().getBasis().scaled(inertiaLocal) * getWorldTransform().getBasis().transpose(); btVector3 L = inertiaTensorWorld*getAngularVelocity(); btMatrix3x3 Itild(0, 0, 0, 0, 0, 0, 0, 0, 0); #if 0 for (int ii = 0; ii<12; ++ii) { Itild[ii] = Itild[ii] * h + I[ii]; } #endif btSetCrossMatrixMinus(Itild, L*step); Itild += inertiaTensorWorld; #if 0 // Compute a new effective 'inertia tensor' // for the implicit step: the cross-product // matrix of the angular momentum plus the // old tensor scaled by the timestep. // Itild may not be symmetric pos-definite, // but we can still use it to compute implicit // gyroscopic torques. dMatrix3 Itild = { 0 }; dSetCrossMatrixMinus(Itild, L, 4); for (int ii = 0; ii<12; ++ii) { Itild[ii] = Itild[ii] * h + I[ii]; } #endif L *= step; //Itild may not be symmetric pos-definite btMatrix3x3 itInv = Itild.inverse(); Itild = inertiaTensorWorld * itInv; btMatrix3x3 ident(1,0,0,0,1,0,0,0,1); Itild -= ident; #if 0 // Scale momentum by inverse time to get // a sort of "torque" dScaleVector3(L, dRecip(h)); // Invert the pseudo-tensor dMatrix3 itInv; // This is a closed-form inversion. // It's probably not numerically stable // when dealing with small masses with // a large asymmetry. // An LU decomposition might be better. if (dInvertMatrix3(itInv, Itild) != 0) { // "Divide" the original tensor // by the pseudo-tensor (on the right) dMultiply0_333(Itild, I, itInv); // Subtract an identity matrix Itild[0] -= 1; Itild[5] -= 1; Itild[10] -= 1; // This new inertia matrix rotates the // momentum to get a new set of torques // that will work correctly when applied // to the old inertia matrix as explicit // torques with a semi-implicit update // step. dVector3 tau0; dMultiply0_331(tau0, Itild, L); // Add the gyro torques to the torque // accumulator for (int ii = 0; ii<3; ++ii) { b->tacc[ii] += tau0[ii]; } #endif btVector3 tau0 = Itild * L; // printf("tau0 = %f,%f,%f\n",tau0.x(),tau0.y(),tau0.z()); return tau0; } btVector3 btRigidBody::computeGyroscopicImpulseImplicit_Ewert(btScalar step) const { // use full newton-euler equations. common practice to drop the wxIw term. want it for better tumbling behavior. // calculate using implicit euler step so it's stable. const btVector3 inertiaLocal = getLocalInertia(); const btVector3 w0 = getAngularVelocity(); btMatrix3x3 I; I = m_worldTransform.getBasis().scaled(inertiaLocal) * m_worldTransform.getBasis().transpose(); // use newtons method to find implicit solution for new angular velocity (w') // f(w') = -(T*step + Iw) + Iw' + w' + w'xIw'*step = 0 // df/dw' = I + 1xIw'*step + w'xI*step btVector3 w1 = w0; // one step of newton's method { const btVector3 fw = evalEulerEqn(w1, w0, btVector3(0, 0, 0), step, I); const btMatrix3x3 dfw = evalEulerEqnDeriv(w1, w0, step, I); const btMatrix3x3 dfw_inv = dfw.inverse(); btVector3 dw; dw = dfw_inv*fw; w1 -= dw; } btVector3 gf = (w1 - w0); return gf; } void btRigidBody::integrateVelocities(btScalar step) { if (isStaticOrKinematicObject()) return; m_linearVelocity += m_totalForce * (m_inverseMass * step); m_angularVelocity += m_invInertiaTensorWorld * m_totalTorque * step; #define MAX_ANGVEL SIMD_HALF_PI /// clamp angular velocity. collision calculations will fail on higher angular velocities btScalar angvel = m_angularVelocity.length(); if (angvel*step > MAX_ANGVEL) { m_angularVelocity *= (MAX_ANGVEL/step) /angvel; } } btQuaternion btRigidBody::getOrientation() const { btQuaternion orn; m_worldTransform.getBasis().getRotation(orn); return orn; }