Module: rwheel.cpp

/*
 * RWheel - a wheel
 * 05-08-00: Created! (03:46:17)
 * 11-12-00: Using RModel instead of direct geode. Bounding box support.
 * NOTES:
 * (C) MarketGraph/RvG
 */

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

#define ENV_INI      "env.ini"

// Skidding parameters; longitudinal
#define SKID_LONG_START   0.15f
#define SKID_LONG_MAX     1.0f
// Skidding sideways
#define SKID_LAT_START    0.15f
#define SKID_LAT_MAX      1.0f

// Apply friction capping to get max total force
#define DO_CAP_FRICTION

// Apply low-speed enhancements?
#define DO_LOW_SPEED

struct SurfaceFriction
{
  float staticFriction,
        kineticFriction;
};
static SurfaceFriction surface,*curSurface;

RWheel::RWheel(RCar *_car)
{
  car=_car;
  mass=750;
  radius=1;
  width=.2;               // 20 cm
  slices=5;
  propFlags=0;
  flags=0;
  lock=40;
  frictionDefault=1;
  crvSlipTraction=0;
  crvSlipBraking=0;
  crvLatForce=0;
  slipRatio=0;
  slipAngle=0;
  load=0;
  rapSkid=0;
  radiusLoaded=0;
  velMagnitude=0;
  distCGM=distCM=0;
  tireRate=0;
  aligningTorque=0;
  inertia.SetToZero();
  offsetWC.SetToZero();
  torqueTC.SetToZero();
  forceRoadTC.SetToZero();
  forceEngineTC.SetToZero();
  forceBrakingTC.SetToZero();
  forceBodyCC.SetToZero();
  forceGravityCC.SetToZero();
  velWheelCC.SetToZero();
  posContactWC.SetToZero();
  slipVectorCC.SetToZero();
  position.SetToZero();
  velocity.SetToZero();
  rotation.SetToZero();
  rotationV.SetToZero();
  rotationA.SetToZero();
  slipVector.SetToZero();
  forceVerticalCC.SetToZero();
  devPoint.SetToZero();
  Init();
  
  // Surface hack
  QInfo info(RFindFile(ENV_INI));
  curSurface=&surface;
  curSurface->staticFriction=info.GetFloat("surface.static_friction");
  curSurface->kineticFriction=info.GetFloat("surface.kinetic_friction");
  
#ifdef RR_GFX_OGL
  quad=0;
#endif
  model=new RModel(car);
  bbox=new DBoundingBox();
}
RWheel::~RWheel()
{
  Destroy();
}

void RWheel::Destroy()
{
#ifdef RR_GFX_OGL
  if(quad){ gluDeleteQuadric(quad); quad=0; }
#endif
  if(model){ delete model; model=0; }
  if(bbox){ delete bbox; bbox=0; }
  // Curves
  if(crvSlipTraction){ delete crvSlipTraction; crvSlipTraction=0; }
  if(crvSlipBraking){ delete crvSlipBraking; crvSlipBraking=0; }
  if(crvLatForce){ delete crvLatForce; crvLatForce=0; }
  crvSlipTraction=0;
  crvSlipBraking=0;
  crvLatForce=0;
}

void RWheel::Init()
// Reset vars to re-use component
{
  stateFlags=ATTACHED;
}

/**********
* Loading *
**********/
bool RWheel::Load(QInfo *info,cstring path)
{
  char buf[128],fname[256];
  QInfo *infoCurve;
  DVector3 p;
  
  Destroy();

  // Location
  sprintf(buf,"%s.x",path);
  offsetWC.x=info->GetFloat(buf);
  sprintf(buf,"%s.y",path);
  offsetWC.y=info->GetFloat(buf);
  sprintf(buf,"%s.z",path);
  offsetWC.z=info->GetFloat(buf);

  // Physical attribs
  sprintf(buf,"%s.mass",path);
  mass=info->GetFloat(buf,20.0f);
  sprintf(buf,"%s.inertia",path);
  inertia.x=info->GetFloat(buf,2);
  sprintf(buf,"%s.radius",path);
  radius=info->GetFloat(buf,.25f);
  sprintf(buf,"%s.width",path);
  width=info->GetFloat(buf,.3f);
  sprintf(buf,"%s.tire_rate",path);
  tireRate=info->GetFloat(buf,140000.0f);

  // Braking force (specified as torque)
  sprintf(buf,"%s.max_braking",path);
  maxBrakingTorque=info->GetFloat(buf,2000.0f);
  // Braking factor (multiplied by maxBrakingTorque to get actual
  // torque). Used to determine brake balance.
  sprintf(buf,"%s.braking_factor",path);
  brakingFactor=info->GetFloat(buf,1);

  // Lock in radians
  sprintf(buf,"%s.lock",path);
  lock=info->GetFloat(buf,40.0f)/RR_RAD_DEG_FACTOR;
  sprintf(buf,"%s.steering",path);
  if(info->GetInt(buf,1))
    propFlags|=STEERING;
  sprintf(buf,"%s.powered",path);
  if(info->GetInt(buf))
    propFlags|=POWERED;
  // Get some friction base values
  sprintf(buf,"%s.friction_road",path);
  frictionDefault=info->GetFloat(buf,1.0f);
  
  // Audio
  if(RMGR->audio)
  { sprintf(buf,"car.skid.sample");
    info->GetString(buf,fname);
    rapSkid=new RAudioProducer();
    rapSkid->LoadSample(RFindFile(fname,car->GetCarDir()));
    rapSkid->SetVolume(0);
    RMGR->audio->AddProducer(rapSkid);
  }

  // Tire model
  //sprintf(buf,"%s.pacejka",path);
  sprintf(buf,"pacejka");
  pacejka.Load(info,buf);
  
  // Curves
  crvSlipTraction=new QCurve();
  crvSlipBraking=new QCurve();
  crvLatForce=new QCurve();
  
  sprintf(buf,"%s.curves.traction_force",path);
  info->GetString(buf,fname);
//qdbg("fname_traction='%s'\n",fname);
  if(fname[0])
  { infoCurve=new QInfo(RFindFile(fname,car->GetCarDir()));
    crvSlipTraction->Load(infoCurve,"curve");
    delete infoCurve;
  }
  sprintf(buf,"%s.curves.braking_force",path);
  info->GetString(buf,fname);
//qdbg("fname_braking='%s'\n",fname);
  if(fname[0])
  { infoCurve=new QInfo(RFindFile(fname,car->GetCarDir()));
    crvSlipBraking->Load(infoCurve,"curve");
    delete infoCurve;
  }

  sprintf(buf,"%s.curves.lat_force",path);
  info->GetString(buf,fname);
//qdbg("fname_latforce='%s'\n",fname);
  if(fname[0])
  { infoCurve=new QInfo(RFindFile(fname,car->GetCarDir()));
    crvLatForce->Load(infoCurve,"curve");
    delete infoCurve;
  }

  // Paint attribs (default wheel representation)
  sprintf(buf,"%s.slices",path);
  slices=info->GetInt(buf,20);
  // Model (or stub gfx)
  model=new RModel(car);
  model->Load(info,path);
  if(!model->IsLoaded())
    quad=gluNewQuadric();
  bbox=new DBoundingBox();
  bbox->EnableCSG();
  bbox->GetBox()->min.x=-width;
  bbox->GetBox()->min.y=-radius;
  bbox->GetBox()->min.z=-radius;
  bbox->GetBox()->max.x=width;
  bbox->GetBox()->max.y=radius;
  bbox->GetBox()->max.z=radius;

  Reset();

#ifdef OBS
  // Take over car initial orientation
  DVector3 p;
  // Note this call is reversed wrt coord systems
  car->ConvertCarToWorldOrientation(&position,&p);
  rotation=*car->GetBody()->GetRotation();
qdbg("RWh: pos=%f,%f,%f, p=%f,%f,%f\n",position.x,position.y,position.z,
p.x,p.y,p.z);
  position=p;
#endif

  // Deduce some facts
  
  // Calc distance of wheel to CGM
  p=*susp->GetPosition()+offsetWC;
  distCGM=sqrt(p.x*p.x+p.z*p.z);
  
  // Calc angle from CGM to wheel
  angleCGM=atan2(p.x,p.z);
  
  // Calculate distance to CM
  distCM=distCGM;
  
  // Debugging
  rotationV.x=RMGR->infoDebug->GetFloat("car.wheel_rvx");
  return TRUE;
}

