Module: rbody.cpp

/*
 * RBody - the sprung mass
 * 05-08-00: Created! (16:48:45)
 * 10-12-00: Model support
 * 16-12-00: Default values to get a usable default body.
 * 03-02-01: Completely revised state; RBody is now a RRigidBody.
 * NOTES:
 * - The body is the centerpoint of the physics; the wheels
 * more or less just hang on the body.
 * (C) Dolphinity/RvG
 */

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

#define DEF_LOCK  10
#define DEF_RADIUS  .20
#define DEF_XA      20

#undef  DBG_CLASS
#define DBG_CLASS "RBody"

/*********
* C/DTOR *
*********/
RBody::RBody(RCar *_car)
  : RRigidBody()
{
  DBG_C("ctor")
  
  // Init rigid body
  SetTimeStep(RMGR->time->span);
  
  car=_car;
#ifdef OBS
  mass=0;
#endif
  cockpitZ=cockpitLength=0;
  quad=0;
  size.SetToZero();
#ifdef OBS
  position.SetToZero();
  velocity.SetToZero();
  translation.SetToZero();
  rotation.SetToZero();
  rotationV.SetToZero();
  rotationA.SetToZero();
#endif
  force.SetToZero();
  forceGravityWC.SetToZero();
  forceGravityCC.SetToZero();
  forceDragCC.SetToZero();
  model=new RModel(car);
  bbox=new DBoundingBox();
  
  // Debugging; start of body
  // Signal start wishes to rigid body class
  rotVel.x=RMGR->infoDebug->GetFloat("car.av_pitch");
  rotVel.y=RMGR->infoDebug->GetFloat("car.av_yaw");
  rotVel.z=RMGR->infoDebug->GetFloat("car.av_roll");
  linVel.SetX(RMGR->infoDebug->GetFloat("car.vx"));
  linVel.SetY(RMGR->infoDebug->GetFloat("car.vy"));
  linVel.SetZ(RMGR->infoDebug->GetFloat("car.vz"));
#ifdef FUTURE
  rotation.SetX(RMGR->infoDebug->GetFloat("car.rotate_x")/RR_RAD_DEG_FACTOR);
  rotation.SetY(RMGR->infoDebug->GetFloat("car.rotate_y")/RR_RAD_DEG_FACTOR);
  rotation.SetZ(RMGR->infoDebug->GetFloat("car.rotate_z")/RR_RAD_DEG_FACTOR);
#endif
//qdbg("car.susp=%d\n",RMGR->infoDebug->GetInt("car.suspension",1));
//qdbg("devFlags=%d\n",RMGR->devFlags);
  if(!RMGR->infoDebug->GetInt("car.suspension",1))
    RMGR->devFlags|=RManager::NO_SUSPENSION;
  if(!RMGR->infoDebug->GetInt("car.gravity_body",1))
    RMGR->devFlags|=RManager::NO_GRAVITY_BODY;
//qdbg("devFlags=%d\n",RMGR->devFlags);
}
RBody::~RBody()
{
  Destroy();
}

void RBody::Destroy()
{
  if(quad){ gluDeleteQuadric(quad); quad=0; }
  if(model){ delete model; model=0; }
  if(bbox)delete bbox;
}

