public void UpdateWheel(RigidBody chassis, ref WheelRaycastInfo raycastInfo)
{
if (m_raycastInfo.m_isInContact)
{
float project = IndexedVector3.Dot(m_raycastInfo.m_contactNormalWS, m_raycastInfo.m_wheelDirectionWS);
IndexedVector3 chassis_velocity_at_contactPoint;
IndexedVector3 relpos = m_raycastInfo.m_contactPointWS - chassis.GetCenterOfMassPosition();
chassis_velocity_at_contactPoint = chassis.GetVelocityInLocalPoint(ref relpos);
float projVel = IndexedVector3.Dot(m_raycastInfo.m_contactNormalWS, chassis_velocity_at_contactPoint);
if (project >= -0.1f)
{
m_suspensionRelativeVelocity = 0f;
m_clippedInvContactDotSuspension = 1.0f / 0.1f;
}
else
{
float inv = -1f / project;
m_suspensionRelativeVelocity = projVel * inv;
m_clippedInvContactDotSuspension = inv;
}
}
else // Not in contact : position wheel in a nice (rest length) position
{
m_raycastInfo.m_suspensionLength = this.GetSuspensionRestLength();
m_suspensionRelativeVelocity = 0f;
m_raycastInfo.m_contactNormalWS = -m_raycastInfo.m_wheelDirectionWS;
m_clippedInvContactDotSuspension = 1f;
}
}
///bilateral constraint between two dynamic objects
///positive distance = separation, negative distance = penetration
public static void ResolveSingleBilateral(RigidBody body1, ref IndexedVector3 pos1,
RigidBody body2, ref IndexedVector3 pos2,
float distance, ref IndexedVector3 normal, ref float impulse, float timeStep)
{
float normalLenSqr = normal.LengthSquared();
Debug.Assert(Math.Abs(normalLenSqr) < 1.1f);
if (normalLenSqr > 1.1f)
{
impulse = 0f;
return;
}
IndexedVector3 rel_pos1 = pos1 - body1.GetCenterOfMassPosition();
IndexedVector3 rel_pos2 = pos2 - body2.GetCenterOfMassPosition();
//this jacobian entry could be re-used for all iterations
IndexedVector3 vel1 = body1.GetVelocityInLocalPoint(ref rel_pos1);
IndexedVector3 vel2 = body2.GetVelocityInLocalPoint(ref rel_pos2);
IndexedVector3 vel = vel1 - vel2;
IndexedBasisMatrix m1 = body1.GetCenterOfMassTransform()._basis.Transpose();
IndexedBasisMatrix m2 = body2.GetCenterOfMassTransform()._basis.Transpose();
JacobianEntry jac = new JacobianEntry(m1, m2, rel_pos1, rel_pos2, normal,
body1.GetInvInertiaDiagLocal(), body1.GetInvMass(),
body2.GetInvInertiaDiagLocal(), body2.GetInvMass());
float jacDiagAB = jac.GetDiagonal();
float jacDiagABInv = 1f / jacDiagAB;
float rel_vel = jac.GetRelativeVelocity(
body1.GetLinearVelocity(),
body1.GetCenterOfMassTransform()._basis.Transpose() * body1.GetAngularVelocity(),
body2.GetLinearVelocity(),
body2.GetCenterOfMassTransform()._basis.Transpose() * body2.GetAngularVelocity());
float a = jacDiagABInv;
rel_vel = normal.Dot(ref vel);
//todo: move this into proper structure
float contactDamping = 0.2f;
if (ONLY_USE_LINEAR_MASS)
{
float massTerm = 1f / (body1.GetInvMass() + body2.GetInvMass());
impulse = -contactDamping * rel_vel * massTerm;
}
else
{
float velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
impulse = velocityImpulse;
}
}
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);
}
}
}
}
public void UpdateWheel(RigidBody chassis, ref WheelRaycastInfo raycastInfo)
{
if (m_raycastInfo.m_isInContact)
{
float project= IndexedVector3.Dot(m_raycastInfo.m_contactNormalWS,m_raycastInfo.m_wheelDirectionWS );
IndexedVector3 chassis_velocity_at_contactPoint;
IndexedVector3 relpos = m_raycastInfo.m_contactPointWS - chassis.GetCenterOfMassPosition();
chassis_velocity_at_contactPoint = chassis.GetVelocityInLocalPoint( ref relpos );
float projVel = IndexedVector3.Dot(m_raycastInfo.m_contactNormalWS,chassis_velocity_at_contactPoint );
if ( project >= -0.1f)
{
m_suspensionRelativeVelocity = 0f;
m_clippedInvContactDotSuspension = 1.0f / 0.1f;
}
else
{
float inv = -1f / project;
m_suspensionRelativeVelocity = projVel * inv;
m_clippedInvContactDotSuspension = inv;
}
}
else // Not in contact : position wheel in a nice (rest length) position
{
m_raycastInfo.m_suspensionLength = this.GetSuspensionRestLength();
m_suspensionRelativeVelocity = 0f;
m_raycastInfo.m_contactNormalWS = -m_raycastInfo.m_wheelDirectionWS;
m_clippedInvContactDotSuspension = 1f;
}
}
public float SolveLinearAxis(
float timeStep,
float jacDiagABInv,
RigidBody body1, ref IndexedVector3 pointInA,
RigidBody body2, ref IndexedVector3 pointInB,
int limit_index,
ref IndexedVector3 axis_normal_on_a,
ref IndexedVector3 anchorPos)
{
///find relative velocity
// IndexedVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition();
// IndexedVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition();
IndexedVector3 rel_pos1 = anchorPos - body1.GetCenterOfMassPosition();
IndexedVector3 rel_pos2 = anchorPos - body2.GetCenterOfMassPosition();
IndexedVector3 vel1 = IndexedVector3.Zero;
body1.InternalGetVelocityInLocalPointObsolete(ref rel_pos1, ref vel1);
IndexedVector3 vel2 = IndexedVector3.Zero; ;
body2.InternalGetVelocityInLocalPointObsolete(ref rel_pos2, ref vel2);
IndexedVector3 vel = vel1 - vel2;
float rel_vel = IndexedVector3.Dot(axis_normal_on_a, vel);
/// apply displacement correction
//positional error (zeroth order error)
float depth = -IndexedVector3.Dot((pointInA - pointInB), axis_normal_on_a);
float lo = float.MinValue;
float hi = float.MaxValue;
float minLimit = m_lowerLimit[limit_index];
float maxLimit = m_upperLimit[limit_index];
//handle the limits
if (minLimit < maxLimit)
{
{
if (depth > maxLimit)
{
depth -= maxLimit;
lo = 0f;
}
else
{
if (depth < minLimit)
{
depth -= minLimit;
hi = 0f;
}
else
{
return 0.0f;
}
}
}
}
float normalImpulse = m_limitSoftness * (m_restitution * depth / timeStep - m_damping * rel_vel) * jacDiagABInv;
float oldNormalImpulse = m_accumulatedImpulse[limit_index];
float sum = oldNormalImpulse + normalImpulse;
m_accumulatedImpulse[limit_index] = (sum > hi ? 0f : sum < lo ? 0f : sum);
normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
IndexedVector3 impulse_vector = axis_normal_on_a * normalImpulse;
//body1.applyImpulse( impulse_vector, rel_pos1);
//body2.applyImpulse(-impulse_vector, rel_pos2);
IndexedVector3 ftorqueAxis1 = IndexedVector3.Cross(rel_pos1, axis_normal_on_a);
IndexedVector3 ftorqueAxis2 = IndexedVector3.Cross(rel_pos2, axis_normal_on_a);
body1.InternalApplyImpulse(axis_normal_on_a * body1.GetInvMass(), body1.GetInvInertiaTensorWorld() * ftorqueAxis1, normalImpulse, "Generic6DoF body1");
body2.InternalApplyImpulse(axis_normal_on_a * body2.GetInvMass(), body2.GetInvInertiaTensorWorld() * ftorqueAxis2, -normalImpulse, "Generic6DoF body2");
return normalImpulse;
}
///bilateral constraint between two dynamic objects
///positive distance = separation, negative distance = penetration
public static void ResolveSingleBilateral(RigidBody body1, ref IndexedVector3 pos1,
RigidBody body2, ref IndexedVector3 pos2,
float distance, ref IndexedVector3 normal, ref float impulse, float timeStep)
{
float normalLenSqr = normal.LengthSquared();
Debug.Assert(Math.Abs(normalLenSqr) < 1.1f);
if (normalLenSqr > 1.1f)
{
impulse = 0f;
return;
}
IndexedVector3 rel_pos1 = pos1 - body1.GetCenterOfMassPosition();
IndexedVector3 rel_pos2 = pos2 - body2.GetCenterOfMassPosition();
//this jacobian entry could be re-used for all iterations
IndexedVector3 vel1 = body1.GetVelocityInLocalPoint(ref rel_pos1);
IndexedVector3 vel2 = body2.GetVelocityInLocalPoint(ref rel_pos2);
IndexedVector3 vel = vel1 - vel2;
IndexedBasisMatrix m1 = body1.GetCenterOfMassTransform()._basis.Transpose();
IndexedBasisMatrix m2 = body2.GetCenterOfMassTransform()._basis.Transpose();
JacobianEntry jac = new JacobianEntry(m1, m2, rel_pos1, rel_pos2, normal,
body1.GetInvInertiaDiagLocal(), body1.GetInvMass(),
body2.GetInvInertiaDiagLocal(), body2.GetInvMass());
float jacDiagAB = jac.GetDiagonal();
float jacDiagABInv = 1f / jacDiagAB;
float rel_vel = jac.GetRelativeVelocity(
body1.GetLinearVelocity(),
body1.GetCenterOfMassTransform()._basis.Transpose() * body1.GetAngularVelocity(),
body2.GetLinearVelocity(),
body2.GetCenterOfMassTransform()._basis.Transpose() * body2.GetAngularVelocity());
float a = jacDiagABInv;
rel_vel = normal.Dot(ref vel);
//todo: move this into proper structure
float contactDamping = 0.2f;
if (ONLY_USE_LINEAR_MASS)
{
float massTerm = 1f / (body1.GetInvMass() + body2.GetInvMass());
impulse = -contactDamping * rel_vel * massTerm;
}
else
{
float velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
impulse = velocityImpulse;
}
}