internal void ResolveAABBAABBStatic(PhysicsBody2D BodyS, PhysicsBody2D BodyD)
{
Shapes.AABB BA = BodyS.shape as Shapes.AABB;
Shapes.AABB BB = BodyD.shape as Shapes.AABB;
Vector3 delta = BB.transform.Position - BA.transform.Position;
Vector3 tD = (BA.Dimensions + BB.Dimensions) / 2;
if (Math.Abs(delta.X) > Math.Abs(delta.Z))
{
if (delta.X < 0)
{// Left
BodyD.transform.parent.Position = BodyS.transform.Position - new Vector3(tD.X, 0, 0);
}
else
{// Right
BodyD.transform.parent.Position = BodyS.transform.Position + new Vector3(tD.X, 0, 0);
}
}
else
{
if (delta.Z < 0)
{// Top
BodyD.transform.parent.Position = BodyS.transform.Position - new Vector3(0, 0, tD.Z);
}
else
{// Bottom
BodyD.transform.parent.Position = BodyS.transform.Position + new Vector3(0, 0, tD.Z);
}
}
}
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 ResolveAABBAABBSimple()
{
Shapes.AABB BA = BodyA.shape as Shapes.AABB;
Shapes.AABB BB = BodyB.shape as Shapes.AABB;
float tM = BodyA.Mass + BodyB.Mass;
float mA = BodyA.Mass / tM;
float mB = BodyB.Mass / tM;
// Calculate a new center of mass for the system, and offset them both their radius away from this center of mass, along the normal
Vector3 newCOM = BodyA.transform.Position * mA + BodyB.transform.Position * mB;
Vector3 delta = BB.transform.Position - BA.transform.Position;
Vector3 tD = (BA.Dimensions + BB.Dimensions) / 2;
if (Math.Abs(delta.X) > Math.Abs(delta.Z))
{
if (delta.X < 0)
{// Left
BodyA.transform.parent.Position = newCOM + new Vector3(tD.X * mA, 0, 0);
BodyB.transform.parent.Position = newCOM - new Vector3(tD.X * mB, 0, 0);
}
else
{// Right
BodyA.transform.parent.Position = newCOM - new Vector3(tD.X * mA, 0, 0);
BodyB.transform.parent.Position = newCOM + new Vector3(tD.X * mB, 0, 0);
}
}
else
{
if (delta.Z < 0)
{// Top
BodyA.transform.parent.Position = newCOM + new Vector3(0, 0, tD.Z * mA);
BodyB.transform.parent.Position = newCOM - new Vector3(0, 0, tD.Z * mB);
}
else
{// Bottom
BodyA.transform.parent.Position = newCOM - new Vector3(0, 0, tD.Z * mA);
BodyB.transform.parent.Position = newCOM + new Vector3(0, 0, tD.Z * mB);
}
}
}
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;
}
}
}