/**********
* Loading *
**********/
bool RBody::Load(QInfo *info,cstring path)
// Default path is "body" (if 'path'==0)
{
  DBG_C("Load")
  char buf[128];
  DVector3 position,vec;
  rfloat r;
  
  if(!path)path="body";
  
  Destroy();

  // Location (=car really)
  sprintf(buf,"%s.x",path);
  position.x=info->GetFloat(buf);
  sprintf(buf,"%s.y",path);
  position.y=info->GetFloat(buf,1.0f);
  sprintf(buf,"%s.z",path);
  position.z=info->GetFloat(buf);
  linPos=position;
  
  // Mass
  sprintf(buf,"%s.mass",path);
  r=info->GetFloat(buf,750.f);
  // Minimal mass (otherwise it will get infinite acceleration)
  if(r<0.1)r=0.1;
  // Set mass using RRigidBody function, since this also
  // calculates 1/mass for faster calculations
  SetMass(r);
  
  // Inertia in 3 dimensions
  sprintf(buf,"%s.inertia.x",path);
  vec.x=info->GetFloat(buf);
  sprintf(buf,"%s.inertia.y",path);
  vec.y=info->GetFloat(buf);
  sprintf(buf,"%s.inertia.z",path);
  vec.z=info->GetFloat(buf);
  SetInertia(vec.x,vec.y,vec.z);
  
  // Size
  sprintf(buf,"%s.width",path);
  size.x=info->GetFloat(buf,1.0f);
  sprintf(buf,"%s.height",path);
  size.y=info->GetFloat(buf,0.4f);
  sprintf(buf,"%s.length",path);
  size.z=info->GetFloat(buf,3.0f);

  sprintf(buf,"%s.cockpit_start",path);
  cockpitZ=info->GetFloat(buf,1.0f);
  sprintf(buf,"%s.cockpit_length",path);
  cockpitLength=info->GetFloat(buf,1.0f);

  // Model
  model=new RModel(car);
  model->Load(info,path);
  if(!model->IsLoaded())
  { quad=gluNewQuadric();
  } else
  {
    // Automatically calculate ACTUAL size
    DBox box;
size.DbgPrint("Body indicated size");
    model->GetGeode()->GetBoundingBox(&box);
    box.GetSize(&size);
size.DbgPrint("Body model-calc'ed size");
  }
  bbox=new DBoundingBox();
  bbox->EnableCSG();
#ifdef FUTURE
  bbox->GetBox()->size.x=size.x;
  bbox->GetBox()->size.y=size.y;
  bbox->GetBox()->size.z=size.z;
#endif
  
  return TRUE;
}

/**********
* Attribs *
**********/
void RBody::DisableBoundingBox()
{
  flags&=~DRAW_BBOX;
}
void RBody::EnableBoundingBox()
// When called, this will draw a bounding box together with the model
{
  flags|=DRAW_BBOX;
}

/********
* Paint *
********/
#ifdef RR_GFX_OGL
static void gluCube(double wid,double hgt,double dep)
// Draws a cube at Z=0 to Z=dep
{
  glBegin(GL_QUADS);
    // Top
    glNormal3d(0,1,0);
    glVertex3d(-wid/2,hgt/2,dep);
    glVertex3d(wid/2,hgt/2,dep);
    glVertex3d(wid/2,hgt/2,0);
    glVertex3d(-wid/2,hgt/2,0);
    // Back
    glNormal3d(0,0,1);
    glVertex3d(-wid/2,hgt/2,dep);
    glVertex3d(-wid/2,-hgt/2,dep);
    glVertex3d(wid/2,-hgt/2,dep);
    glVertex3d(wid/2,hgt/2,dep);
    // Front
    glNormal3d(0,0,-1);
    glVertex3d(-wid/2,hgt/2,0);
    glVertex3d(wid/2,hgt/2,0);
    glVertex3d(wid/2,-hgt/2,0);
    glVertex3d(-wid/2,-hgt/2,0);
    // Left
    glNormal3d(1,0,0);
    glVertex3d(wid/2,hgt/2,0);
    glVertex3d(wid/2,hgt/2,dep);
    glVertex3d(wid/2,-hgt/2,dep);
    glVertex3d(wid/2,-hgt/2,0);
    // Right
    glNormal3d(-1,0,0);
    glVertex3d(-wid/2,hgt/2,dep);
    glVertex3d(-wid/2,hgt/2,0);
    glVertex3d(-wid/2,-hgt/2,0);
    glVertex3d(-wid/2,-hgt/2,dep);
    // Bottom
    glNormal3d(0,-1,0);
    glVertex3d(-wid/2,-hgt/2,dep);
    glVertex3d(-wid/2,-hgt/2,0);
    glVertex3d(wid/2,-hgt/2,0);
    glVertex3d(wid/2,-hgt/2,dep);
  glEnd();
}
#endif