/********
* Reset *
********/
void RWheel::Reset()
{
  // Place wheel near suspension
  position=*susp->GetPosition()+offsetWC;
  position.y-=susp->GetRestLength();
  velocity.SetToZero();
  rotation.SetToZero();
  rotationV.SetToZero();
  rotationA.SetToZero();
//qdbg("RWheel ctor: position=%f/%f/%f\n",position.x,position.y,position.z);
}

/**********
* Attribs *
**********/
void RWheel::SetHeading(float h)
// Set the heading
{
  if(h>lock)h=lock;
  else if(h<-lock)h=-lock;
  rotation.y=h;
}
void RWheel::DisableBoundingBox()
{
  flags&=~DRAW_BBOX;
}
void RWheel::EnableBoundingBox()
{
  flags|=DRAW_BBOX;
}
bool RWheel::IsSteering()
{
  if(propFlags&STEERING)return TRUE;
  return FALSE;
}
bool RWheel::IsPowered()
{
  if(propFlags&POWERED)return TRUE;
  return FALSE;
}
bool RWheel::IsAttached()
{
  if(stateFlags&ATTACHED)return TRUE;
  return FALSE;
}
bool RWheel::IsOnSurface()
// Returns TRUE if wheel is touching a surface
{
  if(stateFlags&ON_SURFACE)return TRUE;
  return FALSE;
}
bool RWheel::IsLowSpeed()
// Returns TRUE if wheel is turning slowly so you may need
// to reduce some effects to avoid low-speed numerical instability
{
  if(stateFlags&LOW_SPEED)return TRUE;
  return FALSE;
}

/********
* Paint *
********/
void RWheel::Paint()
{
  float colTyre[]={ .2,.2,.2 };
  DVector3 pos;
  
#ifdef RR_GFX_OGL
  //
  // OpenGL wheel painting
  //
  glPushMatrix();
  
  // Location
  //pos=position+*susp->GetPosition();
  pos=position;
//qdbg("RWh:Paint; pos=%f/%f/%f\n",pos.x,pos.y,pos.z);
//qdbg("Car pos=%f,%f,%f\n",car->GetPosition()->x,
//car->GetPosition()->y,car->GetPosition()->z);
  //pos.y-=susp->GetLength();
  //glTranslatef(position.GetX(),position.GetY(),position.GetZ());
  glTranslatef(pos.GetX(),pos.GetY(),pos.GetZ());
//qdbg("RWh:Paint: y=%f\n",pos.GetY());
  
#ifdef OBS
  // Forces
  float v[3];
  v[0]=0; v[1]=0; v[2]=force.GetZ();
  glColor3f(0,0,0);
  GfxArrow(0,-radius+.01,0,v,.001);
  // Lateral
  glRotatef(-90,0,1,0);
  v[0]=0; v[1]=0; v[2]=force.GetX();
  GfxArrow(0,-radius+.01,0,v,.001);
  glRotatef(90,0,1,0);
#endif
  
  // Vectors in CC
//qdbg("RWh:Paint; rotation.y=%f deg\n",rotation.y*RR_RAD_DEG_FACTOR);
  //glRotatef(car->GetBody()->GetRotation()->y*RR_RAD_DEG_FACTOR,0,1,0);
  
  if(RMGR->flags&RManager::SHOW_TIRE_FORCES)
  {
    DVector3 v;
    glTranslatef(0,-radius+.01,0);
    v=velWheelCC;
    RGfxVector(&v,.1,0,1,0);
    v=slipVectorCC;
    RGfxVector(&v,.1,1,1,0);
    // Split body force into XZ and Y for clarity
    v=forceBodyCC; v.y=0;
    RGfxVector(&v,.001,1,0,0);
    v=forceBodyCC; v.x=v.z=0;
    RGfxVector(&v,.001,1,0,0);
    glTranslatef(0,radius-.01,0);
  }
  
  //glRotatef(-car->GetBody()->GetRotation()->y*RR_RAD_DEG_FACTOR,0,1,0);
  
#ifdef ND_PAINT_FORCES
  glTranslatef(0,-radius+.01,0);
  car->ConvertCarToWorldCoords(&force,&v);
  //RGfxVector(&v,.001,0,0,0);
  //car->ConvertCarToWorldCoords(&forceTorque,&v);
  v=forceTorque;
  RGfxVector(&v,.001,0,1,0);
  glTranslatef(0,.01,0);
  //car->ConvertCarToWorldCoords(&forceFriction,&v);
  v=forceFriction;
  RGfxVector(&v,.001,1,0,0);
  glTranslatef(0,.01,0);
  car->ConvertWorldToCarCoords(&forceSlip,&v);
  RGfxVector(&v,.001,1,.5,.5);
  car->ConvertWorldToCarCoords(&slipVector,&v);
  RGfxVector(&v,.1,1,1,0);
  glTranslatef(0,-.01,0);
  glTranslatef(0,-.01,0);
  glTranslatef(0,radius-.01,0);
#endif
  
#ifdef OBS
  // Velocity
  v[0]=vx; v[1]=vy; v[2]=vz;
  glColor3f(1,0,0);
  GfxArrow(0,radius,0,v,.1);
#endif
  
#ifdef OBS
  // Text prepare
  GfxGo(0,0,0);
#endif
  
  if(flags&DRAW_BBOX)
  {
    // Draw the bounding box in the model-type rotation
    // (the stub must rotate so the cylinder is at the local Z-axis)
    glPushMatrix();
    // Heading
    glRotatef(rotation.y*RR_RAD_DEG_FACTOR,0,1,0);
    // Rotation of wheel (from radians to degrees)
    glRotatef(rotation.x*RR_RAD_DEG_FACTOR,1,0,0);
    bbox->Paint();
    glPopMatrix();
  }

  // Discs are painted at Z=0
  if(quad)
  {
    glRotatef(90,0,1,0);
    // Heading
    glRotatef(rotation.y*RR_RAD_DEG_FACTOR,0,1,0);
    // Rotation of wheel (from radians to degrees)
    glRotatef(rotation.x*RR_RAD_DEG_FACTOR,0,0,1);
  } else
  {
    // Heading
    glRotatef(rotation.y*RR_RAD_DEG_FACTOR,0,1,0);
    // Rotation of wheel (from radians to degrees)
    glRotatef(rotation.x*RR_RAD_DEG_FACTOR,1,0,0);
  }

  // Primitives
  if(model!=0&&model->IsLoaded())
  {
    model->Paint();
    goto skip_quad;
  } // else paint stub gfx
  
  car->SetDefaultMaterial();
  glDisable(GL_TEXTURE_2D);

  // Center point for quad
  glTranslatef(0,0,-width/2);
  
#ifdef OBS
  if(rdbg->flags&RF_WHEEL_INFO)
  { char buf[80];
    sprintf(buf,"Force lon=%.f, lat=%.f",
      force.z,force.x);
    GfxText3D(buf);
  }
#endif
 
  glMaterialfv(GL_FRONT,GL_DIFFUSE,colTyre);
  gluCylinder(quad,radius,radius,width,slices,1);
  gluQuadricOrientation(quad,GLU_INSIDE);
  gluDisk(quad,0,radius,slices,1);
  gluQuadricOrientation(quad,GLU_OUTSIDE);
  glTranslatef(0,0,width);
  gluDisk(quad,0,radius,slices,1);
  
 skip_quad:
  
  glPopMatrix();
#endif

}

