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;
}
}
//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);
}
//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 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;
}
}
//
// solve unilateral raint (equality, direct method)
//
public void ResolveUnilateralPairConstraint(RigidBody body0, RigidBody body1, ref IndexedBasisMatrix world2A,
ref IndexedBasisMatrix world2B,
ref IndexedVector3 invInertiaADiag,
float invMassA,
ref IndexedVector3 linvelA, ref IndexedVector3 angvelA,
ref IndexedVector3 rel_posA1,
ref IndexedVector3 invInertiaBDiag,
float invMassB,
ref IndexedVector3 linvelB, ref IndexedVector3 angvelB,
ref IndexedVector3 rel_posA2,
float depthA, ref IndexedVector3 normalA,
ref IndexedVector3 rel_posB1, ref IndexedVector3 rel_posB2,
float depthB, ref IndexedVector3 normalB,
out float imp0, out float imp1)
{
//(void)linvelA;
//(void)linvelB;
//(void)angvelB;
//(void)angvelA;
imp0 = 0f;
imp1 = 0f;
float len = Math.Abs(normalA.Length()) - 1f;
if (Math.Abs(len) >= MathUtil.SIMD_EPSILON)
{
return;
}
Debug.Assert(len < MathUtil.SIMD_EPSILON);
//this jacobian entry could be re-used for all iterations
JacobianEntry jacA = new JacobianEntry(ref world2A, ref world2B, ref rel_posA1, ref rel_posA2, ref normalA, ref invInertiaADiag, invMassA,
ref invInertiaBDiag, invMassB);
JacobianEntry jacB = new JacobianEntry(ref world2A, ref world2B, ref rel_posB1, ref rel_posB2, ref normalB, ref invInertiaADiag, invMassA,
ref invInertiaBDiag, invMassB);
// float vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
// float vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
float vel0 = IndexedVector3.Dot(normalA, (body0.GetVelocityInLocalPoint(ref rel_posA1) - body1.GetVelocityInLocalPoint(ref rel_posA1)));
float vel1 = IndexedVector3.Dot(normalB, (body0.GetVelocityInLocalPoint(ref rel_posB1) - body1.GetVelocityInLocalPoint(ref rel_posB1)));
// float penetrationImpulse = (depth*contactTau*timeCorrection) * massTerm;//jacDiagABInv
float massTerm = 1f / (invMassA + invMassB);
// calculate rhs (or error) terms
float dv0 = depthA * m_tau * massTerm - vel0 * m_damping;
float dv1 = depthB * m_tau * massTerm - vel1 * m_damping;
// dC/dv * dv = -C
// jacobian * impulse = -error
//
//impulse = jacobianInverse * -error
// inverting 2x2 symmetric system (offdiagonal are equal!)
//
float nonDiag = jacA.GetNonDiagonal(jacB, invMassA, invMassB);
float invDet = 1f / (jacA.GetDiagonal() * jacB.GetDiagonal() - nonDiag * nonDiag);
//imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
//imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
imp0 = dv0 * jacA.GetDiagonal() * invDet + dv1 * -nonDiag * invDet;
imp1 = dv1 * jacB.GetDiagonal() * invDet + dv0 * -nonDiag * invDet;
//[a b] [d -c]
//[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc)
//[jA nD] * [imp0] = [dv0]
//[nD jB] [imp1] [dv1]
}
//
// solve unilateral raint (equality, direct method)
//
public void ResolveUnilateralPairConstraint(RigidBody body0, RigidBody body1, ref IndexedBasisMatrix world2A,
ref IndexedBasisMatrix world2B,
ref IndexedVector3 invInertiaADiag,
float invMassA,
ref IndexedVector3 linvelA, ref IndexedVector3 angvelA,
ref IndexedVector3 rel_posA1,
ref IndexedVector3 invInertiaBDiag,
float invMassB,
ref IndexedVector3 linvelB, ref IndexedVector3 angvelB,
ref IndexedVector3 rel_posA2,
float depthA, ref IndexedVector3 normalA,
ref IndexedVector3 rel_posB1, ref IndexedVector3 rel_posB2,
float depthB, ref IndexedVector3 normalB,
out float imp0, out float imp1)
{
//(void)linvelA;
//(void)linvelB;
//(void)angvelB;
//(void)angvelA;
imp0 = 0f;
imp1 = 0f;
float len = Math.Abs(normalA.Length()) - 1f;
if (Math.Abs(len) >= MathUtil.SIMD_EPSILON)
return;
Debug.Assert(len < MathUtil.SIMD_EPSILON);
//this jacobian entry could be re-used for all iterations
JacobianEntry jacA = new JacobianEntry(ref world2A,ref world2B,ref rel_posA1,ref rel_posA2,ref normalA,ref invInertiaADiag,invMassA,
ref invInertiaBDiag,invMassB);
JacobianEntry jacB = new JacobianEntry(ref world2A,ref world2B,ref rel_posB1,ref rel_posB2,ref normalB,ref invInertiaADiag,invMassA,
ref invInertiaBDiag,invMassB);
// float vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
// float vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
float vel0 = IndexedVector3.Dot(normalA,(body0.GetVelocityInLocalPoint(ref rel_posA1)-body1.GetVelocityInLocalPoint(ref rel_posA1)));
float vel1 = IndexedVector3.Dot(normalB,(body0.GetVelocityInLocalPoint(ref rel_posB1)-body1.GetVelocityInLocalPoint(ref rel_posB1)));
// float penetrationImpulse = (depth*contactTau*timeCorrection) * massTerm;//jacDiagABInv
float massTerm = 1f / (invMassA + invMassB);
// calculate rhs (or error) terms
float dv0 = depthA * m_tau * massTerm - vel0 * m_damping;
float dv1 = depthB * m_tau * massTerm - vel1 * m_damping;
// dC/dv * dv = -C
// jacobian * impulse = -error
//
//impulse = jacobianInverse * -error
// inverting 2x2 symmetric system (offdiagonal are equal!)
//
float nonDiag = jacA.GetNonDiagonal(jacB,invMassA,invMassB);
float invDet = 1f / (jacA.GetDiagonal() * jacB.GetDiagonal() - nonDiag * nonDiag );
//imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
//imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
imp0 = dv0 * jacA.GetDiagonal() * invDet + dv1 * -nonDiag * invDet;
imp1 = dv1 * jacB.GetDiagonal() * invDet + dv0 * - nonDiag * invDet;
//[a b] [d -c]
//[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc)
//[jA nD] * [imp0] = [dv0]
//[nD jB] [imp1] [dv1]
}
///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;
}
}
