public static void ResolvePenetration_Cheap(ContactP pcon) { var a = pcon.A; var b = pcon.B; var axis = pcon.Axis; var disp = pcon.Disp; ResolvePenetration_Cheap(a, b, axis, disp); }
/// <summary> /// generates a contact (and a list of subcontacts) for a pair of rigid bodies /// </summary> /// <param name="a"></param> /// <param name="b"></param> /// <param name="con"></param> /// <returns></returns> private static bool testAndGenCon(RigidBody a, RigidBody b, out ContactP con) { Vector2 axis; float disp; con = new ContactP(); if (!(a.IsFixed && b.IsFixed) && TestCollisionBody(a, b, out axis, out disp)) { var pcons = FindPenetratingPoints(a, b, axis); con = new ContactP(a, b, axis, disp); con.ConPoints = pcons; return true; } return false; }
/// <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); }
/// <summary> /// calculates contacts between all bodies, but skips enumeration of subcontacts /// </summary> /// <param name="bodies"></param> /// <returns></returns> private static List<ContactP> calcPens_Lite(List<RigidBody> bodies) { var pens = new List<ContactP>(500); Enumerable.Range(0, bodies.Count).PForEach(i => { for (int j = i + 1; j < bodies.Count; j++) { var a = bodies[i]; var b = bodies[j]; ContactP con = new ContactP(); Vector2 axis; float disp; if (!(a.IsFixed && b.IsFixed) && !(a.IsVegetated() && b.IsVegetated()) && TestCollisionBody(a, b, out axis, out disp)) { con = new ContactP(a, b, axis, disp); pens.Add(con); } } }); return pens; }