public static void ResolvePenetration_Cheap(RigidBody a, RigidBody b, Vector2 axis, float disp)
        {
            ObjPenetrationData pA = new ObjPenetrationData();
            ObjPenetrationData pB = new ObjPenetrationData();

            var totInvMass = a.InvMass + b.InvMass;
            pA.dPos = +disp * axis * a.InvMass / totInvMass;
            pB.dPos = -disp * axis * b.InvMass / totInvMass;

            pA.dRot = 0f;
            pB.dRot = 0f;

            a.PenData.Add(pA);
            b.PenData.Add(pB);
        }
 public static void ApplyPenData(RigidBody body, ObjPenetrationData penD)
 {
     body.SetMasterPosition(body.Position + penD.dPos);
     body.SetMasterRotation(body.Rotation + penD.dRot);
 }
        /// <summary>
        /// should exit NOT MODIFYING the system, but adding a ObjCollisionData to each body.PenData
        /// </summary>
        /// <param name="pcon"></param>
        public static void ResolvePenetration(ContactP pcon, int iter)
        {
            //Queue<Vector2> cons = new Queue<Vector2>(pcon.ConPoints);
            //TODO: Priority Queue

            var psA = pcon.A.GetState(); //previous state of A
            var psB = pcon.B.GetState(); //previous state of B

            //while(pcon.ConPoints.Count > 0) //TODO: put limits on this

            /*for (int i = 0; i < iter && pcon.ConPoints.Count > 0; i++)
            {

                resolveSingleCon(pcon.A, pcon.B, pcon.ConPoints[0], pcon.Axis, pcon.Disp);

                var newcons = new ContactP();
                if (testAndGenCon(pcon.A, pcon.B, out newcons))
                {
                    pcon.ConPoints = newcons.ConPoints;
                }
                else
                {
                    break;
                }
            }*/

            foreach (var conp in pcon.ConPoints)
            {
                resolveSingleCon(pcon.A, pcon.B, conp, pcon.Axis, pcon.Disp);
            }

            ObjPenetrationData pdataA = new ObjPenetrationData();
            pdataA.dPos = pcon.A.Position - psA.Position;
            pdataA.dRot = pcon.A.Rotation - psA.Rotation;
            pcon.A.SetState(psA); //revert to pre-penetration simulation state

            ObjPenetrationData pdataB = new ObjPenetrationData();
            pdataB.dPos = pcon.B.Position - psB.Position;
            pdataB.dRot = pcon.B.Rotation - psB.Rotation;
            pcon.B.SetState(psB);

            pcon.A.PenData.Add(pdataA);
            pcon.B.PenData.Add(pdataB);
        }
        public static ObjPenetrationData AggrPenData(List<ObjPenetrationData> cols, float soft)
        {
            ObjPenetrationData dat = new ObjPenetrationData();
            float totWeight = 0f;
            foreach (var c in cols)
            {
                var weight = 1; // c.dPos.Length();

                dat.dPos += weight * c.dPos;
                dat.dRot += weight * c.dRot;
                totWeight += weight;
            }
            if (totWeight == 0f) totWeight = 1;
            dat.dPos /= totWeight * soft; //TODO: make this a const
            dat.dRot /= totWeight * soft;

            return dat;
        }