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