void RBody::Paint()
// Paint the body
// Does NOT push/pop the matrix
{
  DBG_C("Paint")

  double x,y,z,len;
  float colBody[]={ 1,.2,.1 };

#ifdef RR_GFX_OGL
  glTranslatef(linPos.GetX(),linPos.GetY(),linPos.GetZ());

  // Heading is in radians
  // Rotation is done according to the order in
  // "Fundamentals of Vehicle Dynamics" (page 10); yaw, pitch, roll
#ifdef FUTURE_QUAT
  glRotatef(rotation.y*RR_RAD_DEG_FACTOR,0,1,0);
  glRotatef(rotation.x*RR_RAD_DEG_FACTOR,1,0,0);
  glRotatef(rotation.z*RR_RAD_DEG_FACTOR,0,0,1);
#endif
  // Get rotation matrix from the quaternion's derived 3x3 orientation
  DMatrix4 m4;
  m4.FromMatrix3(&mRotPos);
#ifdef OBS
  CreateMatrix4FromMatrix3(mRotPos.GetM(),m4.GetM());
mRotPos.DbgPrint("mRotPos");
m4.DbgPrint("m4 for OpenGL");
#endif
  glMultMatrixf(m4.GetM());
  
  // Visual forces
  DVector3 v;
  glTranslatef(0,2,.01);
  v=forceGravityCC;
//qdbg("RB:Pnt; grav=%f\n",v.y);
  RGfxVector(&v,.001,0,1,1);
  glTranslatef(0,-2,-.01);

  // Remember this as the geometric center of the body
  glPushMatrix();
  
  if(model!=0&&model->IsLoaded())
  {
    model->Paint();
//goto paint_stub;
  } else
  {
   paint_stub:
    // Stub graphics
    glMaterialfv(GL_FRONT,GL_DIFFUSE,colBody);
    // Back part
    glTranslated(0,0,-size.z/2);
    //glTranslated(0,0,cockpitZ/2);
#ifdef OBS
qdbg("  size: %f,%f,%f\n",size.x,size.y,size.z);
qdbg("  cockpit: Z=%f, len=%f\n",cockpitZ,cockpitLength);
qdbg("  backpart: translated %f and %f\n",-size.z/2,cockpitZ/2);
#endif
    gluCube(size.x,size.y,cockpitZ);
    // Cockpit part is half as high
    glTranslated(0,-size.y/4,cockpitZ);
//qdbg("  cockpit: translate=%f\n",cockpitZ/2+cockpitLength/2);
    gluCube(size.x*0.8,size.y/2,cockpitLength);
    // Front part
    len=size.z-cockpitZ-cockpitLength;
    glTranslated(0,size.y/4,cockpitLength);
//qdbg("  front: len=%f, translate=%f\n",len,cockpitLength/2+len/2);
    gluCube(size.x*1.0,size.y,len);
  }

  if(flags&DRAW_BBOX)
    bbox->Paint();
    
  // Hierarchical objects coming up (using center of geometry)
  glPopMatrix();
#endif
}

/**********
* Animate *
**********/
void RBody::Integrate()
// Calculate new rigid body state
{
//totalForce.x=totalForce.z=0;  // $DEV
//totalForce.DbgPrint("RBody:Integrate; totalForce");
//totalTorque.DbgPrint("RBody:Integrate; totalTorque");
#ifdef OBS
totalForce.SetToZero();    // No linear moves $DEV
totalTorque.x=totalTorque.z=0;
#endif
  IntegrateEuler();
#ifdef OBS_QUAT
  // Accelerate vehicle
  velocity+=RMGR->time->span*acceleration;
  translation=RMGR->time->span*velocity;
  position+=translation;
#endif

#ifdef OBS
  // Pass on wheel position change to suspension
  if(!susp->ApplyWheelTranslation(&translation,&velocity))
  { // Can't go there physically; correct
    position+=translation;
  }
#endif
}

