//response between two dynamic objects without friction, assuming 0 penetration depth
public static float ResolveSingleCollision(
RigidBody body1,
CollisionObject colObj2,
ref IndexedVector3 contactPositionWorld,
ref IndexedVector3 contactNormalOnB,
ContactSolverInfo solverInfo,
float distance)
{
RigidBody body2 = RigidBody.Upcast(colObj2);
IndexedVector3 normal = contactNormalOnB;
IndexedVector3 rel_pos1 = contactPositionWorld - body1.GetWorldTransform()._origin;
IndexedVector3 rel_pos2 = contactPositionWorld - colObj2.GetWorldTransform()._origin;
IndexedVector3 vel1 = body1.GetVelocityInLocalPoint(ref rel_pos1);
IndexedVector3 vel2 = body2 != null?body2.GetVelocityInLocalPoint(ref rel_pos2) : IndexedVector3.Zero;
IndexedVector3 vel = vel1 - vel2;
float rel_vel = normal.Dot(ref vel);
float combinedRestitution = body1.GetRestitution() * colObj2.GetRestitution();
float restitution = combinedRestitution * -rel_vel;
float positionalError = solverInfo.m_erp * -distance / solverInfo.m_timeStep;
float velocityError = -(1.0f + restitution) * rel_vel; // * damping;
float denom0 = body1.ComputeImpulseDenominator(ref contactPositionWorld, ref normal);
float denom1 = body2 != null?body2.ComputeImpulseDenominator(ref contactPositionWorld, ref normal) : 0.0f;
float relaxation = 1.0f;
float jacDiagABInv = relaxation / (denom0 + denom1);
float penetrationImpulse = positionalError * jacDiagABInv;
float velocityImpulse = velocityError * jacDiagABInv;
float normalImpulse = penetrationImpulse + velocityImpulse;
normalImpulse = 0.0f > normalImpulse ? 0.0f : normalImpulse;
body1.ApplyImpulse(normal * (normalImpulse), rel_pos1);
if (body2 != null)
{
body2.ApplyImpulse(-normal * (normalImpulse), rel_pos2);
}
return(normalImpulse);
}
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;
}
if (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);
}
}
}
}
//response between two dynamic objects without friction, assuming 0 penetration depth
public static float ResolveSingleCollision(
RigidBody body1,
CollisionObject colObj2,
ref IndexedVector3 contactPositionWorld,
ref IndexedVector3 contactNormalOnB,
ContactSolverInfo solverInfo,
float distance)
{
RigidBody body2 = RigidBody.Upcast(colObj2);
IndexedVector3 normal = contactNormalOnB;
IndexedVector3 rel_pos1 = contactPositionWorld - body1.GetWorldTransform()._origin;
IndexedVector3 rel_pos2 = contactPositionWorld - colObj2.GetWorldTransform()._origin;
IndexedVector3 vel1 = body1.GetVelocityInLocalPoint(ref rel_pos1);
IndexedVector3 vel2 = body2 != null ? body2.GetVelocityInLocalPoint(ref rel_pos2) : IndexedVector3.Zero;
IndexedVector3 vel = vel1 - vel2;
float rel_vel = normal.Dot(ref vel);
float combinedRestitution = body1.GetRestitution() * colObj2.GetRestitution();
float restitution = combinedRestitution * -rel_vel;
float positionalError = solverInfo.m_erp * -distance / solverInfo.m_timeStep;
float velocityError = -(1.0f + restitution) * rel_vel;// * damping;
float denom0 = body1.ComputeImpulseDenominator(ref contactPositionWorld, ref normal);
float denom1 = body2 != null ? body2.ComputeImpulseDenominator(ref contactPositionWorld, ref normal) : 0.0f;
float relaxation = 1.0f;
float jacDiagABInv = relaxation / (denom0 + denom1);
float penetrationImpulse = positionalError * jacDiagABInv;
float velocityImpulse = velocityError * jacDiagABInv;
float normalImpulse = penetrationImpulse + velocityImpulse;
normalImpulse = 0.0f > normalImpulse ? 0.0f : normalImpulse;
body1.ApplyImpulse(normal * (normalImpulse), rel_pos1);
if (body2 != null)
{
body2.ApplyImpulse(-normal * (normalImpulse), rel_pos2);
}
return normalImpulse;
}