/***********************
* Slip ratio and angle *
***********************/
void RWheel::CalcSlipAngle()
// Based on the wheel world velocity and heading, calculate
// the angle from the wheel heading to the wheel's actual velocity
// Contrary to SAE, our Y-axis points up, and slipAngle>0 means
// in our case that the you're in a right turn (basically)
{
  DVector3 velWheelWC;
  rfloat   v,angle;
  
  if(!IsOnSurface())
  {
//qdbg("CalcSA; not on surface\n");
    // Not touching the surface, no need to do slip angle
    slipAngle=0;
    velWheelTC.SetToZero();
    velWheelCC.SetToZero();
    return;
  }

  // Calculate current wheel velocity in body (car) coords
  velWheelWC=*car->GetVelocity();
  // Convert velocity from world to car coords
  car->ConvertWorldToCarOrientation(&velWheelWC,&velWheelCC);
#ifdef OBS
  velWheelCC=*car->GetVelocity();
  car->ConvertCarToWorldOrientation(&velWheelCC,&velWheelWC);
#endif
  
  // Calculate wheel velocity because of rotation around yaw (Y) axis
  velWheelCC.x+=car->GetBody()->GetRotVel()->y*cos(-angleCGM)*distCGM;
  velWheelCC.z+=car->GetBody()->GetRotVel()->y*sin(-angleCGM)*distCGM;
#ifdef OBS
qdbg("RWh:CSA; velWheelCC=%f,%f,%f\n",velWheelCC.x,velWheelCC.y,
velWheelCC.z);
#endif
  // Get world velocity from suspension on body
  car->GetBody()->CalcBodyVelForBodyPos(susp->GetPosition(),&velWheelCC);
//qdbg("RWh:CSA; velWheelCC_RRB=%f,%f,%f\n",
//velWheelCC.x,velWheelCC.y,velWheelCC.z);
//qdbg("RWh:CSA; wheel heading=%f, distCGM=%f\n",GetHeading(),distCGM);
  
  // Calculate slip angle as the angle between the wheel velocity vector
  // and the wheel direction vector
  if(velWheelCC.x>-RR_EPSILON_VELOCITY&&velWheelCC.x<RR_EPSILON_VELOCITY &&
     velWheelCC.z>-RR_EPSILON_VELOCITY&&velWheelCC.z<RR_EPSILON_VELOCITY)
  { // Very low speed; no slip angle
    slipAngle=-GetHeading();
//qdbg("  RWh;CSA; LS => slipAngle=0\n");
  } else
  { // Enough velocity, calculate real angle
    slipAngle=atan2(velWheelCC.x,velWheelCC.z)-GetHeading();
    // Keep slipAngle between -180..180 degrees
    if(slipAngle<-PI)slipAngle+=2*PI;
    else if(slipAngle>PI)slipAngle-=2*PI;
//qdbg("  atan2(%f,%f)=%f\n",velWheelCC.x,velWheelCC.z,
//atan2(velWheelCC.x,velWheelCC.z));
  } 

  ConvertCarToTireOrientation(&velWheelCC,&velWheelTC);
//qdbg("  slipAngle=%.3f\n",slipAngle*RR_RAD_DEG_FACTOR);
//velWheelTC.DbgPrint("velWheelTC");
}
void RWheel::CalcSlipRatio(DVector3 *velWheelTC)
// Uses the Calspan TIRF definition; SR=ang.vel*Rloaded/V-1 (slipAngle=0)
// Note that this won't reach absolute zero in real life situations, because
// of the difference between R-loaded and R-effective.
{
  rfloat vGround,vFreeRolling;

//velWheelTC->DbgPrint("velWheelTC in CalcSR");
  if(!IsOnSurface())
  {
//qdbg("CalcSR; not on surface\n");
    // Not touching the surface, no slip ratio
    slipRatio=0;
    return;
  }

  // Calculate longitudinal velocity of the wheel (in TC) wrt ground
  vGround=velWheelTC->z;

  // Low velocity leads to numerical instability
  if(vGround<.001&&vGround>-.001)       // in m/s (.01=1cm/s)
  {
    // For low speed, switch to a different model to calculate
    // slip ratio
    vFreeRolling=rotationV.x*radius;
#ifdef FUTURE_LINEAR_RATIO
    slipRatio=(vGround-vFreeRolling)*.01;
qdbg("RWh: Low velocity; alternate slip ratio model SR=%f\n",slipRatio);
#endif
//qdbg("RWh:CalcSR; LS: vFreeRolling=%f, vGround=%f\n",vFreeRolling,vGround);
    // Estimate for low speed
    // Leave things for when the tires start moving
    slipRatio=0;
#ifdef OBS
    if(vFreeRolling>-.001&&vFreeRolling<.001)
    {
      slipRatio=vGround-vFreeRolling;
    } else
    {
      if(vFreeRolling<0)slipRatio=-1;
      else              slipRatio=10;
    }
#endif
  } else
  {
#ifdef OBS
    if(vGround<0)
    {
      if(rotationV.x>0)
        slipRatio=rotationV.x*radiusLoaded/-vGround-1;
      else
        slipRatio=rotationV.x*radiusLoaded/vGround-1;
    } else
#endif
    {
      slipRatio=rotationV.x*radiusLoaded/vGround-1;
#ifdef OBS
if(this==car->GetWheel(2))
{qdbg("  SR=%f (rotV.x=%f, vWheel=%f, vGround=%f)\n",slipRatio,
 rotationV.x,rotationV.x*radiusLoaded,vGround);
}
#endif
    }
  }
  // Truncate slip ratio
  if(slipRatio<-2.0f)slipRatio=-2.0f;
  else if(slipRatio>10.0f)slipRatio=10.0f;
//slipRatio=0;
}
void RWheel::CalcLoad()
// Calculate load on wheel (in Newtons)
// Also calculate radius of loaded wheel.
{
  // Take as passed through the suspension
  load=susp->GetForceBody()->y;
  
  // Calculate radius of loaded wheel
  // Should deal with tire pressure and such
  // For now, no deformation
  radiusLoaded=radius;

//qdbg("RWheel:CalcLoad; load=%.4f, radiusLoaded=%.2f\n",load,radiusLoaded);
}
void RWheel::CalcWheelPosWC()
// Calculate wheel position in world coordinates, looking at body
// pitch, roll and yaw.
// 2 different positions are of importance; the contact patch location,
// and the wheel center location. Currently only the CP location is calculated.
// The wheel center may be handy for driving on the side (90 degree roll).
// The CP WC position is used to match against the track (altitude).
{
  DVector3 posContactCC;

#ifdef OBS
qdbg("RWh:CWPWC; position=(%f,%f,%f)\n",
position.x,position.y,position.z);
qdbg("RWh:CWPWC; susp->position=(%f,%f,%f)\n",
susp->GetPosition()->x,susp->GetPosition()->y,susp->GetPosition()->z);
car->GetBody()->GetPosition()->DbgPrint("car Position");
#endif

  // Start with contact patch position in car coordinates
  // Note that 'position' already contains the suspension Y offset
  posContactCC=position;
  // Go from wheel center to edge of wheel
  posContactCC.y-=radius;
#ifdef OBS
qdbg("RWh:CWPWC; contactCC=(%f,%f,%f), pitch=%f\n",
posContactCC.x,posContactCC.y,posContactCC.z,
car->GetBody()->GetRotation()->x*RR_RAD_DEG_FACTOR);
#endif

  // Convert around 
  car->ConvertCarToWorldCoords(&posContactCC,&posContactWC);
#ifdef OBS
qdbg("RWh:CWPWC;   contactCC=(%f,%f,%f)\n",
posContactCC.x,posContactCC.y,posContactCC.z);
qdbg("RWh:CWPWC;   contactWC=(%f,%f,%f)\n",
posContactWC.x,posContactWC.y,posContactWC.z);
car->GetBody()->GetRotPosM()->DbgPrint("Body rotMatrix");
#endif

  // Store to show visually
  devPoint=posContactWC;
  //devPoint.y+=1;
}

