protected float Solve(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw)
{
float maxImpulse = 0;
Vector3 color = new Vector3(0, 1, 0);
if (cp.Distance <= 0)
{
if (iter == 0)
{
if (debugDraw != null)
{
debugDraw.DrawContactPoint(cp.PositionWorldOnB, cp.NormalWorldOnB, cp.Distance, cp.LifeTime, color);
}
}
ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData;
float impulse = cpd.ContactSolverFunc(
bodyA, bodyB,
cp,
info);
if (maxImpulse < impulse)
{
maxImpulse = impulse;
}
}
return(maxImpulse);
}
private bool MyContactDestroyedCallback(object userPersistentData)
{
if (userPersistentData == null)
{
throw new BulletException();
}
ConstraintPersistentData cpd = userPersistentData as ConstraintPersistentData;
_totalCpd--;
return(true);
}
protected float SolveFriction(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw)
{
Vector3 color = new Vector3(0, 1, 0);
if (cp.Distance <= 0)
{
ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData;
cpd.FrictionSolverFunc(
bodyA, bodyB,
cp,
info);
}
return(0);
}
//velocity + friction
//response between two dynamic objects with friction
public static float ResolveSingleCollisionCombined(
RigidBody bodyA,
RigidBody bodyB,
ManifoldPoint contactPoint,
ContactSolverInfo solverInfo)
{
Vector3 posA = contactPoint.PositionWorldOnA;
Vector3 posB = contactPoint.PositionWorldOnB;
Vector3 normal = contactPoint.NormalWorldOnB;
Vector3 relPosA = posA - bodyA.CenterOfMassPosition;
Vector3 relPosB = posB - bodyB.CenterOfMassPosition;
Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA);
Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB);
Vector3 vel = velA - velB;
float relVel;
relVel = Vector3.Dot(normal, vel);
float Kfps = 1f / solverInfo.TimeStep;
//float damping = solverInfo.m_damping;
float Kerp = solverInfo.Erp;
float Kcor = Kerp * Kfps;
ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData;
if (cpd == null)
{
throw new BulletException();
}
float distance = cpd.Penetration;
float positionalError = Kcor * -distance;
float velocityError = cpd.Restitution - relVel; // * damping;
float penetrationImpulse = positionalError * cpd.JacDiagABInv;
float velocityImpulse = velocityError * cpd.JacDiagABInv;
float normalImpulse = penetrationImpulse + velocityImpulse;
// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
float oldNormalImpulse = cpd.AppliedImpulse;
float sum = oldNormalImpulse + normalImpulse;
cpd.AppliedImpulse = 0 > sum ? 0 : sum;
normalImpulse = cpd.AppliedImpulse - oldNormalImpulse;
if (bodyA.InverseMass != 0)
{
bodyA.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyA.InverseMass, cpd.AngularComponentA, normalImpulse);
}
if (bodyB.InverseMass != 0)
{
bodyB.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyB.InverseMass, cpd.AngularComponentB, -normalImpulse);
}
{
//friction
Vector3 vel12 = bodyA.GetVelocityInLocalPoint(relPosA);
Vector3 vel22 = bodyB.GetVelocityInLocalPoint(relPosB);
Vector3 vel3 = vel12 - vel22;
relVel = Vector3.Dot(normal, vel3);
Vector3 latVel = vel3 - normal * relVel;
float lat_rel_vel = latVel.Length();
float combinedFriction = cpd.Friction;
if (cpd.AppliedImpulse > 0)
{
if (lat_rel_vel > float.Epsilon)
{
latVel /= lat_rel_vel;
Vector3 temp1 = Vector3.TransformNormal(Vector3.Cross(relPosA, latVel), bodyA.InvInertiaTensorWorld);
Vector3 temp2 = Vector3.TransformNormal(Vector3.Cross(relPosB, latVel), bodyB.InvInertiaTensorWorld);
float friction_impulse = lat_rel_vel /
(bodyA.InverseMass + bodyB.InverseMass + Vector3.Dot(latVel, Vector3.Cross(temp1, relPosA) + Vector3.Cross(temp2, relPosB)));
float normal_impulse = cpd.AppliedImpulse * combinedFriction;
MathHelper.SetMin(ref friction_impulse, normal_impulse);
MathHelper.SetMin(ref friction_impulse, -normal_impulse);
bodyA.ApplyImpulse(latVel * -friction_impulse, relPosA);
bodyB.ApplyImpulse(latVel * friction_impulse, relPosB);
}
}
}
return(normalImpulse);
}
public static float ResolveSingleFrictionOriginal(
RigidBody bodyA,
RigidBody bodyB,
ManifoldPoint contactPoint,
ContactSolverInfo solverInfo)
{
Vector3 posA = contactPoint.PositionWorldOnA;
Vector3 posB = contactPoint.PositionWorldOnB;
Vector3 relPosA = posA - bodyA.CenterOfMassPosition;
Vector3 relPosB = posB - bodyB.CenterOfMassPosition;
ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData;
if (cpd == null)
{
throw new BulletException();
}
float combinedFriction = cpd.Friction;
float limit = cpd.AppliedImpulse * combinedFriction;
//if (contactPoint.m_appliedImpulse>0.f)
//friction
{
//apply friction in the 2 tangential directions
{
// 1st tangent
Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA);
Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB);
Vector3 vel = velA - velB;
float vrel = Vector3.Dot(cpd.FrictionWorldTangentialA, vel);
// calculate j that moves us to zero relative velocity
float j = -vrel * cpd.JacDiagABInvTangentA;
float total = cpd.AccumulatedTangentImpulseA + j;
if (limit < total)
{
total = limit;
}
if (total < -limit)
{
total = -limit;
}
j = total - cpd.AccumulatedTangentImpulseA;
cpd.AccumulatedTangentImpulseA = total;
bodyA.ApplyImpulse(j * cpd.FrictionWorldTangentialA, relPosA);
bodyB.ApplyImpulse(j * -cpd.FrictionWorldTangentialA, relPosB);
}
{
// 2nd tangent
Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA);
Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB);
Vector3 vel = velA - velB;
float vrel = Vector3.Dot(cpd.FrictionWorldTangentialB, vel);
// calculate j that moves us to zero relative velocity
float j = -vrel * cpd.JacDiagABInvTangentB;
float total = cpd.AccumulatedTangentImpulseB + j;
if (limit < total)
{
total = limit;
}
if (total < -limit)
{
total = -limit;
}
j = total - cpd.AccumulatedTangentImpulseB;
cpd.AccumulatedTangentImpulseB = total;
bodyA.ApplyImpulse(j * cpd.FrictionWorldTangentialB, relPosA);
bodyB.ApplyImpulse(j * -cpd.FrictionWorldTangentialB, relPosB);
}
}
return(cpd.AppliedImpulse);
}
public static float ResolveSingleFriction(RigidBody bodyA, RigidBody bodyB,
ManifoldPoint contactPoint, ContactSolverInfo solverInfo)
{
Vector3 pos1 = contactPoint.PositionWorldOnA;
Vector3 pos2 = contactPoint.PositionWorldOnB;
Vector3 rel_pos1 = pos1 - bodyA.CenterOfMassPosition;
Vector3 rel_pos2 = pos2 - bodyB.CenterOfMassPosition;
ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData;
if (cpd == null)
{
throw new BulletException();
}
float combinedFriction = cpd.Friction;
float limit = cpd.AppliedImpulse * combinedFriction;
//friction
if (cpd.AppliedImpulse > 0)
{
//apply friction in the 2 tangential directions
// 1st tangent
Vector3 vel1 = bodyA.GetVelocityInLocalPoint(rel_pos1);
Vector3 vel2 = bodyB.GetVelocityInLocalPoint(rel_pos2);
Vector3 vel = vel1 - vel2;
float j1, j2;
{
float vrel = Vector3.Dot(cpd.FrictionWorldTangentialA, vel);
// calculate j that moves us to zero relative velocity
j1 = -vrel * cpd.JacDiagABInvTangentA;
float oldTangentImpulse = cpd.AccumulatedTangentImpulseA;
cpd.AccumulatedTangentImpulseA = oldTangentImpulse + j1;
float atia = cpd.AccumulatedTangentImpulseA;
MathHelper.SetMin(ref atia, limit);
MathHelper.SetMax(ref atia, -limit);
cpd.AccumulatedTangentImpulseA = atia;
j1 = cpd.AccumulatedTangentImpulseA - oldTangentImpulse;
}
{
// 2nd tangent
float vrel = Vector3.Dot(cpd.FrictionWorldTangentialB, vel);
// calculate j that moves us to zero relative velocity
j2 = -vrel * cpd.JacDiagABInvTangentB;
float oldTangentImpulse = cpd.AccumulatedTangentImpulseB;
cpd.AccumulatedTangentImpulseB = oldTangentImpulse + j2;
float atib = cpd.AccumulatedTangentImpulseB;
MathHelper.SetMin(ref atib, limit);
MathHelper.SetMax(ref atib, -limit);
cpd.AccumulatedTangentImpulseB = atib;
j2 = cpd.AccumulatedTangentImpulseB - oldTangentImpulse;
}
if (bodyA.InverseMass != 0)
{
bodyA.InternalApplyImpulse(cpd.FrictionWorldTangentialA * bodyA.InverseMass, cpd.FrictionAngularComponent0A, j1);
bodyA.InternalApplyImpulse(cpd.FrictionWorldTangentialB * bodyA.InverseMass, cpd.FrictionAngularComponent1A, j2);
}
if (bodyB.InverseMass != 0)
{
bodyB.InternalApplyImpulse(cpd.FrictionWorldTangentialA * bodyB.InverseMass, cpd.FrictionAngularComponent0B, -j1);
bodyB.InternalApplyImpulse(cpd.FrictionWorldTangentialB * bodyB.InverseMass, cpd.FrictionAngularComponent1B, -j2);
}
}
return(cpd.AppliedImpulse);
}
/// <summary>
/// contact constraint resolution:
/// calculate and apply impulse to satisfy non-penetration and non-negative relative velocity constraint
/// positive distance = separation, negative distance = penetration
/// </summary>
/// <param name="body1"></param>
/// <param name="body2"></param>
/// <param name="contactPoint"></param>
/// <param name="info"></param>
/// <returns></returns>
public static float ResolveSingleCollision(RigidBody bodyA, RigidBody bodyB,
ManifoldPoint contactPoint, ContactSolverInfo solverInfo)
{
Vector3 pos1 = contactPoint.PositionWorldOnA;
Vector3 pos2 = contactPoint.PositionWorldOnB;
// printf("distance=%f\n",distance);
Vector3 normal = contactPoint.NormalWorldOnB;
Vector3 rel_pos1 = pos1 - bodyA.CenterOfMassPosition;
Vector3 rel_pos2 = pos2 - bodyB.CenterOfMassPosition;
Vector3 vel1 = bodyA.GetVelocityInLocalPoint(rel_pos1);
Vector3 vel2 = bodyB.GetVelocityInLocalPoint(rel_pos2);
Vector3 vel = vel1 - vel2;
float rel_vel;
rel_vel = Vector3.Dot(normal, vel);
float Kfps = 1f / solverInfo.TimeStep;
//float damping = solverInfo.m_damping;
float Kerp = solverInfo.Erp;
float Kcor = Kerp * Kfps;
//printf("dist=%f\n",distance);
ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData;
if (cpd == null)
{
throw new BulletException();
}
float distance = cpd.Penetration; //contactPoint.getDistance();
//distance = 0.f;
float positionalError = Kcor * -distance;
//jacDiagABInv;
float velocityError = cpd.Restitution - rel_vel; // * damping;
float penetrationImpulse = positionalError * cpd.JacDiagABInv;
float velocityImpulse = velocityError * cpd.JacDiagABInv;
float normalImpulse = penetrationImpulse + velocityImpulse;
// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
float oldNormalImpulse = cpd.AppliedImpulse;
float sum = oldNormalImpulse + normalImpulse;
cpd.AppliedImpulse = 0f > sum ? 0f : sum;
normalImpulse = cpd.AppliedImpulse - oldNormalImpulse;
if (bodyA.InverseMass != 0)
{
bodyA.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyA.InverseMass, cpd.AngularComponentA, normalImpulse);
}
if (bodyB.InverseMass != 0)
{
bodyB.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyB.InverseMass, cpd.AngularComponentB, -normalImpulse);
}
/*body1.applyImpulse(normal * (normalImpulse), rel_pos1);
* body2.applyImpulse(-normal * (normalImpulse), rel_pos2);*/
return(normalImpulse);
}
protected void PrepareConstraints(PersistentManifold manifold, ContactSolverInfo info)
{
RigidBody body0 = manifold.BodyA as RigidBody;
RigidBody body1 = manifold.BodyB as RigidBody;
//only necessary to refresh the manifold once (first iteration). The integration is done outside the loop
{
manifold.RefreshContactPoints(body0.CenterOfMassTransform, body1.CenterOfMassTransform);
int numpoints = manifold.ContactsCount;
_totalContactPoints += numpoints;
Vector3 color = new Vector3(0, 1, 0);
for (int i = 0; i < numpoints; i++)
{
ManifoldPoint cp = manifold.GetContactPoint(i);
if (cp.Distance <= 0)
{
Vector3 pos1 = cp.PositionWorldOnA;
Vector3 pos2 = cp.PositionWorldOnB;
Vector3 rel_pos1 = pos1 - body0.CenterOfMassPosition;
Vector3 rel_pos2 = pos2 - body1.CenterOfMassPosition;
//this jacobian entry is re-used for all iterations
JacobianEntry jac = new JacobianEntry(MatrixOperations.Transpose(body0.CenterOfMassTransform),
MatrixOperations.Transpose(body1.CenterOfMassTransform),
rel_pos1, rel_pos2, cp.NormalWorldOnB, body0.InvInertiaDiagLocal, body0.InverseMass,
body1.InvInertiaDiagLocal, body1.InverseMass);
float jacDiagAB = jac.Diagonal;
ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData;
if (cpd != null)
{
//might be invalid
cpd.PersistentLifeTime++;
if (cpd.PersistentLifeTime != cp.LifeTime)
{
//printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
cpd = new ConstraintPersistentData();
cpd.PersistentLifeTime = cp.LifeTime;
}
}
else
{
cpd = new ConstraintPersistentData();
_totalCpd++;
//printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd);
cp.UserPersistentData = cpd;
cpd.PersistentLifeTime = cp.LifeTime;
//printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.getLifeTime());
}
if (cpd == null)
throw new BulletException();
cpd.JacDiagABInv = 1f / jacDiagAB;
//Dependent on Rigidbody A and B types, fetch the contact/friction response func
//perhaps do a similar thing for friction/restutution combiner funcs...
cpd.FrictionSolverFunc = _frictionDispatch[(int)body0.FrictionSolverType, (int)body1.FrictionSolverType];
cpd.ContactSolverFunc = _contactDispatch[(int)body0.ContactSolverType, (int)body1.ContactSolverType];
Vector3 vel1 = body0.GetVelocityInLocalPoint(rel_pos1);
Vector3 vel2 = body1.GetVelocityInLocalPoint(rel_pos2);
Vector3 vel = vel1 - vel2;
float rel_vel;
rel_vel = Vector3.Dot(cp.NormalWorldOnB, vel);
float combinedRestitution = cp.CombinedRestitution;
cpd.Penetration = cp.Distance;
cpd.Friction = cp.CombinedFriction;
cpd.Restitution = RestitutionCurve(rel_vel, combinedRestitution);
if (cpd.Restitution < 0f)
{
cpd.Restitution = 0.0f;
};
//restitution and penetration work in same direction so
//rel_vel
float penVel = -cpd.Penetration / info.TimeStep;
if (cpd.Restitution > penVel)
{
cpd.Penetration = 0;
}
float relaxation = info.Damping;
if ((_solverMode & SolverMode.UseWarmstarting) != 0)
{
cpd.AppliedImpulse *= relaxation;
}
else
{
cpd.AppliedImpulse = 0f;
}
//for friction
cpd.PreviousAppliedImpulse = cpd.AppliedImpulse;
//re-calculate friction direction every frame, todo: check if this is really needed
Vector3 fwta = cpd.FrictionWorldTangentialA;
Vector3 fwtb = cpd.FrictionWorldTangentialB;
MathHelper.PlaneSpace1(cp.NormalWorldOnB, ref fwta, ref fwtb);
cpd.FrictionWorldTangentialA = fwta;
cpd.FrictionWorldTangentialB = fwtb;
cpd.AccumulatedTangentImpulseA = 0;
cpd.AccumulatedTangentImpulseB = 0;
float denom0 = body0.ComputeImpulseDenominator(pos1, cpd.FrictionWorldTangentialA);
float denom1 = body1.ComputeImpulseDenominator(pos2, cpd.FrictionWorldTangentialA);
float denom = relaxation / (denom0 + denom1);
cpd.JacDiagABInvTangentA = denom;
denom0 = body0.ComputeImpulseDenominator(pos1, cpd.FrictionWorldTangentialB);
denom1 = body1.ComputeImpulseDenominator(pos2, cpd.FrictionWorldTangentialB);
denom = relaxation / (denom0 + denom1);
cpd.JacDiagABInvTangentB = denom;
Vector3 totalImpulse = cp.NormalWorldOnB * cpd.AppliedImpulse;
{
Vector3 torqueAxis0 = Vector3.Cross(rel_pos1, cp.NormalWorldOnB);
cpd.AngularComponentA = Vector3.TransformNormal(torqueAxis0, body0.InvInertiaTensorWorld);
Vector3 torqueAxis1 = Vector3.Cross(rel_pos2, cp.NormalWorldOnB);
cpd.AngularComponentB = Vector3.TransformNormal(torqueAxis1, body1.InvInertiaTensorWorld);
}
{
Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos1, cpd.FrictionWorldTangentialA);
cpd.FrictionAngularComponent0A = Vector3.TransformNormal(ftorqueAxis0, body0.InvInertiaTensorWorld);
}
{
Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos1, cpd.FrictionWorldTangentialB);
cpd.FrictionAngularComponent1A = Vector3.TransformNormal(ftorqueAxis1, body0.InvInertiaTensorWorld);
}
{
Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos2, cpd.FrictionWorldTangentialA);
cpd.FrictionAngularComponent0B = Vector3.TransformNormal(ftorqueAxis0, body1.InvInertiaTensorWorld);
}
{
Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos2, cpd.FrictionWorldTangentialB);
cpd.FrictionAngularComponent1B = Vector3.TransformNormal(ftorqueAxis1, body1.InvInertiaTensorWorld);
}
//apply previous frames impulse on both bodies
body0.ApplyImpulse(totalImpulse, rel_pos1);
body1.ApplyImpulse(-totalImpulse, rel_pos2);
}
}
}
}
protected void PrepareConstraints(PersistentManifold manifold, ContactSolverInfo info)
{
RigidBody body0 = manifold.BodyA as RigidBody;
RigidBody body1 = manifold.BodyB as RigidBody;
//only necessary to refresh the manifold once (first iteration). The integration is done outside the loop
{
manifold.RefreshContactPoints(body0.CenterOfMassTransform, body1.CenterOfMassTransform);
int numpoints = manifold.ContactsCount;
_totalContactPoints += numpoints;
Vector3 color = new Vector3(0, 1, 0);
for (int i = 0; i < numpoints; i++)
{
ManifoldPoint cp = manifold.GetContactPoint(i);
if (cp.Distance <= 0)
{
Vector3 pos1 = cp.PositionWorldOnA;
Vector3 pos2 = cp.PositionWorldOnB;
Vector3 rel_pos1 = pos1 - body0.CenterOfMassPosition;
Vector3 rel_pos2 = pos2 - body1.CenterOfMassPosition;
//this jacobian entry is re-used for all iterations
JacobianEntry jac = new JacobianEntry(MatrixOperations.Transpose(body0.CenterOfMassTransform),
MatrixOperations.Transpose(body1.CenterOfMassTransform),
rel_pos1, rel_pos2, cp.NormalWorldOnB, body0.InvInertiaDiagLocal, body0.InverseMass,
body1.InvInertiaDiagLocal, body1.InverseMass);
float jacDiagAB = jac.Diagonal;
ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData;
if (cpd != null)
{
//might be invalid
cpd.PersistentLifeTime++;
if (cpd.PersistentLifeTime != cp.LifeTime)
{
//printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
cpd = new ConstraintPersistentData();
cpd.PersistentLifeTime = cp.LifeTime;
}
}
else
{
cpd = new ConstraintPersistentData();
_totalCpd++;
//printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd);
cp.UserPersistentData = cpd;
cpd.PersistentLifeTime = cp.LifeTime;
//printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.getLifeTime());
}
if (cpd == null)
{
throw new BulletException();
}
cpd.JacDiagABInv = 1f / jacDiagAB;
//Dependent on Rigidbody A and B types, fetch the contact/friction response func
//perhaps do a similar thing for friction/restutution combiner funcs...
cpd.FrictionSolverFunc = _frictionDispatch[(int)body0.FrictionSolverType, (int)body1.FrictionSolverType];
cpd.ContactSolverFunc = _contactDispatch[(int)body0.ContactSolverType, (int)body1.ContactSolverType];
Vector3 vel1 = body0.GetVelocityInLocalPoint(rel_pos1);
Vector3 vel2 = body1.GetVelocityInLocalPoint(rel_pos2);
Vector3 vel = vel1 - vel2;
float rel_vel;
rel_vel = Vector3.Dot(cp.NormalWorldOnB, vel);
float combinedRestitution = cp.CombinedRestitution;
cpd.Penetration = cp.Distance;
cpd.Friction = cp.CombinedFriction;
cpd.Restitution = RestitutionCurve(rel_vel, combinedRestitution);
if (cpd.Restitution < 0f)
{
cpd.Restitution = 0.0f;
}
;
//restitution and penetration work in same direction so
//rel_vel
float penVel = -cpd.Penetration / info.TimeStep;
if (cpd.Restitution > penVel)
{
cpd.Penetration = 0;
}
float relaxation = info.Damping;
if ((_solverMode & SolverMode.UseWarmstarting) != 0)
{
cpd.AppliedImpulse *= relaxation;
}
else
{
cpd.AppliedImpulse = 0f;
}
//for friction
cpd.PreviousAppliedImpulse = cpd.AppliedImpulse;
//re-calculate friction direction every frame, todo: check if this is really needed
Vector3 fwta = cpd.FrictionWorldTangentialA;
Vector3 fwtb = cpd.FrictionWorldTangentialB;
MathHelper.PlaneSpace1(cp.NormalWorldOnB, ref fwta, ref fwtb);
cpd.FrictionWorldTangentialA = fwta;
cpd.FrictionWorldTangentialB = fwtb;
cpd.AccumulatedTangentImpulseA = 0;
cpd.AccumulatedTangentImpulseB = 0;
float denom0 = body0.ComputeImpulseDenominator(pos1, cpd.FrictionWorldTangentialA);
float denom1 = body1.ComputeImpulseDenominator(pos2, cpd.FrictionWorldTangentialA);
float denom = relaxation / (denom0 + denom1);
cpd.JacDiagABInvTangentA = denom;
denom0 = body0.ComputeImpulseDenominator(pos1, cpd.FrictionWorldTangentialB);
denom1 = body1.ComputeImpulseDenominator(pos2, cpd.FrictionWorldTangentialB);
denom = relaxation / (denom0 + denom1);
cpd.JacDiagABInvTangentB = denom;
Vector3 totalImpulse = cp.NormalWorldOnB * cpd.AppliedImpulse;
{
Vector3 torqueAxis0 = Vector3.Cross(rel_pos1, cp.NormalWorldOnB);
cpd.AngularComponentA = Vector3.TransformNormal(torqueAxis0, body0.InvInertiaTensorWorld);
Vector3 torqueAxis1 = Vector3.Cross(rel_pos2, cp.NormalWorldOnB);
cpd.AngularComponentB = Vector3.TransformNormal(torqueAxis1, body1.InvInertiaTensorWorld);
}
{
Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos1, cpd.FrictionWorldTangentialA);
cpd.FrictionAngularComponent0A = Vector3.TransformNormal(ftorqueAxis0, body0.InvInertiaTensorWorld);
}
{
Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos1, cpd.FrictionWorldTangentialB);
cpd.FrictionAngularComponent1A = Vector3.TransformNormal(ftorqueAxis1, body0.InvInertiaTensorWorld);
}
{
Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos2, cpd.FrictionWorldTangentialA);
cpd.FrictionAngularComponent0B = Vector3.TransformNormal(ftorqueAxis0, body1.InvInertiaTensorWorld);
}
{
Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos2, cpd.FrictionWorldTangentialB);
cpd.FrictionAngularComponent1B = Vector3.TransformNormal(ftorqueAxis1, body1.InvInertiaTensorWorld);
}
//apply previous frames impulse on both bodies
body0.ApplyImpulse(totalImpulse, rel_pos1);
body1.ApplyImpulse(-totalImpulse, rel_pos2);
}
}
}
}
