internal void ResolveCircleAABBSimple()
{
// TODO Circle AABB
Shapes.Circle BA = (BodyA.shape is Shapes.Circle) ? BodyA.shape as Shapes.Circle : BodyB.shape as Shapes.Circle;
Shapes.AABB BB = (BodyA.shape is Shapes.Circle) ? BodyB.shape as Shapes.AABB : BodyA.shape as Shapes.AABB;
Vector3 dN = Vector3.Normalize(BA.lastOverlap_delta);
float theta = (float)Math.Atan(dN.Z / dN.X);
float length = BB.LengthAtAngle(theta);
// Time to move the circle and the aabb away from each other along the normal
// Calculate a new center of mass for the system, and offset them both their radii away from this center of mass, along the normal, based on their mass percentage of the system
float dR = length + BA.Radius;
float tM = BodyA.Mass + BodyB.Mass;
float mA = BodyA.Mass / tM;
float mB = BodyB.Mass / tM;
Vector3 newCOM = BodyA.transform.Position * mA + BodyB.transform.Position * mB;
BodyA.transform.parent.Position = newCOM - (dN * dR * (1 - mA));
BodyB.transform.parent.Position = newCOM + (dN * dR * (1 - mB));
// Calculate the resulting velocities based on the restitution scalars for both bodies
if (BodyA.Velocity.LengthSquared() + BodyB.Velocity.LengthSquared() > 0)
{
float projection = (2 * (Vector3.Dot(BodyA.Velocity * BodyA.Restitution, dN) - Vector3.Dot(BodyB.Velocity * BodyB.Restitution, dN))) / (tM);
if (BodyA.shape is Shapes.AABB)
{
BodyA.Velocity = BodyA.Velocity - projection * BodyA.Mass * dN;
if (Math.Abs(dN.X) > Math.Abs(dN.Y))
{
BodyB.Velocity = new Vector3(-BodyB.Velocity.X, BodyB.Velocity.Y, BodyB.Velocity.Z);
}
else
{
BodyB.Velocity = new Vector3(BodyB.Velocity.X, BodyB.Velocity.Y, -BodyB.Velocity.Z);
}
}
else
{
BodyB.Velocity = BodyB.Velocity + projection * BodyB.Mass * dN;
if (Math.Abs(dN.X) > Math.Abs(dN.Y))
{
BodyA.Velocity = new Vector3(-BodyA.Velocity.X, BodyA.Velocity.Y, BodyA.Velocity.Z);
}
else
{
BodyA.Velocity = new Vector3(BodyA.Velocity.X, BodyA.Velocity.Y, -BodyA.Velocity.Z);
}
}
}
}
internal void ResolveCircleAABBStatic(PhysicsBody2D BodyS, PhysicsBody2D BodyD)
{
// TODO Circle AABB
Shapes.Circle BA = (BodyS.shape is Shapes.Circle) ? BodyS.shape as Shapes.Circle : BodyD.shape as Shapes.Circle;
Shapes.AABB BB = (BodyS.shape is Shapes.Circle) ? BodyD.shape as Shapes.AABB : BodyS.shape as Shapes.AABB;
// Might as well normalize the delta while we are at it
Vector3 dN = Vector3.Normalize(BA.lastOverlap_delta);
float theta = (float)Math.Atan(dN.Z / dN.X);
float length = BB.LengthAtAngle(theta);
// Time to move the circle and the aabb away from each other along the normal
// Calculate a new center of mass for the system, and offset them both their radii away from this center of mass, along the normal, based on their mass percentage of the system
float dR = length + BA.Radius;
BodyD.transform.parent.Position = BodyS.transform.Position + (dN * dR);
if (BodyB.Velocity.LengthSquared() > 0 && Vector3.Dot(Vector3.Normalize(BodyB.Velocity), dN) > 0)
{
float projection = 2 * -Vector3.Dot(BodyB.Velocity * MathHelper.Min(BodyB.Restitution, BodyA.Restitution), dN);
if (BodyA.shape is Shapes.AABB)
{
if (Math.Abs(dN.X) > Math.Abs(dN.Y))
{
BodyB.Velocity = new Vector3(-BodyB.Velocity.X, BodyB.Velocity.Y, BodyB.Velocity.Z);
}
else
{
BodyB.Velocity = new Vector3(BodyB.Velocity.X, BodyB.Velocity.Y, -BodyB.Velocity.Z);
}
}
else
{
BodyB.Velocity = BodyB.Velocity + projection * BodyB.Mass * dN;
}
}
}