/**********
* Physics *
**********/
void RBody::PreAnimate()
{
  IntegrateInit();
}

/***************************
* Calculate forces/torques *
***************************/
void RBody::CalcForces()
{
  // Gravity
  forceGravityWC.x=0;
  forceGravityWC.y=-GetMass()*RMGR->scene->env->GetGravity();
  forceGravityWC.z=0;
#ifdef OBS
  // Into car coordinates
  car->ConvertWorldToCarOrientation(&forceGravityWC,&forceGravityCC);
qdbg("Gravity: WC=(%.2f,%.2f,%.2f), CC=(%.2f,%.2f,%.2f)\n",
forceGravityWC.x,forceGravityWC.y,forceGravityWC.z,
forceGravityCC.x,forceGravityCC.y,forceGravityCC.z);
#endif

  // Drag
  forceDragCC.SetToZero();
}

/**************************
* Applying forces/torques *
**************************/
void RBody::ApplyForces()
{
  int i;
  double ax,ay,az;
  double accMag;
  DVector3 force,forceWC,carAcc;
  
  // Linear acc.
  
  force.SetToZero();
  //
  // VERTICAL forces (Y)
  //
  for(i=0;i<car->GetWheels();i++)
  {
    // Force at suspension i
    force+=*car->GetSuspension(i)->GetForceBody();
//car->GetSuspension(i)->GetForceBody()->DbgPrint("susp->body force");
//qdbg("RBody: susp%d force.y=%f\n",
//i,car->GetWheel(i)->GetSuspension()->GetForceBody()->y);
  }

#ifdef OBS
  // Gravity
  if(!(RMGR->devFlags&RManager::NO_GRAVITY_BODY))
    force+=forceGravityWC;
#endif

//qdbg("  gravity force.y=%f\n",forceGravityWC.y);
//qdbg("RB:AF; sum(suspForces)=(%f,%f,%f)\n",
//force.x,force.y,force.z);

  // LATERAL and LONGITUDINAL (XZ) forces (in the ground plane)
  // Sum of all wheel forces (translational), in car coords
  for(i=0;i<car->GetWheels();i++)
  {
    force+=*car->GetWheel(i)->GetForceBodyCC();
//car->GetWheel(i)->GetForceBodyCC()->DbgPrint("wheel.forceBodyCC");
  }
#ifdef OBS
qdbg("RB:AF; sum(latLongForces)=(%f,%f,%f)\n",
force.x,force.y,force.z);
#endif
  // Acceleration is in the world coordinate system
  car->ConvertCarToWorldOrientation(&force,&forceWC);
  
  // Add gravity (simpler in world coordinates)
  if(!(RMGR->devFlags&RManager::NO_GRAVITY_BODY))
    forceWC+=forceGravityWC;
    
  // Total acceleration at CG in world coordinates
  // (future: add force separately, let RRigidBody add the forces)
  DVector3 pos(0,0,0);
//forceWC.DbgPrint("forceWC total");
  AddWorldForceAtBodyPos(&forceWC,&pos);
//qdbg("Body net forceWC=(%f,%f,%f)\n",forceWC.x,forceWC.y,forceWC.z);
}

