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;
}