/*************
* PreAnimate *
*************/
void RWheel::PreAnimate()
// Calculate wheel state
{
  rfloat y;
  
  // Reset vars
  forceVerticalCC.SetToZero();
  
  // Calculate info on the wheel's state (to be used later on)
  CalcWheelPosWC();
  
  // Get surface underneath the wheel
  // Should take actual downward direction instead of simple down vector
  DVector3 dir(0.0f,-1.0f,0.0f);
  DVector3 org;
  dfloat   dist;
QTRACE("scene=%p\n",RMGR->scene);
QTRACE("scene->track=%p\n",RMGR->scene->track);
  // Start origin a meter above the wheel, to avoid missing the track,
  // but also not too much to avoid sensing a possible bridge ABOVE
  // the car (see the Carrera track for example).
  dist=1.0f;
  org.x=posContactWC.x-dist*dir.x;
  org.y=posContactWC.y-dist*dir.y;
  org.z=posContactWC.z-dist*dir.z;
  // Trace into the track
  RMGR->GetTrack()->GetSurfaceInfo(&org,&dir,&surfaceInfo);
//surfaceInfo.x=surfaceInfo.y=surfaceInfo.z=0;
//qdbg("surface found Y=%.2f\n",surfaceInfo.y);
#ifdef OBS
qdbg("car position: %.2f,%.2f,%.2f\n",
car->GetPosition()->x,car->GetPosition()->y,car->GetPosition()->z);
#endif
//surfaceInfo.y-=5.33;

  // Are we touching a surface? (simple Y only version)
//#ifdef ND_FUTURE
  //y=car->GetPosition()->y-susp->GetLength()-radius;
  y=posContactWC.y-surfaceInfo.y;
//qdbg("RWheel:Pre; carY=%f, suspLen=%f, y=%f\n",
//car->GetPosition()->y,susp->GetLength(),y);
  // On surface when very near ground
  if(y<0.01)
  { stateFlags|=ON_SURFACE;
  } else
  { stateFlags&=~ON_SURFACE;
  }
//qdbg("  IsOnSurface: %d\n",IsOnSurface());
//#endif

  // Load transfer
  CalcLoad();
  
  // Slip angle and ratio
  CalcSlipAngle();
  CalcSlipRatio(&velWheelTC);

  // Calculate slip vector (for debugging and some sounds)
  DVector3 slipVectorTC;
  slipVectorTC.x=0;
  slipVectorTC.y=0;
  slipVectorTC.z=rotationV.x*radius;
  ConvertTireToCarOrientation(&slipVectorTC,&slipVectorCC);
  slipVectorCC-=velWheelCC;
  
  // Low speed detection
  if(rotationV.x>-RMGR->lowSpeed&&rotationV.x<RMGR->lowSpeed)
  { stateFlags|=LOW_SPEED;
    // Calculate factor from 0..1. 0 means real low, 1 is on the edge
    // of becoming high-enough speed.
    lowSpeedFactor=rotationV.x/RMGR->lowSpeed;
    // Don't zero out all
    if(lowSpeedFactor<0.01f)lowSpeedFactor=0.01f;
  } else
  { stateFlags&=~LOW_SPEED;
  }
  
#ifdef OBS
  // Calculate velocity magnitude to base softening of forces on
  // when dealing with low speed numerical instability (wheels
  // shaking back & forth)
  velMagnitude=slipVectorCC.Length();
#endif
  
#ifdef OBS
qdbg("RWh:Pre; slipAngle=%.2f, SR=%.5f (vzTC=%.2f, rotX=%.2f),"
  " load=%.f\n",
slipAngle*RR_RAD_DEG_FACTOR,slipRatio,velWheelTC.z,rotationV.x,load);
qdbg("         v=%f, vCarCC=%f,%f,%f\n",velMagnitude,
car->GetVelocity()->x,car->GetVelocity()->y,car->GetVelocity()->z);
#endif
#ifdef OBS
qdbg("  velWheelTC=%f,%f,%f\n",velWheelTC.x,velWheelTC.y,velWheelTC.z);
#endif
}

