///bilateral constraint between two dynamic objects
///positive distance = separation, negative distance = penetration
public static void ResolveSingleBilateral(RigidBody body1, ref Vector3 pos1,
RigidBody body2, ref Vector3 pos2,
float distance, ref Vector3 normal, ref float impulse, float timeStep)
{
float normalLenSqr = normal.LengthSquared();
Debug.Assert(System.Math.Abs(normalLenSqr) < 1.1f);
if (normalLenSqr > 1.1f)
{
impulse = 0f;
return;
}
Vector3 rel_pos1 = pos1 - body1.GetCenterOfMassPosition();
Vector3 rel_pos2 = pos2 - body2.GetCenterOfMassPosition();
//this jacobian entry could be re-used for all iterations
Vector3 vel1 = body1.GetVelocityInLocalPoint(ref rel_pos1);
Vector3 vel2 = body2.GetVelocityInLocalPoint(ref rel_pos2);
Vector3 vel = vel1 - vel2;
Matrix m1 = MathUtil.TransposeBasis(body1.GetCenterOfMassTransform());
Matrix m2 = MathUtil.TransposeBasis(body2.GetCenterOfMassTransform());
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(),Vector3.TransformNormal(body1.GetAngularVelocity(),m1),
body2.GetLinearVelocity(),Vector3.TransformNormal(body2.GetAngularVelocity(),m2));
float a = jacDiagABInv;
rel_vel = Vector3.Dot(normal,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 float SolveLinearAxis(
float timeStep,
float jacDiagABInv,
RigidBody body1, ref Vector3 pointInA,
RigidBody body2, ref Vector3 pointInB,
int limit_index,
ref Vector3 axis_normal_on_a,
ref Vector3 anchorPos)
{
///find relative velocity
// Vector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition();
// Vector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition();
Vector3 rel_pos1 = anchorPos - body1.GetCenterOfMassPosition();
Vector3 rel_pos2 = anchorPos - body2.GetCenterOfMassPosition();
Vector3 vel1 = Vector3.Zero;
body1.InternalGetVelocityInLocalPointObsolete(ref rel_pos1,ref vel1);
Vector3 vel2 = Vector3.Zero;;
body2.InternalGetVelocityInLocalPointObsolete(ref rel_pos2,ref vel2);
Vector3 vel = vel1 - vel2;
float rel_vel = Vector3.Dot(axis_normal_on_a,vel);
/// apply displacement correction
//positional error (zeroth order error)
float depth = -Vector3.Dot((pointInA - pointInB),axis_normal_on_a);
float lo = float.MinValue;
float hi = float.MaxValue;
float minLimit = MathUtil.VectorComponent(ref m_lowerLimit,limit_index);
float maxLimit = MathUtil.VectorComponent(ref 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 = MathUtil.VectorComponent(ref m_accumulatedImpulse,limit_index);
float sum = oldNormalImpulse + normalImpulse;
MathUtil.VectorComponent(ref m_accumulatedImpulse,limit_index,(sum > hi ? 0f: sum < lo ? 0f : sum));
normalImpulse = MathUtil.VectorComponent(ref m_accumulatedImpulse,limit_index) - oldNormalImpulse;
Vector3 impulse_vector = axis_normal_on_a * normalImpulse;
//body1.applyImpulse( impulse_vector, rel_pos1);
//body2.applyImpulse(-impulse_vector, rel_pos2);
Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos1,axis_normal_on_a);
Vector3 ftorqueAxis2 = Vector3.Cross(rel_pos2,axis_normal_on_a);
body1.InternalApplyImpulse(axis_normal_on_a*body1.GetInvMass(), Vector3.TransformNormal(ftorqueAxis1,body1.GetInvInertiaTensorWorld()),normalImpulse);
body2.InternalApplyImpulse(axis_normal_on_a * body2.GetInvMass(), Vector3.TransformNormal(ftorqueAxis2,body2.GetInvInertiaTensorWorld()), -normalImpulse);
return normalImpulse;
}
public void UpdateWheel(RigidBody chassis, ref WheelRaycastInfo raycastInfo)
{
if (m_raycastInfo.m_isInContact)
{
float project= Vector3.Dot(m_raycastInfo.m_contactNormalWS,m_raycastInfo.m_wheelDirectionWS );
Vector3 chassis_velocity_at_contactPoint;
Vector3 relpos = m_raycastInfo.m_contactPointWS - chassis.GetCenterOfMassPosition();
chassis_velocity_at_contactPoint = chassis.GetVelocityInLocalPoint( ref relpos );
float projVel = Vector3.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;
}
}