Module: rrigid.cpp

/*
 * RRigidBody class definition
 * 03-02-01: Created!
 * NOTES:
 * - To use any RRigidBody, do the following:
 *   1. Set a time step using RRigidBody::SetTimeStep()
 * - To use a RRigidBody, take the following steps:
 *   1. Create it
 *   2. Set its mass
 *   3. Set its inertia values
 * - After that, use it as follows:
 *   1. Call IntegrateInit() before adding forces and such
 *   2. Call Add...Force/Torque() to add forces and torques
 *   3. IntegrateEuler() to integrate using Euler
 * FUTURE:
 * - Automatically call IntegrateInit() after IntegrateEuler()?
 * (c) Dolphinity/RvG
 */

#include <racer/racer.h>
#include <qlib/debug.h>
#pragma hdrstop
DEBUG_ENABLE

// Gyroscopic calculations?
#define USE_GYROSCOPIC_PRECESSION

rfloat RRigidBody::h=0.0f;
rfloat RRigidBody::h2=0.0f;
rfloat RRigidBody::h4=0.0f;
rfloat RRigidBody::h6=0.0f;

/*******
* CTOR *
*******/
RRigidBody::RRigidBody()
  //: qRotPos(1,0,0,0)             // Normal start
  //: qRotPos(1,0,-1,0)          // 90 degrees clockwise around yaw
  : qRotPos(0,0,-1,0)          // 180 degrees yaw
{
  mass=0;
  linPos.SetToZero();
  linVel.SetToZero();
  rotVel.SetToZero();
  // Make initial 3x3 matrix
  mRotPos.FromQuaternionL2(&qRotPos,qRotPos.LengthSquared());
  // q/mRotPos
  inertiaTensor.SetIdentity();
  IntegrateInit();
}

/*************
* ATTRIBUTES *
*************/
void RRigidBody::SetInertia(rfloat x,rfloat y,rfloat z)
// Set evenly massed inertia tensor
// The x/y/z are the diagonal values in a 3x3 inertia tensor
// For most cars, it is reasonably ok to have balanced masses across
// all 3 axes.
{
  inertiaTensor.SetIdentity();
  inertiaTensor.SetRC(0,0,x);
  inertiaTensor.SetRC(1,1,y);
  inertiaTensor.SetRC(2,2,z);
}

void RRigidBody::SetMass(rfloat m)
{
  mass=m;
  // Precalculate 1/mass for optimization reasons
  if(mass!=0.0f)oneOverMass=1.0f/mass;
  else          oneOverMass=0.0f;
}

void RRigidBody::SetTimeStep(rfloat _h)
// Set time step for all rigid bodies
{
  h=_h;
  // Precalculate other time steps for fast RK integration
  h2=h/2.0f;
  h4=h/4.0f;
  h6=h/6.0f;
}

/************************
* ADDING FORCES/TORQUES *
************************/
void RRigidBody::AddWorldForceAtBodyPos(DVector3 *force,DVector3 *pos)
// The 'force' is applied at location 'pos'
// 'force' is in world coordinates, 'pos' is in body coordinates
{
//qdbg("RRB:AddW@B\n");
//force->DbgPrint("  force");
//pos->DbgPrint("  pos");

  DVector3 forceBody;
  // Calculate force in body coordinates
  mRotPos.TransposeTransform(force,&forceBody);
//mRotPos.DbgPrint("mRotPos");
//forceBody.DbgPrint("  forceBody");
//force->DbgPrint("  force");
  // Add the linear force in world coordinates
  totalForce.Add(force);
  
  DVector3 torqueBody;
  // Calculate torque in body coordinates
  torqueBody.Cross(pos,&forceBody);
  totalTorque.Add(&torqueBody);
//totalForce.DbgPrint("  totalForce");
//totalTorque.DbgPrint("  totalTorque");
}

void RRigidBody::AddBodyTorque(DVector3 *torque)
// Add 'torque', given in body coordinates
{
#ifdef OBS
qdbg("************ AddBodyTorque\n");
torque->DbgPrint("torque");
#endif
  totalTorque.Add(torque);
#ifdef OBS
totalForce.DbgPrint("  totalForce");
totalTorque.DbgPrint("  totalTorque");
#endif
}

void RRigidBody::AddWorldTorque(DVector3 *torque)
// Add 'torque', given in world coordinates
{
#ifdef OBS
qdbg("************ AddWorldTorque\n");
torque->DbgPrint("torque");
#endif
  DVector3 torqueBody;
  // Convert world to body coordinates
  mRotPos.TransposeTransform(torque,&torqueBody);
  totalTorque.Add(torque);
}