/************
* Integrate *
************/
void RWheel::Integrate()
// Integrate physics
// Assumes: body->Animate() has already been called (translation is valid)
{
  DVector3 translation;
  rfloat   oldVX;
  rfloat   netForce;

//qdbg("RWh:Int0; pos=(%.2f,%.2f,%.2f)\n",position.x,position.y,position.z);
//qdbg("RWh;Int; rotV.x=%f, rotA.x=%f\n",rotationV.x,rotationA.x);

  // Wheel rotation (rolling wheel)
  //rotation.x+=(rotationV.x+0.5f*rotationA.x*RMGR->time->span)*RMGR->time->span;
  rotation.x+=rotationV.x*RMGR->time->span;
  // Keep rotation in limits
  while(rotation.x>=2*PI)
    rotation.x-=2*PI;
  while(rotation.x<0)
    rotation.x+=2*PI;
  
  // Rotational acceleration
  oldVX=rotationV.x;
  // Check for locked wheel braking
  netForce=forceEngineTC.z+forceRoadTC.z;
  if(rotationV.x>-RR_EPSILON_VELOCITY&&rotationV.x<RR_EPSILON_VELOCITY)
  { if(forceBrakingTC.z<netForce||forceBrakingTC.z>-netForce)
    {
//qdbg("RWh; braking force keeps wheel still\n");
      rotationV.x=0;
      goto skip_rot;
    }
  }
  rotationV.x+=rotationA.x*RMGR->time->span;
 skip_rot:

//qdbg("RWh;Int; engine+road (z)=%f\n",forceEngineTC.z+forceRoadTC.z);
//qdbg("RWh;Int; braking force=%f\n",forceBrakingTC.z);

  // Friction reversal measures
  if( (oldVX<0&&rotationV.x>0) || (oldVX>0&&rotationV.x<0) )
  { rotationV.x=0;
//qdbg("RWh:Int; friction reversal halt; %f -> %f\n",oldVX,rotationV.x);
  }

  // Translation of wheel in CC (car coords); relative to body (!)
  translation=(velocity+0.5f*acceleration*RMGR->time->span)*RMGR->time->span;
  position+=translation;
  velocity+=acceleration*RMGR->time->span;
#ifdef OBS
acceleration.DbgPrint("RWheel:acceleration");
velocity.DbgPrint("RWheel:velocity");
#endif

#ifdef OBS
qdbg("RWh:An; velZ=%f, accZ=%f, carAccZ=%f\n",
velocity.z,acceleration.z,car->GetAcceleration()->z);
#endif
#ifdef OBS
qdbg("RWh:Int; pos=(%.2f,%.2f,%.2f)\n",position.x,position.y,position.z);
qdbg("  velocity=%f,%f,%f\n",velocity.x,velocity.y,velocity.z);
qdbg("  acc=%f,%f,%f\n",acceleration.x,acceleration.y,acceleration.z);
qdbg("  dt=%f\n",RMGR->time->span);
#endif

  // As 'position' is relative to the car's body, we can pass
  // the translation on to the suspension and see if the suspension
  // is ok with this movement (bump stops etc)
  if(!susp->ApplyWheelTranslation(&translation,&velocity))
  {
    // Can't go there physically; correct
#ifdef OBS
qdbg("RWh:Integrate; correct susp move by (%.2f,%.2f,%.2f)\n",
translation.x,translation.y,translation.z);
qdbg("RWh:Integrate; position (%.2f,%.2f,%.2f) PRE\n",
position.x,position.y,position.z);
#endif
    position+=translation;
#ifdef OBS
qdbg("RWh:Integrate; position (%.2f,%.2f,%.2f) POST\n",
position.x,position.y,position.z);
#endif
  }
}

void RWheel::OnGfxFrame()
// Update audio frequencies and volumes (only once per gfx frame)
{
  // Audio
  // Do a rough approximation of slip sound
  if(!IsOnSurface())
  {
    // Can't skid in the air
    rapSkid->SetVolume(0);
  } else
  {
    rfloat len,skid;
    rfloat skidFactor=0.0f;
    
    // Longitudinal slip sounds
    if(slipRatio<0)
    {
      if(slipRatio<-SKID_LONG_MAX)skidFactor+=1.0f;
      else if(slipRatio<-SKID_LONG_START)
      {
        // Ratio
        skidFactor+=-(slipRatio+SKID_LONG_START)/
          (SKID_LONG_MAX-SKID_LONG_START);
      }
    } else
    {
      // Driving
      if(slipRatio>SKID_LONG_MAX)skidFactor+=1.0f;
      else if(slipRatio>SKID_LONG_START)
      {
        // Ratio
        skidFactor+=(slipRatio-SKID_LONG_START)/
          (SKID_LONG_MAX-SKID_LONG_START);
      }
    }
//qdbg("RWheel: SR=%f, longSkidFactor=%f\n",slipRatio,skidFactor);

    // Adjust skid factor for lowspeed (avoid squeeks at low speed)
    if(IsLowSpeed())
      skidFactor*=lowSpeedFactor;
    
    // Lateral skidding
    
    // Take lateral slipvelocity; should use TC (tire coords) actually,
    // but it's close enough
    len=fabs(slipVectorCC.x);
    if(len>SKID_LAT_MAX)skidFactor+=1.0f;
    else if(len>SKID_LAT_START)
    {
      skidFactor+=(len-SKID_LAT_START)/(SKID_LAT_MAX-SKID_LAT_START);
    }
    
    // Set skid volume
    rapSkid->SetVolume(skidFactor*256.0f);
  }
}