void RBody::ApplyRotations()
// See what the forces are doing to the torques
// The torques make the car rotate around its CM
// FUTURE: project force onto the perpendicular line moving around
// the circle from CM to wheel center. Now the lateral force
// is just taken as if perpendicular.
{
  DVector3 *force,forceCar;
  DVector3 torque;
  rfloat r,v;
  int    i;
  RWheel *w;
  
  torque.SetToZero();

//qdbg("RCar:ApplyRotations\n");
  for(i=0;i<car->GetWheels();i++)
  {
    w=car->GetWheel(i);

    // Yaw moment/torque
    //ConvertWorldToCarCoords(w->GetForceBody(),&forceCar);
    forceCar=*w->GetForceBodyCC();
    // To get the torque around the car's CM, we could project the lateral
    // force to be perpendicular to the circle with centerpoint CM
    // and radius r (distance from CM to wheel)
    // However, a 2nd way to express torque is to project 'r' itself, and
    // leave F intact. Since the LATERAL force is always perpendicular
    // to the Z axis, we can use the alternative:
    //   torque=F*wheel_z
    // Remember: torque also is rot_intertia*ang.acc, so:
    //   ang.acc.=F*wheel_z/rot_inertia
    // That works out much more elegantly, although in the other case
    // we are using cosines that stay constant, so not much extra work
    // would be needed in fact. But this is simpler.
    // 04-02-01; keeping it at torque to let RRigidBody do
    // the inertia right.
    torque.y+=forceCar.x*w->GetSuspension()->GetZ();
//qdbg("RBody:AR; forceCar.x=%f,suspZ=%f, torque.y+=%f\n",
//forceCar.x,w->GetSuspension()->GetZ(),forceCar.x*w->GetSuspension()->GetZ());
#ifdef OBS_QUAT
    aaLat+=forceCar.x*w->GetSuspension()->GetZ()/I;
#endif
#ifdef OBS
qdbg("  Body rotsX: wheel %d; forceLat=%f, wheelZ=%f, aaLat+=%f\n",
i,forceCar.x,w->GetSuspension()->GetZ(),
forceCar.x*w->GetSuspension()->GetZ()/I);
#endif
    // Same goes for longitudinal forces, only these are
    // projected so that we need the wheel_x coordinate
    torque.y+=forceCar.z*w->GetSuspension()->GetX();
#ifdef OBS_QUAT
    aaLat+=forceCar.z*w->GetSuspension()->GetX()/I;
#endif
#ifdef OBS
qdbg("  Body rotsZ: wheel %d; forceLong=%f, wheelX=%f, aaLat+=%f\n",
i,forceCar.z,w->GetSuspension()->GetX(),
forceCar.z*w->GetSuspension()->GetX()/I);
#endif

//qdbg("devFlags=%x\n",RMGR->devFlags);
    if(RMGR->devFlags&RManager::NO_SUSPENSION)goto skip_pitch;

    // Pitch torque
    // Upward force because of surface normal force
    //force=w->GetForceRoadTC();
    force=w->GetForceBodyCC();
    torque.x-=force->y*w->GetPosition()->z;
    // Acceleration force applied at CG
    // The application point is probably not right, but should
    // use some suspension point somewhere instead.
    // Also, the CG is not really constant as the car pitches
    // and rolls.
    torque.x-=force->z*car->GetCGHeight();
//qdbg("RBody:AA; torque.x+=%f, force_y=%f, whl_z=%f\n",
//force->y*w->GetPosition()->z,force->y,w->GetPosition()->z);
//qdbg("RBody:AA; torque.x+=%f, force_z=%f, cg hgt=%f\n",
//force->z*car->GetCGHeight(),force->z,car->GetCGHeight());

   skip_pitch:;

    // Roll torque
    //
    // Rolling moment because of suspension upward force
    force=w->GetForceBodyCC();
    torque.z+=force->y*w->GetPosition()->x;

    // Rolling moment because of lateral forces (jacking?)
    // Note we should use the roll center of the wheel, and NOT
    // the CG.
    force=w->GetForceBodyCC();
    torque.z+=force->x*
      (car->GetCGHeight()-car->GetSuspension(i)->GetRollCenter()->y);
  }

  // Add a torque in body coordinates
  AddBodyTorque(&torque);
}