//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); }
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; }