Vehicle/Rigid Collision Fixes

bool Rigid::resolveCollision(const Point3F& p, Point3F normal, Rigid* rigid)
{
   atRest = false;
   Point3F v1,v2,r1,r2;
   getOriginVector(p,&r1);
   getVelocity(r1,&v1);
   rigid->getOriginVector(p,&r2);
   rigid->getVelocity(r2,&v2);
 
   // Make sure they are converging
   F32 nv = mDot(v1,normal);
   nv -= mDot(v2,normal);
   if (nv > -0.001f)
      return false;
 
   // Compute impulse
   F32 d, n = -nv * (1 + restitution * rigid->restitution);
   Point3F a1,b1,c1;
   mCross(r1,normal,&a1);
   invWorldInertia.mulV(a1,&b1);
   mCross(b1,r1,&c1);
 
   Point3F a2,b2,c2;
   mCross(r2,normal,&a2);
   rigid->invWorldInertia.mulV(a2,&b2);
   mCross(b2,r2,&c2);
 
   Point3F c3 = c1 + c2;
   d = oneOverMass + rigid->oneOverMass + mDot(c3,normal);
   Point3F impulse = normal * (n / d);
 
   applyImpulse(r1,impulse);
   impulse.neg();
   rigid->applyImpulse(r2,impulse);
   return true;
}
 
bool Rigid::resolveCollision(const Point3F& p, Point3F normal)
{
   atRest = false;
   Point3F v,r;
   getOriginVector(p,&r);
   getVelocity(r,&v);   
   F32 n = -mDot(v,normal);
   if ( n < 0 )
      return false;
   
   // Collision impulse, straight forward force stuff.
   F32 d = getZeroImpulse(r,normal);
   F32 j = n * (1 + restitution) * d;
   Point3F impulse = normal * j;
 
   // Friction impulse, calculated as a function of the
   // amount of force it would take to stop the motion
   // perpendicular to the normal.
   Point3F uv = v + (normal * n);
   F32 ul = uv.len();
   if (ul) {
      uv /= -ul;
      F32 u = ul * getZeroImpulse(r,uv);
      j *= friction;
      if (u > j)
         u = j;
      impulse += uv * u;
   }
 
   //
   applyImpulse(r,impulse);
 
   return true;
}