#ifdef OBS
// Direct torques
// Forces with a point of application
void RRigidBody::AddWorldWorldForce(DVector3 *force,DVector3 *pos);
void RRigidBody::AddWorldBodyForce(DVector3 *force,DVector3 *pos);
void RRigidBody::AddBodyBodyForce(DVector3 *force,DVector3 *pos);

// Coordinate system conversions
void RRigidBody::CalcWorldPos(DVector3 *bodyPos,DVector3 *worldPos);
void RRigidBody::CalcBodyPos(DVector3 *worldPos,DVector3 *bodyPos);
void RRigidBody::CalcBodyWorldVel(DVector3 *bodyPos,DVector3 *worldVel);
void RRigidBody::CalcWorldWorldVel(DVector3 *worldPos,DVector3 *worldVel);
void RRigidBody::CalcWorldPosVel(DVector3 *bodyPos,DVector3 *worldPos,
  DVector3 *worldVel);

// Debugging helpers
rfloat RRigidBody::GetEnergy();
void   RRigidBody::GetMomentum(DVector3 *linear,DVector3 *rotational);
#endif

/**************
* INTEGRATION *
**************/
void RRigidBody::IntegrateInit()
// Call this function before adding forces/torques
// This function resets the current force/torque
{
  totalForce.SetToZero();
  totalTorque.SetToZero();
  // Remember start values
  qRotPos0=qRotPos;
}
void RRigidBody::IntegrateEuler()
// Integrate current force/torque over time 'h' ('h' given earlier)
{
#ifdef OBS
qdbg("RRB:IntegrateEuler; BEFORE\n");
linPos.DbgPrint("linPos");
linVel.DbgPrint("linVel");
rotVel.DbgPrint("rotVel");
//inertiaTensor.DbgPrint("inertiaTensor");
#endif

  // Linear movement
  linPos.x+=h*linVel.x;
  linPos.y+=h*linVel.y;
  linPos.z+=h*linVel.z;

  // Linear acceleration
#ifdef OBS
totalForce.DbgPrint("totalForce");
qdbg("oneOverMass=%f, h=%f\n",oneOverMass,h);
#endif
  linVel.x+=totalForce.x*oneOverMass*h;
  linVel.y+=totalForce.y*oneOverMass*h;
  linVel.z+=totalForce.z*oneOverMass*h;

  // Rotation
#ifdef OBS
qdbg("qRotPos0=(%.2f,%.2f,%.2f,%.2f)\n",
qRotPos0.w,qRotPos0.x,qRotPos0.y,qRotPos0.z);
#endif
  qRotPos.w-=h2*(qRotPos0.x*rotVel.x+qRotPos0.y*rotVel.y+qRotPos0.z*rotVel.z);
  qRotPos.x+=h2*(qRotPos0.w*rotVel.x-qRotPos0.z*rotVel.y+qRotPos0.y*rotVel.z);
  qRotPos.y+=h2*(qRotPos0.z*rotVel.x+qRotPos0.w*rotVel.y-qRotPos0.x*rotVel.z);
  qRotPos.z+=h2*(qRotPos0.x*rotVel.y-qRotPos0.y*rotVel.x+qRotPos0.w*rotVel.z);

  // Check if quaternion needs normalizing
  rfloat l2=qRotPos.x*qRotPos.x+qRotPos.y*qRotPos.y+
            qRotPos.z*qRotPos.z+qRotPos.w*qRotPos.w;
  if(l2<0.9999f||l2>1.0001f)
  {
    // Push quat back to 1.0 (avoid a sqrt())
//qdbg("RRB:IntegrateEuler; renormalize quat\n");
    float n=(l2+1.0f)/(2.0f*l2);
    qRotPos.x*=n;
    qRotPos.y*=n;
    qRotPos.z*=n;
    qRotPos.w*=n;
    l2*=n*n;
  }
  // Make new 3x3 matrix from quaternion
  mRotPos.FromQuaternionL2(&qRotPos,l2);

  // Rotational acceleration (with gyroscopic precession)
  rfloat IyyMinusIzzOverIxx,IzzMinusIxxOverIyy,IxxMinusIyyOverIzz;
  rfloat oneOverIxx,oneOverIyy,oneOverIzz;
  IyyMinusIzzOverIxx=(inertiaTensor.GetRC(1,1)-inertiaTensor.GetRC(2,2))/
                     inertiaTensor.GetRC(0,0);
  IzzMinusIxxOverIyy=(inertiaTensor.GetRC(2,2)-inertiaTensor.GetRC(0,0))/
                     inertiaTensor.GetRC(1,1);
  IxxMinusIyyOverIzz=(inertiaTensor.GetRC(0,0)-inertiaTensor.GetRC(1,1))/
                     inertiaTensor.GetRC(2,2);
  oneOverIxx=1.0f/inertiaTensor.GetRC(0,0);
  oneOverIyy=1.0f/inertiaTensor.GetRC(1,1);
  oneOverIzz=1.0f/inertiaTensor.GetRC(2,2);
#ifndef USE_GYROSCOPIC_PRECESSION
  IyyMinusIzzOverIxx=0;
  IzzMinusIxxOverIyy=0;
  IxxMinusIyyOverIzz=0;
#endif
  rotVel1.x=rotVel.x+(rotVel.y*rotVel.z*IyyMinusIzzOverIxx+
            totalTorque.x*oneOverIxx)*h;
  rotVel1.y=rotVel.y+(rotVel.z*rotVel.x*IzzMinusIxxOverIyy+
            totalTorque.y*oneOverIyy)*h;
  rotVel1.z=rotVel.z+(rotVel.x*rotVel.y*IxxMinusIyyOverIzz+
            totalTorque.z*oneOverIzz)*h;
  rotVel=rotVel1;

#ifdef OBS
qdbg("AFTER\n");
linPos.DbgPrint("linPos");
linVel.DbgPrint("linVel");
rotVel.DbgPrint("rotVel");
#endif
}

