public static int SphereAndTruePlane(CollisionSphere sphere, CollisionPlane plane, ref CollisionData data)
{
if (data.ContactsLeft <= 0) return 0;
Vector3 position = sphere.GetAxis(3);
float centreDistance = Vector3.Dot(plane.Normal, position) - plane.D;
if (centreDistance * centreDistance > sphere.Radius * sphere.Radius)
{
return 0;
}
Vector3 normal = plane.Normal;
float penetration = -centreDistance;
if (centreDistance < 0)
{
normal *= -1;
penetration = -penetration;
}
penetration += sphere.Radius;
Contact contact = new Contact();
contact.ContactNormal = normal;
contact.Penetration = penetration;
contact.ContactPoint = position - plane.Normal * centreDistance;
contact.SetData(sphere.Body, null, data.Friction, data.Restitution);
data.Contacts.Add(contact);
data.AddContacts(1);
return 1;
}
public static int SphereAndHalfSpace(CollisionSphere sphere, CollisionPlane plane, ref CollisionData data)
{
if (data.ContactsLeft <= 0) return 0;
Vector3 position = sphere.GetAxis(3);
float ballDistance = Vector3.Dot(plane.Normal, position) - sphere.Radius - plane.D;
if (ballDistance >= 0) return 0;
Contact contact = new Contact();
contact.ContactNormal = plane.Normal;
contact.Penetration = -ballDistance;
contact.ContactPoint = position - plane.Normal * (ballDistance + sphere.Radius);
contact.SetData(sphere.Body, null, data.Friction, data.Restitution);
data.Contacts.Add(contact);
data.AddContacts(1);
return 1;
}
public static int SphereAndSphere(CollisionSphere one, CollisionSphere two, ref CollisionData data)
{
if (data.ContactsLeft <= 0) return 0;
Vector3 positionOne = one.GetAxis(3);
Vector3 positionTwo = two.GetAxis(3);
Vector3 midline = positionOne - positionTwo;
float size = midline.Magnitude;
if (size <= 0.0f || size >= one.Radius + two.Radius)
{
return 0;
}
Vector3 normal = midline * ((1.0f) / size);
Contact contact = new Contact();
contact.ContactNormal = normal;
contact.ContactPoint = positionOne + midline * (float)0.5;
contact.Penetration = (one.Radius + two.Radius - size);
contact.SetData(one.Body, two.Body, data.Friction, data.Restitution);
data.Contacts.Add(new Contact());
data.AddContacts(1);
return 1;
}
public static bool SphereAndSphere(CollisionSphere one, CollisionSphere two)
{
Vector3 midline = one.GetAxis(3) - two.GetAxis(3);
return midline.SquaredMagnitude < (one.Radius + two.Radius) * (one.Radius + two.Radius);
}
public static bool SphereAndHalfSpace(CollisionSphere sphere, CollisionPlane plane)
{
float ballDistance = Vector3.Dot(plane.Normal, sphere.GetAxis(3)) - sphere.Radius;
return ballDistance <= plane.D;
}
public static int BoxAndSphere(CollisionBox box, CollisionSphere sphere, ref CollisionData data)
{
Vector3 centre = sphere.GetAxis(3);
Vector3 relCentre = box.Transform.TransformInverse(centre);
if (MathHelper.Abs(relCentre.X) - sphere.Radius > box.HalfSize.X ||
MathHelper.Abs(relCentre.Y) - sphere.Radius > box.HalfSize.Y ||
MathHelper.Abs(relCentre.Z) - sphere.Radius > box.HalfSize.Z)
{
return 0;
}
Vector3 closestPt = new Vector3();
float dist;
dist = relCentre.X;
if (dist > box.HalfSize.X) dist = box.HalfSize.X;
if (dist < -box.HalfSize.X) dist = -box.HalfSize.X;
closestPt.X = dist;
dist = relCentre.Y;
if (dist > box.HalfSize.Y) dist = box.HalfSize.Y;
if (dist < -box.HalfSize.Y) dist = -box.HalfSize.Y;
closestPt.Y = dist;
dist = relCentre.Z;
if (dist > box.HalfSize.Z) dist = box.HalfSize.Z;
if (dist < -box.HalfSize.Z) dist = -box.HalfSize.Z;
closestPt.Z = dist;
dist = (closestPt - relCentre).SquaredMagnitude;
if (dist > sphere.Radius * sphere.Radius) return 0;
Vector3 closestPtWorld = box.Transform.Transform(closestPt);
Contact contact = new Contact();
contact.ContactNormal = (closestPtWorld - centre);
contact.ContactNormal.Normalize();
contact.ContactPoint = closestPtWorld;
contact.Penetration = sphere.Radius - MathHelper.Sqrt(dist);
contact.SetData(box.Body, sphere.Body,
data.Friction, data.Restitution);
data.Contacts.Add(contact);
data.AddContacts(1);
return 1;
}
