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