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