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(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);
}
public unsafe static float ComputeImpulseDenominator(this RigidBody obj, ref OpenTK.Vector3 pos, ref OpenTK.Vector3 normal)
{
fixed(OpenTK.Vector3 *posPtr = &pos)
{
fixed(OpenTK.Vector3 *normalPtr = &normal)
{
return(obj.ComputeImpulseDenominator(ref *(BulletSharp.Math.Vector3 *)posPtr, ref *(BulletSharp.Math.Vector3 *)normalPtr));
}
}
}
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;
}
