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