public static void ResolveContact(Contact c)
{
var a = c.A;
var b = c.B;
var disp = c.Disp;
var axis = c.Axis; //axis from A to B
var con = c.ConPoint;
var Ia = a.GetInertiaMoment();
var Ib = b.GetInertiaMoment();
var totInvMass = a.InvMass + b.InvMass;
a.Position += +disp * axis * a.InvMass / totInvMass;
b.Position += -disp * axis * b.InvMass / totInvMass;
//
//impulse calculations to set momentum
//
var radA = (con - a.Position);
var radB = (con - b.Position);
Vector2 velA, velB, tvelA, tvelB;
tvelA = Vector3.Cross(a.AngularVelocity * Vector3.UnitZ, radA.ToVector3()).ToVector2(); // tangentVel = (contactpt - centroid)x(angVel) // remember angVel is really on the Z axis.
tvelB = Vector3.Cross(b.AngularVelocity * Vector3.UnitZ, radB.ToVector3()).ToVector2(); //also, remember position is the position of the axis of rotation (centroid, or fixed point). which works our well.
velA = a.Velocity + tvelA; //velocity of contact point = (vel of object) + (tangential vel of pt due to ang_vel)
velB = b.Velocity + tvelB;
var sepVel = Vector2.Dot((velB - velA), axis); //find the separating velocity of the contact points
if (sepVel > 0) return; //if the objects are already separatingm, then skip the impulse calcs below
var res = (a.Restitution + b.Restitution) / 2;
var n = -axis; //-axis?
var tmpA = Vector3.Dot(n.ToVector3(), (Vector3.Cross(radA.ToVector3(), n.ToVector3()) / Ia));
var tmpB = Vector3.Dot(n.ToVector3(), (Vector3.Cross(radB.ToVector3(), n.ToVector3()) / Ib));
var impulse = -sepVel * (res + 1) / (a.InvMass + b.InvMass + tmpA + tmpB); //angular
//var t = -Vector2.Normalize((velB - velA));
var t = Vector3.Cross(Vector3.Cross(n.ToVector3(), (velA - velB).ToVector3()), n.ToVector3()).ToVector2();
var u = (a.Friction + b.Friction) / 2;
a.Velocity += (impulse * n + impulse * u * t) * a.InvMass;
b.Velocity += (-impulse * n + impulse * u * t) * b.InvMass;
a.AngularVelocity += (Vector3.Cross(radA.ToVector3(), (impulse * n.ToVector3() + impulse * u * t.ToVector3()))).Z * a.InvMass;
b.AngularVelocity += (Vector3.Cross(radB.ToVector3(), (-impulse * n.ToVector3() + impulse * u * t.ToVector3()))).Z * b.InvMass;
}
public static void CalcCollisionNoFrict(Contact c, out ObjCollisionData dataA, out ObjCollisionData dataB)
{
dataA.dVel = Vector2.Zero;
dataB.dVel = Vector2.Zero;
dataA.dAngVel = 0;
dataB.dAngVel = 0;
dataA.dPos = Vector2.Zero;
dataB.dPos = Vector2.Zero;
var con = c.ConPoint;
var a = c.A;
var b = c.B;
var disp = c.Disp;
var Ia = a.GetInertiaMoment();
var Ib = b.GetInertiaMoment();
var Ia_inv = a.GetInverseInertiaMoment();
var Ib_inv = b.GetInverseInertiaMoment();
var axis = c.Axis; //from A->B
var n = -axis; //points from B->A
var n3 = n.ToVector3();
var u = (a.Friction + b.Friction) / 2;
var res = (a.Restitution + b.Restitution) / 2;
//ADD: limit the rest. if sep vel is slow
var totInvMass = a.InvMass + b.InvMass;
dataA.dPos = +disp * axis * a.InvMass / totInvMass;
dataB.dPos = -disp * axis * b.InvMass / totInvMass;
var relPosA = (con - a.Position).ToVector3();
var relPosB = (con - b.Position).ToVector3();
Vector3 velA, velB, tvelA, tvelB;
tvelA = Vector3.Cross(a.AngularVelocity * Vector3.UnitZ, relPosA); // tangentVel = (contactpt - centroid)x(angVel) // remember angVel is really on the Z axis.
tvelB = Vector3.Cross(b.AngularVelocity * Vector3.UnitZ, relPosB); //also, remember position is the position of the axis of rotation (centroid, or fixed point). which works our well.
velA = a.Velocity.ToVector3() + tvelA; //velocity of contact point = (vel of object) + (tangential vel of pt due to ang_vel)
velB = b.Velocity.ToVector3() + tvelB;
var torquePerImpulseA = Vector3.Cross(relPosA, n3);
var torquePerImpulseB = Vector3.Cross(relPosB, n3);
var rotPerImpA = torquePerImpulseA * Ia_inv;
var rotPerImpB = torquePerImpulseB * Ib_inv;
var velPerImpA = Vector3.Cross(rotPerImpA, relPosA); //"deltaVelWorld"
var velPerImpB = Vector3.Cross(rotPerImpB, relPosB);
Matrix localToWorld = CreateOrthonormalBasis(n, n.Perpen());
Matrix worldToLocal = Matrix.Transpose(localToWorld);
var velPerImpA_Contact = velPerImpA.TransformToLocal(worldToLocal); //"deltaVelocity"
var velPerImpB_Contact = velPerImpB.TransformToLocal(worldToLocal);
var angCompA = velPerImpA_Contact.X;
var angCompB = velPerImpB_Contact.X;
var deltaVelocity = angCompA + angCompB + a.InvMass + b.InvMass;
var sepVel = velA - velB;
var conVel = sepVel.TransformToLocal(worldToLocal);
var desiredVel = -conVel.X * (1 + res);
Vector3 impulseContact;
impulseContact.X = desiredVel / deltaVelocity;
impulseContact.Y = 0;
impulseContact.Z = 0;
var impulse = impulseContact.TransformToWorld(localToWorld);
var impulse2 = impulse.ToVector2();
dataA.dVel = impulse2 * a.InvMass;
dataB.dVel = -impulse2 * b.InvMass;
var impulsiveTorqueA = Vector3.Cross(relPosA, impulse);
var impulsiveTorqueB = Vector3.Cross(relPosB, -impulse);
dataA.dAngVel = impulsiveTorqueA.Z * a.GetInverseInertiaMoment();
dataB.dAngVel = impulsiveTorqueB.Z * b.GetInverseInertiaMoment();
}
public static void CalcCollisionOld(Contact c, out ObjCollisionData dataA, out ObjCollisionData dataB)
{
dataA.dVel = Vector2.Zero; //incase the method exits before ang velocities need to be calculated (ie, if the contact point is separating)
dataB.dVel = Vector2.Zero;
dataA.dAngVel = 0;
dataB.dAngVel = 0;
dataA.dPos = Vector2.Zero;
dataB.dPos = Vector2.Zero;
var a = c.A;
var b = c.B;
var disp = c.Disp;
var axis = c.Axis; //axis from A to B
var con = c.ConPoint;
var Ia = a.GetInertiaMoment();
var Ib = b.GetInertiaMoment();
var totInvMass = a.InvMass + b.InvMass;
dataA.dPos = +disp * axis * a.InvMass / totInvMass;
dataB.dPos = -disp * axis * b.InvMass / totInvMass;
//
//impulse calculations to set momentum
//
var radA = (con - a.Position);
var radB = (con - b.Position);
Vector2 velA, velB, tvelA, tvelB;
tvelA = Vector3.Cross(a.AngularVelocity * Vector3.UnitZ, radA.ToVector3()).ToVector2(); // tangentVel = (contactpt - centroid)x(angVel) // remember angVel is really on the Z axis.
tvelB = Vector3.Cross(b.AngularVelocity * Vector3.UnitZ, radB.ToVector3()).ToVector2(); //also, remember position is the position of the axis of rotation (centroid, or fixed point). which works our well.
velA = a.Velocity + tvelA; //velocity of contact point = (vel of object) + (tangential vel of pt due to ang_vel)
velB = b.Velocity + tvelB;
var sepVel = Vector2.Dot((velB - velA), axis); //find the separating velocity of the contact points
if (sepVel > 0) return; //if the objects are already separatingm, then skip the impulse calcs below
var res = (a.Restitution + b.Restitution) / 2;
var n = -axis; //-axis?
var tmpA = Vector3.Dot(Vector3.Cross(n.ToVector3(), Vector3.Cross(radA.ToVector3(), n.ToVector3()) / Ia), radA.ToVector3());
var tmpB = Vector3.Dot(Vector3.Cross(n.ToVector3(), Vector3.Cross(radB.ToVector3(), n.ToVector3()) / Ib), radB.ToVector3());
var impulse = -sepVel * (res + 1) / (a.InvMass + b.InvMass + tmpA + tmpB); //angular
var relVel = (velA - velB);
var t = Vector3.Cross(Vector3.Cross(n.ToVector3(), relVel.ToVector3()), n.ToVector3()).ToVector2();
t = relVel - n*Vector2.Dot(relVel, n);
if (t != Vector2.Zero) t.Normalize();
var u = (a.Friction + b.Friction) / 2;
dataA.dVel = (impulse * n + impulse * u * t) * a.InvMass;
dataB.dVel = (-impulse * n + impulse * u * t) * b.InvMass;
dataA.dAngVel = (Vector3.Cross(radA.ToVector3(), (impulse * n.ToVector3() + impulse * u * t.ToVector3()))).Z * a.GetInverseInertiaMoment();
dataB.dAngVel = (Vector3.Cross(radB.ToVector3(), (-impulse * n.ToVector3() + impulse * u * t.ToVector3()))).Z * b.GetInverseInertiaMoment();
}
public static void CalcCollision(Contact c, out ObjCollisionData dataA, out ObjCollisionData dataB)
{
dataA.dVel = Vector2.Zero;
dataB.dVel = Vector2.Zero;
dataA.dAngVel = 0;
dataB.dAngVel = 0;
dataA.dPos = Vector2.Zero;
dataB.dPos = Vector2.Zero;
var con = c.ConPoint;
var a = c.A;
var b = c.B;
var disp = c.Disp;
var Ia = a.GetInertiaMoment();
var Ib = b.GetInertiaMoment();
var Ia_inv = a.GetInverseInertiaMoment();
var Ib_inv = b.GetInverseInertiaMoment();
var axis = c.Axis; //from A->B
var n = -axis; //points from B->A
var n3 = n.ToVector3();
var u = (a.Friction + b.Friction) / 2;
var res = (a.Restitution + b.Restitution) / 2;
//TODO: limit the rest. if sep vel is slow
//var totInvMass = a.InvMass + b.InvMass;
//dataA.dPos = +disp * axis * a.InvMass / totInvMass;
//dataB.dPos = -disp * axis * b.InvMass / totInvMass;
var relPosA = (con - a.Position).ToVector3();
var relPosB = (con - b.Position).ToVector3();
Vector3 velA, velB, tvelA, tvelB;
tvelA = Vector3.Cross(a.AngularVelocity * Vector3.UnitZ, relPosA); // tangentVel = (contactpt - centroid)x(angVel) // remember angVel is really on the Z axis.
tvelB = Vector3.Cross(b.AngularVelocity * Vector3.UnitZ, relPosB); //also, remember position is the position of the axis of rotation (centroid, or fixed point). which works our well.
velA = a.Velocity.ToVector3() + tvelA; //velocity of contact point = (vel of object) + (tangential vel of pt due to ang_vel)
velB = b.Velocity.ToVector3() + tvelB;
var relVel = velB - velA;
var t = relVel - n3 * Vector3.Dot(relVel, n3);
if (t != Vector3.Zero)
t.Normalize();
else
t = n.Perpen().ToVector3();
Matrix localToWorld = CreateOrthonormalBasis(n, n.Perpen());
Matrix worldToLocal = Matrix.Transpose(localToWorld);
var sepVel = velA - velB;
var conVel = sepVel.TransformToLocal(worldToLocal);
var desiredVel = -conVel.X * (1 + res);
//
//Friction-collision calcs
//
var impulseToTorqueA = CreateSkewSymmetric(relPosA);
/*var torquePerUnitImpulseA = impulseToTorqueA * localToWorld;
var angVelPerUnitImpulseA = torquePerUnitImpulseA * a.GetInverseInertiaMoment();
var velPerUnitImpulseA = -angVelPerUnitImpulseA * impulseToTorqueA;
var velPerUnitImpulseContactA = worldToLocal * velPerUnitImpulseA; //used to transform an impulse in contact coordinates -> velocity in contact coordinates
*/
var impulseToTorqueB = CreateSkewSymmetric(relPosB);
/*var torquePerUnitImpulseB = impulseToTorqueB * localToWorld;
var angVelPerUnitImpulseB = torquePerUnitImpulseB * b.GetInverseInertiaMoment();
var velPerUnitImpulseB = -angVelPerUnitImpulseB * impulseToTorqueB;
var velPerUnitImpulseContactB = worldToLocal * velPerUnitImpulseB;
*/
var delVelWorldA = -1*(impulseToTorqueA * a.GetInverseInertiaMoment() * impulseToTorqueA); //matrix which transforms impluse -> velocity in world coordinates ie: Velocity Per Unit Impulse
var delVelWorldB = -1*(impulseToTorqueB * b.GetInverseInertiaMoment() * impulseToTorqueB);
var delVelWorld = delVelWorldA + delVelWorldB;
var delVelContact = worldToLocal * delVelWorld * localToWorld;
var invM = a.InvMass + b.InvMass;
delVelContact.M11 += invM;
delVelContact.M22 += invM;
delVelContact.M33 += invM;
MathNet.Numerics.LinearAlgebra.Matrix m = delVelContact.ToMathNet();
var mi = m.Inverse(); //from vel per impulse -> impulse per vel
Matrix impulsePerVel = mi.ToXNA();
//var velKill = (desiredVel * Vector3.UnitX); //NO FRICTION
var velKill = (desiredVel * Vector3.UnitX) - conVel.Y*Vector3.UnitY - conVel.Z*Vector3.UnitZ; //default = super mega-friction (ie: kill all talgential velocities
var impulseContact = Vector3.TransformNormal(velKill, impulsePerVel);
var planarImp = (float)Math.Sqrt(impulseContact.Y * impulseContact.Y + impulseContact.Z * impulseContact.Z);
if (planarImp != 0 && planarImp > impulseContact.X * u)
{
//handle dynamic friction
impulseContact.Y /= planarImp; //basically normalize (preserve direction, set len (y,x)=0
impulseContact.Z /= planarImp;
impulseContact.X = desiredVel / (delVelContact.M11 + delVelContact.M12 * u * impulseContact.Y + delVelContact.M13 * u * impulseContact.Z); //shorhand for the matrix transform
impulseContact.Y *= u * impulseContact.X; //scale up the impluses as friction*normal (since impCon.X = normal)
impulseContact.Z *= u * impulseContact.X;
}
//
//impulse from ImpulseContact + apply
//
var impulse = impulseContact.TransformToWorld(localToWorld);
var impulse2 = impulse.ToVector2();
dataA.dVel = impulse2 * a.InvMass;
dataB.dVel = -impulse2 * b.InvMass;
var impulsiveTorqueA = Vector3.Cross(relPosA, impulse);
var impulsiveTorqueB = Vector3.Cross(relPosB, -impulse);
dataA.dAngVel = impulsiveTorqueA.Z * a.GetInverseInertiaMoment();
dataB.dAngVel = impulsiveTorqueB.Z * b.GetInverseInertiaMoment();
}