/********************************
* Coordinate system conversions *
********************************/
void RRigidBody::ConvertBodyToWorldPos(DVector3 *bodyPos,DVector3 *worldPos)
// Takes 'bodyPos', converts it to world coordinates, and puts
// the result in 'worldPos'
{
#ifdef OBS
qdbg("RRB: B2WPos\n");
bodyPos->DbgPrint("  bodyPos");
//mRotPos.DbgPrint("mRotPos");
#endif
  mRotPos.Transform(bodyPos,worldPos);
//worldPos->DbgPrint("  worldPos");
  worldPos->Add(&linPos);
//worldPos->DbgPrint("  worldPos+linPos");
}
void RRigidBody::ConvertBodyToWorldOrientation(DVector3 *bodyOr,
  DVector3 *worldOr)
// Takes 'bodyOr', converts it to world coordinates, and puts
// the result in 'worldOr'. Only does orientation, not position.
{
  mRotPos.Transform(bodyOr,worldOr);
}

void RRigidBody::ConvertWorldToBodyPos(DVector3 *worldPos,DVector3 *bodyPos)
// Takes 'worldPos', converts it to body coordinates, and puts
// the result in 'bodyPos'
{
  bodyPos->Subtract(&linPos);
  mRotPos.TransposeTransform(worldPos,bodyPos);
}
void RRigidBody::ConvertWorldToBodyOrientation(DVector3 *worldOr,
void RRigidBody::ConvertWorldToBodyOrientation(DVector3 *worldOr,  DVector3 *bodyOr)
// Takes 'worldOr', converts it to body coordinates, and puts
// the result in 'bodyOr'. Only does orientation, not position.
{
  mRotPos.TransposeTransform(worldOr,bodyOr);
}

/*************
* Velocities *
*************/
void RRigidBody::CalcWorldVelForBodyPos(const DVector3 *posBC,
void RRigidBody::CalcWorldVelForBodyPos(const DVector3 *posBC,  DVector3 *velWC)
// Calculate velocity in world coordinates for body position 'posBC'
// Stores result in 'velWC'
{
  DVector3 velBC;
  velBC.Cross(&rotVel,posBC);
  mRotPos.Transform(&velBC,velWC);
  // Add world linear velocity
  velWC->Add(&linVel);
}
void RRigidBody::CalcBodyVelForBodyPos(const DVector3 *posBC,
void RRigidBody::CalcBodyVelForBodyPos(const DVector3 *posBC,  DVector3 *velBC)
// Calculate velocity in body coordinates for body position 'posBC'
// Stores result in 'velBC'
{
  DVector3 linVelBC;
  // Calculate velocity because of rotation in body coords
  velBC->Cross(&rotVel,posBC);
  // Add body linear velocity
  mRotPos.TransposeTransform(&linVel,&linVelBC);
  velBC->Add(&linVelBC);
}