/**************
* Slip vector *
**************/
void RWheel::CalcSlipVector()
// OBS
// Based on the current wheel speed/rotation, calculate
// a slip vector; in which direction the wheel is slipping with
// respect to its theoretical direction
// The slip vector is in world coordinates
{
#ifdef OBS_NO_SLIPVECTOR
  DVector3 velocity,wheelDirection;
  rfloat   vAngle,hAngle,angle;
  rfloat   rollV;
  rfloat   r;
  
  //
  // Calculate ACTUAL wheel velocity; based out of 2 components:
  // the car's movement and the rotational velocity around the car's CM
  // Calculates it in world coordinates (car velocity is in world coords)
  //

  // Vehicle translational component; how fast is the car moving?
  // NOTE: this is in body coordinates
  velocity=*car->GetVelocity();

  // Calculate wheel velocity because of rotation around yaw (Y) axis
  //
  // Calc distance of CM to wheel
  r=sqrt(position.GetX()*position.GetX()+position.GetZ()*position.GetZ());
  // Angle of CM to wheel
  vAngle=atan2(position.GetX(),position.GetZ());
#ifdef OBS
qdbg("AS: r=%f, vAngle[CMtoWheel]=%f deg\n",r,vAngle*RR_RAD_DEG_FACTOR);
qdbg("  wheel X=%f, Z=%f\n",position.GetX(),position.GetZ());
#endif
  vAngle+=car->GetHeading();
#ifdef OBS
qdbg("AS:   vAngle total=%f deg\n",vAngle*RR_RAD_DEG_FACTOR);
#endif
  // Calculate resulting velocity based on rotation around Y-axis only
  velocity.x+=car->GetBody()->GetRotationV()->y*cos(vAngle)*r;
  velocity.y=0;
  velocity.z+=car->GetBody()->GetRotationV()->y*sin(-vAngle)*r;
//qdbg("AS:   vRotation=(%f,%f,%f)\n",vx,vy,vz);

  // Now calculate the velocity of the contact patch
  //
  // Calculate heading of wheel in world coordinates
  angle=car->GetHeading()+GetHeading();
#ifdef OBS
qdbg("  car_heading=%f, wheel heading=%f\n",car->GetHeading(),
GetHeading());
#endif
  // Calculate direction as the rotational speed of the wheel
  // around the axle point times the radius (circumferences/sec)
  rollV=rotationV.x;
#ifdef OBS
qdbg("rotationV.x=%f, radius=%f, angle=%f\n",rollV,radius,angle);
#endif
  wheelDirection.x=rollV*radius*sin(angle);
  wheelDirection.y=0;
  wheelDirection.z=rollV*radius*cos(angle);
  
  // Slip vector is the difference between the two
  slipVector=velocity-wheelDirection;
//slipVector.SetToZero();

#ifdef OBS
qdbg("  wheeldir=%f,,%f, velocity=%f,,%f\n",
wheelDirection.x,wheelDirection.z,velocity.x,velocity.z);
qdbg("  slipvx=%f, slipvz=%f, |slipv|=%f\n",slipVector.x,slipVector.z,
slipVector.Length());
#endif
#endif
}

void RWheel::CalcFrictionCoefficient()
// OBS
// Based on the calculated slip vector, calculate the friction coefficient
// The faster a wheel is slipping with respect to the ground,
// the less grip will be available.
{
  rfloat c,v,factor;
  // Very simple friction algorithm; less friction at high slips
  v=slipVector.Length();
//qdbg("CFC: v=%f\n",v);
  c=frictionDefault-frictionDefault*v;
  if(c<frictionDefault/4)
    c=frictionDefault/4;
  frictionCoeff=c;
//qdbg("  frictionCoefficient=%f\n",frictionCoeff);
}

/*********
* Forces *
*********/
void RWheel::CapFrictionCircle()
// Make sure longitudinal and lateral force, when combined, don't
// exceed the maximum force the tire can generate
{
  rfloat maxForce,maxForceSquared;
  rfloat curForceSquared;

  maxForce=pacejka.CalcMaxForce();
  maxForceSquared=maxForce*maxForce;
  // Calculate current force that was calculated using Pacejka
  curForceSquared=forceRoadTC.x*forceRoadTC.x+forceRoadTC.z*forceRoadTC.z;

#ifdef OBS
qdbg("curF^2=%f, maxF^2=%f\n",curForceSquared,maxForceSquared);
qdbg("curF=%f, maxForce=%f\n",sqrtf(curForceSquared),maxForce);
qdbg("  long=%f, lat=%f\n",forceRoadTC.z,forceRoadTC.x);
#endif
  if(curForceSquared>maxForceSquared)
  {
    if(forceRoadTC.x<0)
      forceRoadTC.x=-sqrtf(maxForceSquared-forceRoadTC.z*forceRoadTC.z);
    else
      forceRoadTC.x=sqrtf(maxForceSquared-forceRoadTC.z*forceRoadTC.z);
#ifdef OBS
qdbg("  CAP lat\n");
curForceSquared=forceRoadTC.x*forceRoadTC.x+forceRoadTC.z*forceRoadTC.z;
qdbg("  to curF=%f, maxForce=%f\n",sqrtf(curForceSquared),maxForce);
#endif
  }
}

#ifdef ND_CAP_FORCE_TO_FRICTION_LATER
  maxForceMag=frictionDefault*load;
  curForceMag=forceRoadTC.Length();
//qdbg("CF: force cap; max=%f, current=%f\n",maxForceMag,curForceMag);
  if(curForceMag>maxForceMag)
  { // Lower total force
    if(maxForceMag>0)
    { forceRoadTC*=maxForceMag/curForceMag;
//qdbg("  CAPPED!\n");
//curForceMag=forceRoadTC.Length();
//qdbg("  Capped to %f\n",curForceMag);
    } else
    { // No load, no friction
      forceRoadTC.SetToZero();
    }
  }
