public static bool TestNarrowPolyPoly(IPolyBody targetP, IPolyBody impactP, out Contact contact)
{
contact.TargetBody = targetP;
contact.ImpactBody = impactP;
contact.Disp = 0;
contact.Normal = Vector2.Zero;
contact.ConPoint = Vector2.Zero;
throw new NotImplementedException();
}
public static bool TestNarrowCircleCircle(ICircleBody targetC, ICircleBody impactC, out Contact contact)
{
contact.TargetBody = targetC;
contact.ImpactBody = impactC;
contact.Disp = 0;
contact.Normal = Vector2.Zero;
contact.ConPoint = Vector2.Zero;
float disp = (impactC.Position - targetC.Position).Length() - targetC.Radius - impactC.Radius;
if (disp <= 0)
{
var norm = Vector2.Normalize(impactC.Position - targetC.Position);
var conpt = targetC.Position + norm * (targetC.Radius + disp / 2); //go out to edge, then in halfway (disp is negative)
contact.Disp = disp;
contact.Normal = norm;
contact.ConPoint = conpt;
return true;
}
else
return false;
}
/// <summary>
///
/// </summary>
/// <param name="targetB"></param>
/// <param name="impactB"></param>
/// <param name="contact"></param>
/// <returns></returns>
public static bool TestNarrowCollision(IRigidBody targetB, IRigidBody impactB, out Contact contact)
{
contact.TargetBody = targetB;
contact.ImpactBody = impactB;
contact.Disp = 0; //default vals so compiler doesn't complain
contact.Normal = Vector2.Zero;
contact.ConPoint = Vector2.Zero;
if (targetB is ICircleBody && impactB is ICircleBody)
{
return TestNarrowCircleCircle((ICircleBody)targetB, (ICircleBody)impactB, out contact);
}
else if (targetB is ICircleBody && impactB is IPolyBody)
{
return TestNarrowCirclePoly((ICircleBody)targetB, (IPolyBody)impactB, out contact);
}
else if (targetB is IPolyBody && impactB is ICircleBody)
{
var b = TestNarrowCirclePoly((ICircleBody)impactB, (IPolyBody)targetB, out contact);
//invert all contact attributes, since we passed inverted targetB/impactB pair
var tmp = contact.TargetBody;
contact.TargetBody = contact.ImpactBody;
contact.ImpactBody = tmp;
contact.Disp *= -1;
contact.Normal *= -1;
return b;
}
else if (targetB is IPolyBody && impactB is IPolyBody)
{
return TestNarrowPolyPoly((IPolyBody)targetB, (IPolyBody)impactB, out contact);
}
//EVERYTHING BELOW THIS POINT IS HAX UNTIL WE HAVE POLY
else if (targetB is ICircleBody && impactB is IVertWallBody)
{
var targetC = (ICircleBody)targetB;
var impactW = (IVertWallBody)impactB;
var BB = impactW.BoundingBox;
if (targetC.Position.Y + targetC.Radius > BB.YMin && targetC.Position.Y - targetC.Radius < BB.YMax)
{
if (targetC.Position.X - targetC.Radius < BB.XMax && targetC.Position.X - targetC.Radius > BB.XMin) //right hand wall
{
contact.Disp = -(BB.XMax - (targetC.Position.X - targetC.Radius));
contact.Normal = -Vector2.UnitX; //from circle to wall
return true;
}
else if (targetC.Position.X + targetC.Radius > BB.XMin && targetC.Position.X + targetC.Radius < BB.XMax) //left hand wall
{
contact.Disp = -((targetC.Position.X + targetC.Radius) - BB.XMin);
contact.Normal = Vector2.UnitX;
return true;
}
contact.ConPoint = targetC.Position + contact.Normal * (targetC.Radius + contact.Disp / 2);
}
return false;
}
else if (targetB is ICircleBody && impactB is IHorizWallBody)
{
var targetC = (ICircleBody)targetB;
var impactW = (IHorizWallBody)impactB;
var BB = impactW.BoundingBox;
if (targetC.Position.X + targetC.Radius > BB.XMin && targetC.Position.X - targetC.Radius < BB.XMax)
{
if (targetC.Position.Y - targetC.Radius < BB.YMax && targetC.Position.Y - targetC.Radius > BB.YMin) //bottom wall
{
contact.Disp = -(BB.YMax - (targetC.Position.Y - targetC.Radius));
contact.Normal = -Vector2.UnitY; //from circle to wall
return true;
}
else if (targetC.Position.Y + targetC.Radius > BB.YMin && targetC.Position.Y + targetC.Radius < BB.YMax) //top wall
{
contact.Disp = -((targetC.Position.Y + targetC.Radius) - BB.YMin);
contact.Normal = Vector2.UnitY;
return true;
}
contact.ConPoint = targetC.Position + contact.Normal * (targetC.Radius + contact.Disp / 2);
}
return false;
}
else
{
throw new NotImplementedException();
//SAT
//sub-contact detection
//...etc.
}
}
public static CollisionData CalcCollision(Contact con)
{
var cdata = new CollisionData();
cdata.TargetBody = con.TargetBody;
cdata.dPos = Vector2.Zero;
cdata.dVel = Vector2.Zero;
cdata.dAngVel = 0;
var A = con.TargetBody;
var B = con.ImpactBody;
//penetration resolution
var totalInvMass = A.InvMass() + B.InvMass();
cdata.dPos = (con.Disp * A.InvMass() / (totalInvMass)) * con.Normal; //more massive-> less weight. objects in collision share burden of displacing proportional to 1/M.
//collision response
var sepVel = Vector2.Dot(B.Velocity - A.Velocity, con.Normal); //separating velocity: relative velocity of impact body along normal.
if (sepVel >= 0) return cdata; //return no change if bodies are separating
float rest = (A.Restitution+B.Restitution)/2;
float impulse = sepVel * (1 + rest) / (totalInvMass);
cdata.dVel = (impulse * A.InvMass()) * con.Normal;
return cdata;
}
