public void UpdateWheel(RigidBody chassis, RaycastInfo raycastInfo)
{
if (raycastInfo.IsInContact)
{
float project = Vector3.Dot(raycastInfo.ContactNormalWS, raycastInfo.WheelDirectionWS);
Vector3 chassis_velocity_at_contactPoint;
Vector3 relpos = raycastInfo.ContactPointWS - chassis.CenterOfMassPosition;
chassis_velocity_at_contactPoint = chassis.GetVelocityInLocalPoint(relpos);
float projVel = Vector3.Dot(raycastInfo.ContactNormalWS, chassis_velocity_at_contactPoint);
if (project >= -0.1f)
{
SuspensionRelativeVelocity = 0;
ClippedInvContactDotSuspension = 1.0f / 0.1f;
}
else
{
float inv = -1.0f / project;
SuspensionRelativeVelocity = projVel * inv;
ClippedInvContactDotSuspension = inv;
}
}
else // Not in contact : position wheel in a nice (rest length) position
{
RaycastInfo.SuspensionLength = SuspensionRestLength;
SuspensionRelativeVelocity = 0;
RaycastInfo.ContactNormalWS = -raycastInfo.WheelDirectionWS;
ClippedInvContactDotSuspension = 1.0f;
}
}
public void UpdateWheel(RigidBody chassis, RaycastInfo raycastInfo)
{
if (raycastInfo.IsInContact)
{
float project = Vector3.Dot(raycastInfo.ContactNormalWS, raycastInfo.WheelDirectionWS);
Vector3 chassis_velocity_at_contactPoint;
Vector3 relpos = raycastInfo.ContactPointWS - chassis.CenterOfMassPosition;
chassis_velocity_at_contactPoint = chassis.GetVelocityInLocalPoint(relpos);
float projVel = Vector3.Dot(raycastInfo.ContactNormalWS, chassis_velocity_at_contactPoint);
if (project >= -0.1f)
{
SuspensionRelativeVelocity = 0;
ClippedInvContactDotSuspension = 1.0f / 0.1f;
}
else
{
float inv = -1.0f / project;
SuspensionRelativeVelocity = projVel * inv;
ClippedInvContactDotSuspension = inv;
}
}
else // Not in contact : position wheel in a nice (rest length) position
{
RaycastInfo.SuspensionLength = SuspensionRestLength;
SuspensionRelativeVelocity = 0;
RaycastInfo.ContactNormalWS = -raycastInfo.WheelDirectionWS;
ClippedInvContactDotSuspension = 1.0f;
}
}
public unsafe static OpenTK.Vector3 GetVelocityInLocalPoint(this RigidBody obj, ref OpenTK.Vector3 rel_pos)
{
fixed(OpenTK.Vector3 *rel_posPtr = &rel_pos)
{
return(obj.GetVelocityInLocalPoint(ref *(BulletSharp.Math.Vector3 *)rel_posPtr).ToOpenTK());
}
}
double CalcRollingFriction(RigidBody body0, RigidBody body1, Vector3 contactPosWorld, Vector3 frictionDirectionWorld, double maxImpulse)
{
double denom0 = body0.ComputeImpulseDenominator(contactPosWorld, frictionDirectionWorld);
double denom1 = body1.ComputeImpulseDenominator(contactPosWorld, frictionDirectionWorld);
const double relaxation = 1.0f;
double jacDiagABInv = relaxation / (denom0 + denom1);
double j1;
Vector3 rel_pos1 = contactPosWorld - body0.CenterOfMassPosition;
Vector3 rel_pos2 = contactPosWorld - body1.CenterOfMassPosition;
Vector3 vel1 = body0.GetVelocityInLocalPoint(rel_pos1);
Vector3 vel2 = body1.GetVelocityInLocalPoint(rel_pos2);
Vector3 vel = vel1 - vel2;
double vrel;
Vector3.Dot(ref frictionDirectionWorld, ref vel, out vrel);
// calculate j that moves us to zero relative velocity
j1 = -vrel * jacDiagABInv;
j1 = System.Math.Min(j1, maxImpulse);
j1 = System.Math.Max(j1, -maxImpulse);
return(j1);
}
private void ResolveSingleBilateral(RigidBody body1, Vector3 pos1, RigidBody body2, Vector3 pos2, float distance, Vector3 normal, ref float impulse, float timeStep)
{
float normalLenSqr = normal.LengthSquared;
Debug.Assert(System.Math.Abs(normalLenSqr) < 1.1f);
if (normalLenSqr > 1.1f)
{
impulse = 0;
return;
}
Vector3 rel_pos1 = pos1 - body1.CenterOfMassPosition;
Vector3 rel_pos2 = pos2 - body2.CenterOfMassPosition;
Vector3 vel1 = body1.GetVelocityInLocalPoint(rel_pos1);
Vector3 vel2 = body2.GetVelocityInLocalPoint(rel_pos2);
Vector3 vel = vel1 - vel2;
Matrix world2A = Matrix.Transpose(body1.CenterOfMassTransform.Basis);
Matrix world2B = Matrix.Transpose(body2.CenterOfMassTransform.Basis);
Vector3 m_aJ = Vector3.TransformCoordinate(Vector3.Cross(rel_pos1, normal), world2A);
Vector3 m_bJ = Vector3.TransformCoordinate(Vector3.Cross(rel_pos2, -normal), world2B);
Vector3 m_0MinvJt = body1.InvInertiaDiagLocal * m_aJ;
Vector3 m_1MinvJt = body2.InvInertiaDiagLocal * m_bJ;
float dot0, dot1;
Vector3.Dot(ref m_0MinvJt, ref m_aJ, out dot0);
Vector3.Dot(ref m_1MinvJt, ref m_bJ, out dot1);
float jacDiagAB = body1.InvMass + dot0 + body2.InvMass + dot1;
float jacDiagABInv = 1.0f / jacDiagAB;
float rel_vel;
Vector3.Dot(ref normal, ref vel, out rel_vel);
//todo: move this into proper structure
const float contactDamping = 0.2f;
#if ONLY_USE_LINEAR_MASS
float massTerm = 1.0f / (body1.InvMass + body2.InvMass);
impulse = -contactDamping * rel_vel * massTerm;
#else
float velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
impulse = velocityImpulse;
#endif
}
private void ResolveSingleBilateral(RigidBody body1, Vector3 pos1, RigidBody body2, Vector3 pos2, float distance, Vector3 normal, ref float impulse, float timeStep)
{
float normalLenSqr = normal.LengthSquared();
Debug.Assert(Math.Abs(normalLenSqr) < 1.1f);
if (normalLenSqr > 1.1f)
{
impulse = 0;
return;
}
Vector3 rel_pos1 = pos1 - body1.CenterOfMassPosition;
Vector3 rel_pos2 = pos2 - body2.CenterOfMassPosition;
Vector3 vel1 = body1.GetVelocityInLocalPoint(rel_pos1);
Vector3 vel2 = body2.GetVelocityInLocalPoint(rel_pos2);
Vector3 vel = vel1 - vel2;
Matrix world2A = body1.CenterOfMassTransform;
world2A.Origin = Vector3.Zero;
world2A = Matrix.Transpose(world2A);
Matrix world2B = body2.CenterOfMassTransform;
world2B.Origin = Vector3.Zero;
world2B = Matrix.Transpose(world2B);
Vector3 m_aJ = Vector3.TransformCoordinate(Vector3.Cross(rel_pos1, normal), world2A);
Vector3 m_bJ = Vector3.TransformCoordinate(Vector3.Cross(rel_pos2, -normal), world2B);
Vector3 m_0MinvJt = body1.InvInertiaDiagLocal * m_aJ;
Vector3 m_1MinvJt = body2.InvInertiaDiagLocal * m_bJ;
float jacDiagAB = body1.InvMass + Vector3.Dot(m_0MinvJt, m_aJ) + body2.InvMass + Vector3.Dot(m_1MinvJt, m_bJ);
float jacDiagABInv = 1.0f / jacDiagAB;
float rel_vel = Vector3.Dot(normal, vel);
//todo: move this into proper structure
const float contactDamping = 0.2f;
#if ONLY_USE_LINEAR_MASS
float massTerm = 1.0f / (body1.InvMass + body2.InvMass);
impulse = - contactDamping * rel_vel * massTerm;
#else
float velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
impulse = velocityImpulse;
#endif
}
float CalcRollingFriction(RigidBody body0, RigidBody body1, Vector3 contactPosWorld, Vector3 frictionDirectionWorld, float maxImpulse)
{
float denom0 = body0.ComputeImpulseDenominator(contactPosWorld, frictionDirectionWorld);
float denom1 = body1.ComputeImpulseDenominator(contactPosWorld, frictionDirectionWorld);
const float relaxation = 1.0f;
float jacDiagABInv = relaxation / (denom0 + denom1);
float j1;
Vector3 rel_pos1 = contactPosWorld - body0.CenterOfMassPosition;
Vector3 rel_pos2 = contactPosWorld - body1.CenterOfMassPosition;
Vector3 vel1 = body0.GetVelocityInLocalPoint(rel_pos1);
Vector3 vel2 = body1.GetVelocityInLocalPoint(rel_pos2);
Vector3 vel = vel1 - vel2;
float vrel = Vector3.Dot(frictionDirectionWorld, vel);
// calculate j that moves us to zero relative velocity
j1 = -vrel * jacDiagABInv;
j1 = Math.Min(j1, maxImpulse);
j1 = Math.Max(j1, -maxImpulse);
return j1;
}