#endif
void RWheel::CalcForces()
{
  rfloat coeff;
  rfloat maxForceMag,curForceMag;

  // Reset
  torqueTC.SetToZero();
  forceRoadTC.SetToZero();

  // VERTICAL FORCES
  
  // Calculate gravitional force
  DVector3 forceGravityWC(0.f,-mass*RMGR->scene->env->GetGravity(),0.f);
  car->ConvertWorldToCarOrientation(&forceGravityWC,&forceGravityCC);
#ifdef OBS
  forceGravityCC.x=forceGravityCC.z=0;
  forceGravityCC.y=-mass*RMGR->scene->env->GetGravity();
// $BUG; gravity is now in WC; convert to CC!
#endif
//qdbg("RWH:CF; forceGravityCC.y=%.2f (mass=%.2f)\n",forceGravityCC.y,mass);

  // Calculate road force pushing up against wheel
  rfloat   d;
  // Calculate distance to ground
  d=posContactWC.y-surfaceInfo.y;
#ifdef OBS
qdbg("  distance to ground (d)=%f, posContactWC.y=%f, surfaceY=%f\n",
d,posContactWC.y,surfaceInfo.y);
#endif
  if(d<0)
  { // Tire is on the ground
    // Calculate resulting force because of tire rate
    forceRoadTC.y=-tireRate*d;
#ifdef OBS
qdbg("RWH:CF; posContactWC.y=%.2f, surfaceY=%.2f\n",
posContactWC.y,surfaceInfo.y);
qdbg("RWH:CF; radius=%f, dGround=%f, F=%f\n",radius,d,forceRoadTC.y);
#endif
  } else
  {
    // Wheel in the air
    forceRoadTC.y=0;
  }

  // Total vertical forces
  // Note that for vertical forces, TC orientation==CC orientation
  forceVerticalCC=forceGravityCC+forceRoadTC+*susp->GetForceWheel();
#ifdef OBS
qdbg("RWH:CF; susp->GetForceWheel()=%.2f,%.2f,%.2f\n",
susp->GetForceWheel()->x,susp->GetForceWheel()->y,susp->GetForceWheel()->z);
forceRoadTC.DbgPrint("  forceRoadTC");
forceGravityCC.DbgPrint("  forceGravityCC");
forceVerticalCC.DbgPrint("  forceVerticalCC");
qdbg("RWH:CF; forceVerticalCC=%.2f,%.2f,%.2f\n",
forceVerticalCC.x,forceVerticalCC.y,forceVerticalCC.z);
#endif
  
  // LONGITUDINAL FORCES

  // Calc long. force because of wheel torque (at contact patch)
  if(IsPowered())
  {
    torqueTC.x=car->GetEngine()->GetTorqueForWheel(this);
//qdbg("engine torque=%f\n",torqueTC.x);
    forceEngineTC.z=-torqueTC.x/radiusLoaded;
  } else
  {
    // Not powered
    forceEngineTC.z=0;
  }

#ifdef OBS_PACEJKA
  // Calc long. force as a function of slip ratio (road reaction)
  if(slipRatio>=0.0)
  {
    // Traction
    coeff=crvSlipTraction->GetValue(slipRatio);
//qdbg("SR>=0: coeff=%f\n",coeff);
  } else
  {
    // Braking (notice the curve's X axis is negated)
    coeff=-crvSlipBraking->GetValue(-slipRatio);
//qdbg("SR(%f)<0: coeff=%f\n",slipRatio,coeff);
  }
  // Calculate actual road reaction force based on coefficient
  // and friction coefficient of current surface.
  // The force will be in the direction of the movement of the tire
  // (or reversed when braking, because coeff<0)
  if(velWheelTC.z<0)
  { // The tire is moving in reverse direction
    forceRoadTC.z=-coeff*frictionDefault*load;
  } else
  { // Regular forward motion of the tire
    forceRoadTC.z=coeff*frictionDefault*load;
  }
#endif

  // Calculate longitudinal force using Pacejka's magic formula
  rfloat pf;
  pacejka.SetCamber(0);
  pacejka.SetSlipAngle(-slipAngle);
  pacejka.SetSlipRatio(slipRatio);
  pacejka.SetNormalForce(forceRoadTC.y);
  pf=pacejka.CalcFx();
//qdbg("Curve Fx=%f, Pacejka Fx=%f\n",forceRoadTC.z,pf);
#ifdef OBS
if(this==car->GetWheel(2))
{ qdbg("Pacejka Fx=%.2f, nrmF=%f, SR=%f, SA=%f\n",
  pf,forceRoadTC.y,slipRatio,slipAngle);
}
#endif
  if(velWheelTC.z<0)
  { // The tire is moving in reverse direction
    forceRoadTC.z=-pf;
  } else
  { // Regular forward motion of the tire
    forceRoadTC.z=pf;
  }

//qdbg("RWh:CF; coeff=%f, load=%f\n",coeff,load);

//qdbg("RWh:CF; Engine force=%f, road reaction=%f\n",
//forceEngineTC.z,forceRoadTC.z);

  // Calculate braking torque
  // Note this re-uses torqueTC, which can't be used further on to base
  // engine+braking on
  rfloat curBrakingTorque;
  curBrakingTorque=((rfloat)car->GetBraking())*maxBrakingTorque/1000.0f;
  // Apply brake balance factor (to be able to get braking balance front/rear)
  curBrakingTorque*=brakingFactor;
  // Apply braking torque in the reverse direction of the wheel's rotation
  if(rotationV.x>0)
  { torqueTC.x=curBrakingTorque;
  } else
  { torqueTC.x=curBrakingTorque;
  }
  forceBrakingTC.z=torqueTC.x/radiusLoaded;
//qdbg("RWh:CF; curBrakingTorque=%f\n",curBrakingTorque);

  // LATERAL FORCES

#ifdef OBS_PACEJKA
  // Calc lateral force as a function of slip angle
  // Curve is only defined for positive slip angles
  if(slipAngle>=0.0)
  {
    coeff=crvLatForce->GetValue(slipAngle*RR_RAD_DEG_FACTOR);
  } else
  {
    coeff=-crvLatForce->GetValue(-slipAngle*RR_RAD_DEG_FACTOR);
  }
  // Hmm, include frictionDefault factor??
  forceRoadTC.x=-coeff*frictionDefault*load;
//qdbg("RWh:CF; slipAngle=%.2f deg, load=%f, Flat=%.2f\n",
//slipAngle*RR_RAD_DEG_FACTOR,load,forceRoadTC.x);
#endif

  pf=pacejka.CalcFy();
//qdbg("Curve Fy=%f, Pacejka Fy=%f\n",forceRoadTC.x,pf);
  forceRoadTC.x=pf;

#ifdef DO_CAP_FRICTION
  // Friction circle may not be exceeded
  CapFrictionCircle();
#endif

#ifdef DO_LOW_SPEED
  // Low speed numerical instability patching
  if(IsLowSpeed())
  {
//qdbg("LS: factor=%f\n",lowSpeedFactor);
    // Long. force is often too high at low speed
    //forceRoadTC.z*=lowSpeedFactor;
  }
#endif

#ifdef OBS
qdbg("RWh: torqueTC.x=%f, fEngine(TC)=%f,%f,%f\n",
torqueTC.x,forceEngineTC.x,forceEngineTC.y,forceEngineTC.z);
qdbg("     fRoad(TC)=%f,%f,%f\n",
forceRoadTC.x,forceRoadTC.y,forceRoadTC.z);
#endif
  
  // WHEEL LOCKING
  
  if(slipRatio<0)
  {
    DVector3 forceLockedCC,forceLockedTC;
    rfloat slideFactor,oneMinusSlideFactor,lenSlip,lenNormal,x,y,z;
    
    // As the wheel is braked, more and more sliding kicks in.
    // At the moment of 100% slide, the braking force points
    // in the reverse direction of the slip velocity. Inbetween
    // SR=0 and SR=-1 (and beyond), there is more and more sliding,
    // so the force begins to point more and more like the slip vel.
    
    // Calculate sliding factor (0..1)
    if(slipRatio<-1.0f)slideFactor=1.0f;
    else               slideFactor=-slipRatio;
//slideFactor*=.75f;
    oneMinusSlideFactor=1.0f-slideFactor;
    // Calculate 100% wheel lock direction
    forceLockedCC.x=slipVectorCC.x;
    forceLockedCC.y=0;
    forceLockedCC.z=slipVectorCC.z;
    // Make it match forceRoadTC's coordinate system
    ConvertCarToTireOrientation(&forceLockedCC,&forceLockedTC);
    // Calc magnitude of normal and locked forces
    lenSlip=forceLockedTC.Length();
    y=forceRoadTC.y; forceRoadTC.y=0;
    lenNormal=forceRoadTC.Length();
    forceRoadTC.y=y;
    if(lenSlip<D3_EPSILON)
    {
      // No force
      forceLockedTC.SetToZero();
    } else
    {
      // Equalize force magnitude
      forceLockedTC.Scale(lenNormal/lenSlip);
    }
#ifdef OBS
//if(this==car->GetWheel(2))
{forceRoadTC.DbgPrint("forceRoadTC");
forceLockedCC.DbgPrint("forceLockedCC");
qdbg("lenSlip=%f, lenNormal=%f, slideFactor=%f\n",
lenSlip,lenNormal,slideFactor);
}
#endif
    // Interpolate between both extreme forces
    forceRoadTC.x=oneMinusSlideFactor*forceRoadTC.x+
                  slideFactor*forceLockedTC.x;
    forceRoadTC.z=oneMinusSlideFactor*forceRoadTC.z+
                  slideFactor*forceLockedTC.z;
  }
  
  // ALIGNING TORQUE
  
  aligningTorque=pacejka.CalcMz();
  if(IsLowSpeed())
  {
    aligningTorque*=lowSpeedFactor;
  }
}

