public virtual void UpdateFriction(float timeStep)
{
//calculate the impulse, so that the wheels don't move sidewards
int numWheel = GetNumWheels();
if (numWheel == 0)
{
return;
}
//m_forwardWS.resize(numWheel);
//m_axle.resize(numWheel);
//m_forwardImpulse.resize(numWheel);
//m_sideImpulse.resize(numWheel);
int numWheelsOnGround = 0;
//collapse all those loops into one!
for (int i = 0; i < numWheel; i++)
{
WheelInfo wheelInfo = m_wheelInfo[i];
RigidBody groundObject = wheelInfo.m_raycastInfo.m_groundObject as RigidBody;
if (groundObject != null)
{
numWheelsOnGround++;
}
m_sideImpulse[i] = 0f;
m_forwardImpulse[i] = 0f;
}
//f (numWheelsOnGround != 4)
//
// int ibreak = 0;
//
{
//foreach(WheelInfo wheelInfo in m_wheelInfo)
for (int i = 0; i < numWheel; ++i)
{
WheelInfo wheelInfo = m_wheelInfo[i];
RigidBody groundObject = wheelInfo.m_raycastInfo.m_groundObject as RigidBody;
if (groundObject != null)
{
IndexedMatrix wheelTrans = GetWheelTransformWS(i);
IndexedBasisMatrix wheelBasis0 = wheelTrans._basis;
m_axle[i] = new IndexedVector3(
wheelBasis0._el0[m_indexRightAxis],
wheelBasis0._el1[m_indexRightAxis],
wheelBasis0._el2[m_indexRightAxis]);
IndexedVector3 surfNormalWS = wheelInfo.m_raycastInfo.m_contactNormalWS;
float proj = IndexedVector3.Dot(m_axle[i], surfNormalWS);
m_axle[i] -= surfNormalWS * proj;
m_axle[i].Normalize();
m_forwardWS[i] = IndexedVector3.Cross(surfNormalWS, m_axle[i]);
m_forwardWS[i].Normalize();
IndexedVector3 tempAxle = m_axle[i];
float tempImpulse = m_sideImpulse[i];
ContactConstraint.ResolveSingleBilateral(m_chassisBody, ref wheelInfo.m_raycastInfo.m_contactPointWS,
groundObject, ref wheelInfo.m_raycastInfo.m_contactPointWS,
0f, ref tempAxle, ref tempImpulse, timeStep);
m_sideImpulse[i] = (tempImpulse * sideFrictionStiffness2);
}
}
}
float sideFactor = 1f;
float fwdFactor = 0.5f;
bool sliding = false;
{
for (int wheel = 0; wheel < numWheel; wheel++)
{
WheelInfo wheelInfo = m_wheelInfo[wheel];
RigidBody groundObject = wheelInfo.m_raycastInfo.m_groundObject as RigidBody;
float rollingFriction = 0f;
if (groundObject != null)
{
if (wheelInfo.m_engineForce != 0.0f)
{
rollingFriction = wheelInfo.m_engineForce * timeStep;
}
else
{
float defaultRollingFrictionImpulse = 0f;
float maxImpulse = (wheelInfo.m_brake != 0f) ? wheelInfo.m_brake : defaultRollingFrictionImpulse;
IndexedVector3 tempWheel = m_forwardWS[wheel];
WheelContactPoint contactPt = new WheelContactPoint(m_chassisBody, groundObject, ref wheelInfo.m_raycastInfo.m_contactPointWS, ref tempWheel, maxImpulse);
m_forwardWS[wheel] = tempWheel;
rollingFriction = CalcRollingFriction(contactPt);
}
}
//switch between active rolling (throttle), braking and non-active rolling friction (no throttle/break)
m_forwardImpulse[wheel] = 0f;
m_wheelInfo[wheel].m_skidInfo = 1f;
if (groundObject != null)
{
m_wheelInfo[wheel].m_skidInfo = 1f;
float maximp = wheelInfo.m_wheelsSuspensionForce * timeStep * wheelInfo.m_frictionSlip;
float maximpSide = maximp;
float maximpSquared = maximp * maximpSide;
m_forwardImpulse[wheel] = rollingFriction; //wheelInfo.m_engineForce* timeStep;
float x = (m_forwardImpulse[wheel]) * fwdFactor;
float y = (m_sideImpulse[wheel]) * sideFactor;
float impulseSquared = (x * x + y * y);
if (impulseSquared > maximpSquared)
{
sliding = true;
float factor = (float)(maximp / Math.Sqrt(impulseSquared));
m_wheelInfo[wheel].m_skidInfo *= factor;
}
}
}
}
if (sliding)
{
for (int wheel = 0; wheel < numWheel; wheel++)
{
if (m_sideImpulse[wheel] != 0f)
{
if (m_wheelInfo[wheel].m_skidInfo < 1f)
{
m_forwardImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
m_sideImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
}
}
}
}
// apply the impulses
{
for (int wheel = 0; wheel < numWheel; wheel++)
{
WheelInfo wheelInfo = m_wheelInfo[wheel];
IndexedVector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS -
m_chassisBody.GetCenterOfMassPosition();
//if (m_forwardImpulse[wheel] > 5f || m_sideImpulse[wheel] > 5f)
//{
// int ibreak = 0;
//}
if (m_forwardImpulse[wheel] != 0f)
{
m_chassisBody.ApplyImpulse(m_forwardWS[wheel] * (m_forwardImpulse[wheel]), rel_pos);
}
if (m_sideImpulse[wheel] != 0f)
{
RigidBody groundObject = m_wheelInfo[wheel].m_raycastInfo.m_groundObject as RigidBody;
IndexedVector3 rel_pos2 = wheelInfo.m_raycastInfo.m_contactPointWS -
groundObject.GetCenterOfMassPosition();
IndexedVector3 sideImp = m_axle[wheel] * m_sideImpulse[wheel];
#if ROLLING_INFLUENCE_FIX // fix. It only worked if car's up was along Y - VT.
IndexedVector3 vChassisWorldUp = GetRigidBody().GetCenterOfMassTransform()._basis.GetColumn(m_indexUpAxis);
rel_pos -= vChassisWorldUp * (IndexedVector3.Dot(vChassisWorldUp, rel_pos) * (1.0f - wheelInfo.m_rollInfluence));
#else
rel_pos[m_indexUpAxis] *= wheelInfo.m_rollInfluence;
#endif
m_chassisBody.ApplyImpulse(ref sideImp, ref rel_pos);
//apply friction impulse on the ground
IndexedVector3 temp = -sideImp;
groundObject.ApplyImpulse(ref temp, ref rel_pos2);
}
}
}
}
public virtual void IntegrateTransforms(float timeStep)
{
BulletGlobals.StartProfile("integrateTransforms");
IndexedMatrix predictedTrans;
int length = m_nonStaticRigidBodies.Count;
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugDiscreteDynamicsWorld)
{
BulletGlobals.g_streamWriter.WriteLine("IntegrateTransforms [{0}]", length);
}
#endif
for (int i = 0; i < length; ++i)
{
RigidBody body = m_nonStaticRigidBodies[i];
if (body != null)
{
body.SetHitFraction(1f);
if (body.IsActive() && (!body.IsStaticOrKinematicObject()))
{
body.PredictIntegratedTransform(timeStep, out predictedTrans);
float squareMotion = (predictedTrans._origin - body.GetWorldTransform()._origin).LengthSquared();
//if (body.GetCcdSquareMotionThreshold() != 0 && body.GetCcdSquareMotionThreshold() < squareMotion)
if (GetDispatchInfo().m_useContinuous&& body.GetCcdSquareMotionThreshold() != 0.0f && body.GetCcdSquareMotionThreshold() < squareMotion)
{
BulletGlobals.StartProfile("CCD motion clamping");
if (body.GetCollisionShape().IsConvex())
{
gNumClampedCcdMotions++;
using (ClosestNotMeConvexResultCallback sweepResults = BulletGlobals.ClosestNotMeConvexResultCallbackPool.Get())
{
sweepResults.Initialize(body, body.GetWorldTransform()._origin, predictedTrans._origin, GetBroadphase().GetOverlappingPairCache(), GetDispatcher());
//btConvexShape* convexShape = static_cast<btConvexShape*>(body.GetCollisionShape());
SphereShape tmpSphere = BulletGlobals.SphereShapePool.Get();
tmpSphere.Initialize(body.GetCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body.GetCollisionShape());
sweepResults.m_allowedPenetration = GetDispatchInfo().GetAllowedCcdPenetration();
sweepResults.m_collisionFilterGroup = body.GetBroadphaseProxy().m_collisionFilterGroup;
sweepResults.m_collisionFilterMask = body.GetBroadphaseProxy().m_collisionFilterMask;
IndexedMatrix modifiedPredictedTrans = predictedTrans;
modifiedPredictedTrans._basis = body.GetWorldTransform()._basis;
modifiedPredictedTrans._origin = predictedTrans._origin;
ConvexSweepTest(tmpSphere, body.GetWorldTransform(), modifiedPredictedTrans, sweepResults, 0f);
if (sweepResults.HasHit() && (sweepResults.m_closestHitFraction < 1.0f))
{
//printf("clamped integration to hit fraction = %f\n",fraction);
body.SetHitFraction(sweepResults.m_closestHitFraction);
body.PredictIntegratedTransform(timeStep * body.GetHitFraction(), out predictedTrans);
body.SetHitFraction(0.0f);
body.ProceedToTransform(ref predictedTrans);
#if false
btVector3 linVel = body.getLinearVelocity();
float maxSpeed = body.getCcdMotionThreshold() / getSolverInfo().m_timeStep;
float maxSpeedSqr = maxSpeed * maxSpeed;
if (linVel.LengthSquared() > maxSpeedSqr)
{
linVel.normalize();
linVel *= maxSpeed;
body.setLinearVelocity(linVel);
float ms2 = body.getLinearVelocity().LengthSquared();
body.predictIntegratedTransform(timeStep, predictedTrans);
float sm2 = (predictedTrans._origin - body.GetWorldTransform()._origin).LengthSquared();
float smt = body.getCcdSquareMotionThreshold();
printf("sm2=%f\n", sm2);
}
#else
//response between two dynamic objects without friction, assuming 0 penetration depth
float appliedImpulse = 0.0f;
float depth = 0.0f;
appliedImpulse = ContactConstraint.ResolveSingleCollision(body, sweepResults.m_hitCollisionObject, ref sweepResults.m_hitPointWorld, ref sweepResults.m_hitNormalWorld, GetSolverInfo(), depth);
#endif
continue;
}
BulletGlobals.SphereShapePool.Free(tmpSphere);
}
}
BulletGlobals.StopProfile();
}
body.ProceedToTransform(ref predictedTrans);
}
}
}
}
