//velocity + friction
//response between two dynamic objects with friction
public virtual float ResolveSingleCollisionCombinedCacheFriendly(
SolverBody bodyA,
SolverBody bodyB,
SolverConstraint contactConstraint,
ContactSolverInfo solverInfo)
{
float normalImpulse = 0;
if (contactConstraint.Penetration < 0)
{
return(0);
}
float relVel;
float velADotn = Vector3.Dot(contactConstraint.ContactNormal, bodyA.LinearVelocity)
+ Vector3.Dot(contactConstraint.RelPosACrossNormal, bodyA.AngularVelocity);
float velBDotn = Vector3.Dot(contactConstraint.ContactNormal, bodyB.LinearVelocity)
+ Vector3.Dot(contactConstraint.RelPosBCrossNormal, bodyB.AngularVelocity);
relVel = velADotn - velBDotn;
float positionalError = contactConstraint.Penetration;
float velocityError = contactConstraint.Restitution - relVel; // * damping;
float penetrationImpulse = positionalError * contactConstraint.JacDiagABInv;
float velocityImpulse = velocityError * contactConstraint.JacDiagABInv;
normalImpulse = penetrationImpulse + velocityImpulse;
// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
float oldNormalImpulse = contactConstraint.AppliedImpulse;
float sum = oldNormalImpulse + normalImpulse;
contactConstraint.AppliedImpulse = 0 > sum ? 0 : sum;
float oldVelocityImpulse = contactConstraint.AppliedVelocityImpulse;
float velocitySum = oldVelocityImpulse + velocityImpulse;
contactConstraint.AppliedVelocityImpulse = 0 > velocitySum ? 0 : velocitySum;
normalImpulse = contactConstraint.AppliedImpulse - oldNormalImpulse;
if (bodyA.InvMass != 0)
{
bodyA.ApplyImpulse(contactConstraint.ContactNormal * bodyA.InvMass,
contactConstraint.AngularComponentA, normalImpulse);
}
if (bodyB.InvMass != 0)
{
bodyB.ApplyImpulse(contactConstraint.ContactNormal * bodyB.InvMass,
contactConstraint.AngularComponentB, -normalImpulse);
}
return(normalImpulse);
}
private void InitSolverBody(out SolverBody solverBody, RigidBody rigidBody)
{
solverBody = new SolverBody();
solverBody.AngularVelocity = rigidBody.AngularVelocity;
solverBody.CenterOfMassPosition = rigidBody.CenterOfMassPosition;
solverBody.Friction = rigidBody.Friction;
solverBody.InvMass = rigidBody.InverseMass;
solverBody.LinearVelocity = rigidBody.LinearVelocity;
solverBody.OriginalBody = rigidBody;
solverBody.AngularFactor = rigidBody.AngularFactor;
}
public virtual float ResolveSingleFrictionCacheFriendly(
SolverBody bodyA,
SolverBody bodyB,
SolverConstraint contactConstraint,
ContactSolverInfo solverInfo,
float appliedNormalImpulse)
{
float combinedFriction = contactConstraint.Friction;
float limit = appliedNormalImpulse * combinedFriction;
if (appliedNormalImpulse > 0)
//friction
{
float j1;
{
float relVel;
float velADotn = Vector3.Dot(contactConstraint.ContactNormal, bodyA.LinearVelocity)
+ Vector3.Dot(contactConstraint.RelPosACrossNormal, bodyA.AngularVelocity);
float velBDotn = Vector3.Dot(contactConstraint.ContactNormal, bodyB.LinearVelocity)
+ Vector3.Dot(contactConstraint.RelPosBCrossNormal, bodyB.AngularVelocity);
relVel = velADotn - velBDotn;
// calculate j that moves us to zero relative velocity
j1 = -relVel * contactConstraint.JacDiagABInv;
float oldTangentImpulse = contactConstraint.AppliedImpulse;
contactConstraint.AppliedImpulse = oldTangentImpulse + j1;
float test = contactConstraint.AppliedImpulse;
MathHelper.SetMin(ref test, limit);
MathHelper.SetMax(ref test, -limit);
contactConstraint.AppliedImpulse = test;
j1 = contactConstraint.AppliedImpulse - oldTangentImpulse;
}
if (bodyA.InvMass != 0)
{
bodyA.ApplyImpulse(contactConstraint.ContactNormal * bodyA.InvMass, contactConstraint.AngularComponentA, j1);
}
if (bodyB.InvMass != 0)
{
bodyB.ApplyImpulse(contactConstraint.ContactNormal * bodyB.InvMass, contactConstraint.AngularComponentB, -j1);
}
}
return(0);
}
private void InitSolverBody(out SolverBody solverBody, RigidBody rigidBody)
{
solverBody = new SolverBody();
solverBody.AngularVelocity = rigidBody.AngularVelocity;
solverBody.CenterOfMassPosition = rigidBody.CenterOfMassPosition;
solverBody.Friction = rigidBody.Friction;
solverBody.InvMass = rigidBody.InverseMass;
solverBody.LinearVelocity = rigidBody.LinearVelocity;
solverBody.OriginalBody = rigidBody;
solverBody.AngularFactor = rigidBody.AngularFactor;
}
public virtual float ResolveSingleFrictionCacheFriendly(
SolverBody bodyA,
SolverBody bodyB,
SolverConstraint contactConstraint,
ContactSolverInfo solverInfo,
float appliedNormalImpulse)
{
float combinedFriction = contactConstraint.Friction;
float limit = appliedNormalImpulse * combinedFriction;
if (appliedNormalImpulse > 0)
//friction
{
float j1;
{
float relVel;
float velADotn = Vector3.Dot(contactConstraint.ContactNormal, bodyA.LinearVelocity)
+ Vector3.Dot(contactConstraint.RelPosACrossNormal, bodyA.AngularVelocity);
float velBDotn = Vector3.Dot(contactConstraint.ContactNormal, bodyB.LinearVelocity)
+ Vector3.Dot(contactConstraint.RelPosBCrossNormal, bodyB.AngularVelocity);
relVel = velADotn - velBDotn;
// calculate j that moves us to zero relative velocity
j1 = -relVel * contactConstraint.JacDiagABInv;
float oldTangentImpulse = contactConstraint.AppliedImpulse;
contactConstraint.AppliedImpulse = oldTangentImpulse + j1;
float test = contactConstraint.AppliedImpulse;
MathHelper.SetMin(ref test, limit);
MathHelper.SetMax(ref test, -limit);
contactConstraint.AppliedImpulse = test;
j1 = contactConstraint.AppliedImpulse - oldTangentImpulse;
}
if (bodyA.InvMass != 0)
{
bodyA.ApplyImpulse(contactConstraint.ContactNormal * bodyA.InvMass, contactConstraint.AngularComponentA, j1);
}
if (bodyB.InvMass != 0)
{
bodyB.ApplyImpulse(contactConstraint.ContactNormal * bodyB.InvMass, contactConstraint.AngularComponentB, -j1);
}
}
return 0;
}
//velocity + friction
//response between two dynamic objects with friction
public virtual float ResolveSingleCollisionCombinedCacheFriendly(
SolverBody bodyA,
SolverBody bodyB,
SolverConstraint contactConstraint,
ContactSolverInfo solverInfo)
{
float normalImpulse = 0;
if (contactConstraint.Penetration < 0)
return 0;
float relVel;
float velADotn = Vector3.Dot(contactConstraint.ContactNormal,bodyA.LinearVelocity)
+ Vector3.Dot(contactConstraint.RelPosACrossNormal,bodyA.AngularVelocity);
float velBDotn = Vector3.Dot(contactConstraint.ContactNormal,bodyB.LinearVelocity)
+ Vector3.Dot(contactConstraint.RelPosBCrossNormal,bodyB.AngularVelocity);
relVel = velADotn - velBDotn;
float positionalError = contactConstraint.Penetration;
float velocityError = contactConstraint.Restitution - relVel;// * damping;
float penetrationImpulse = positionalError * contactConstraint.JacDiagABInv;
float velocityImpulse = velocityError * contactConstraint.JacDiagABInv;
normalImpulse = penetrationImpulse + velocityImpulse;
// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
float oldNormalImpulse = contactConstraint.AppliedImpulse;
float sum = oldNormalImpulse + normalImpulse;
contactConstraint.AppliedImpulse = 0 > sum ? 0 : sum;
float oldVelocityImpulse = contactConstraint.AppliedVelocityImpulse;
float velocitySum = oldVelocityImpulse + velocityImpulse;
contactConstraint.AppliedVelocityImpulse = 0 > velocitySum ? 0 : velocitySum;
normalImpulse = contactConstraint.AppliedImpulse - oldNormalImpulse;
if (bodyA.InvMass != 0)
{
bodyA.ApplyImpulse(contactConstraint.ContactNormal * bodyA.InvMass,
contactConstraint.AngularComponentA, normalImpulse);
}
if (bodyB.InvMass != 0)
{
bodyB.ApplyImpulse(contactConstraint.ContactNormal * bodyB.InvMass,
contactConstraint.AngularComponentB, -normalImpulse);
}
return normalImpulse;
}