void RWheel::CalcTorqueForces(rfloat torque)
// OBS
// Apply torque (motor) to get a force that wants to rotate the wheel.
// The force on the ground may results in a friction force
// that pushes the car forward, rotating the wheels etc.
// Calc's forceTorque in car coords, forceFriction in car coords
{
}

void RWheel::CalcSuspForces()
// OBS
// Calculate suspension forces
{
#ifdef OBS
  DVector3 forceGravityWC,forceGroundWC;
  
#ifdef OBS
  // Let suspension calculate its forces
  susp->CalcForces();
#endif
  
  // Gravity
  forceGravityWC.x=0;
  forceGravityWC.y=-GetMass()*RMGR->scene->env->GetGravity();
  forceGravityWC.z=0;
  
//qdbg("RWh:CSF:  IsOnSurface: %d\n",IsOnSurface());
  // Total force
  if(IsOnSurface())
  { // Ground pushes up as hard as all forces pushing down
    // This is NOT true if we are on a slope. (future)
    // Then the vertical cc force gets a direction (of the slope)
    forceGroundWC=-forceGravityWC-*susp->GetForceWheel();
  } else
  {
    // Gravity has free play; direction is down (Y-)
    //forceGroundWC.SetToZero();
  }
  
  // Total force
  forceVerticalCC=*susp->GetForceWheel()+forceGravityWC+forceGroundWC;
qdbg("RW:CSF: suspF.y=%f, gravity=%f, ground=%f, total=%f\n",
susp->GetForceWheel()->y,forceGravityWC.y,forceGroundWC.y,
forceVerticalCC.y);
#endif
}

/******************
* Applying forces *
******************/
void RWheel::CalcWheelAngAcc()
// Calculate wheel angular acceleration
{
  rfloat force,inertia;
  
  // Wheel rotation
  // Uses the engine torque minus the road reaction torque
  // Should also include braking torque
  force=forceEngineTC.z+forceRoadTC.z+forceBrakingTC.z;

  // Apply force at contact patch, making the wheel rotate
  inertia=GetMass()*radius*radius/2;     // 0,5*m*r^2 (cylinder equal mass)
//qdbg("inertia prev.=%f\n",inertia);
  inertia=car->GetEngine()->GetInertiaForWheel(this);
//qdbg("  inertia with clutch=%f\n",inertia);
  //inertia=GetMass()*radius*radius;     // m*r^2 (mass at outside of wheel)
  rotationA.x=-(force*radiusLoaded)/inertia;

#ifdef OBS
if(this==car->GetWheel(2))
{qdbg("AF: forceEngine.z=%f, roadZ=%f, brakingZ=%f\n",
forceEngineTC.z,forceRoadTC.z,forceBrakingTC.z);
qdbg("AF: netCPforce.z(TC)=%f, inertia=%.2f, rotA.x=%.2f, mass=%.1f\n",
force,inertia,rotationA.x,mass);
}
qdbg("RWh:AF; rotationA=%.2f,%.2f,%.2f\n",
rotationA.x,rotationA.y,rotationA.z);
#endif
}
void RWheel::CalcBodyForce()
// Calculate force for body acceleration
{
  DVector3 forceBodyTC;
  
  // The force on the body is exactly the road force
  forceBodyTC=forceRoadTC;
  
#ifdef OBS
qdbg("RWh:AF; forceBodyTC=%.2f,%.2f,%.2f\n",
forceBodyTC.x,forceBodyTC.y,forceBodyTC.z);
#endif
  // Convert to car coordinates
  ConvertTireToCarOrientation(&forceBodyTC,&forceBodyCC);
//qdbg("RWh:AF; forceBodyCC=%.2f,%.2f,%.2f\n",
//forceBodyCC.x,forceBodyCC.y,forceBodyCC.z);
}
void RWheel::ApplyForces()
// Apply the forces to get the net results
// Leaves 'forceBody' for the car's acceleration (CG)
{
  rotationA.SetToZero();
  
  CalcWheelAngAcc();
  CalcBodyForce();

  // Vertical forces; suspension, gravity, ground, tire rate
  //acceleration=forceVerticalCC/GetMass();
  acceleration.x=0.0f;
  acceleration.y=forceVerticalCC.y/GetMass();
  acceleration.z=0.0f;
#ifdef OBS
qdbg("RWh:AF; forceVerticalCC.y=%f, mass=%f\n",
forceVerticalCC.y,GetMass());
qdbg("RWh:AF; acceleration=%.2f,%.2f,%.2f\n",
acceleration.x,acceleration.y,acceleration.z);
#endif

#ifdef ND_NO_COLL
  if(IsOnSurface())
  {
    // Make sure the tire doesn't go through the ground
    // A simple collision detection with the ground
  }
#endif
//qdbg("RWh:AF; acc=%f,%f,%f\n",acceleration.x,acceleration.y,acceleration.z);
}

void RWheel::AdjustCoupling()
// OBS
// Hierarchy of car is not what it's supposed to be
{
}

/*********************
* Coordinate systems *
*********************/
void RWheel::ConvertCarToTireOrientation(DVector3 *from,DVector3 *to)
// Convert vector 'from' from car coordinates
// to tire coordinates (into 'to')
// Assumes: from!=to
{
  rfloat angle,sinAngle,cosAngle;
  
  // Heading
  angle=-GetHeading();
  sinAngle=sin(angle);
  cosAngle=cos(angle);
  
  // Rotate around Y axis to get heading of car right
  to->x=from->z*sinAngle+from->x*cosAngle;
  to->y=from->y;
  to->z=from->z*cosAngle-from->x*sinAngle;
}
void RWheel::ConvertCarToTireCoords(DVector3 *from,DVector3 *to)
{
  ConvertCarToTireOrientation(from,to);
  // Translate
  //...
  qerr("RWheel:ConvertCarToTireCoords NYI");
}
void RWheel::ConvertTireToCarOrientation(DVector3 *from,DVector3 *to)
// Convert vector 'from' from tire coordinates
// to car coordinates (into 'to')
// Assumes: from!=to
{
  rfloat angle,sinAngle,cosAngle;
  
  // Note that the tire is constrained in 5 dimensions, so only
  // 1 rotation needs to be done

  // Heading
  angle=GetHeading();
  sinAngle=sin(angle);
  cosAngle=cos(angle);
  
  // Rotate around Y axis to get heading of car right
  to->x=from->z*sinAngle+from->x*cosAngle;
  to->y=from->y;
  to->z=from->z*cosAngle-from->x*sinAngle;
}
void RWheel::ConvertTireToCarCoords(DVector3 *from,DVector3 *to)
{
  ConvertTireToCarOrientation(from,to);
  // Translate
  //...
  qerr("RWheel:ConvertTireToCarCoords NYI");
}