public static void SphereAndSphere(CollisionSphere one, CollisionSphere two, CollisionData data)
{
// Make sure we have contacts
if (data.NoMoreContacts())
{
return;
}
// Cache the sphere positions
Vector3d positionOne = one.GetAxis(3);
Vector3d positionTwo = two.GetAxis(3);
// Find the vector between the objects
Vector3d midline = positionOne - positionTwo;
double size = midline.Magnitude;
// See if it is large enough.
if (size <= 0.0f || size >= one.Radius + two.Radius)
{
return;
}
// We manually create the normal, because we have the
// size to hand.
Vector3d normal = midline * (1.0 / size);
var contact = data.GetContact();
contact.ContactNormal = normal;
contact.ContactPoint = positionOne + midline * 0.5;
contact.Penetration = one.Radius + two.Radius - size;
contact.SetBodyData(one.Body, two.Body, data.Friction, data.Restitution);
}
public static void SphereAndHalfSpace(CollisionSphere sphere, CollisionPlane plane, CollisionData data)
{
// Make sure we have contacts
if (data.NoMoreContacts())
{
return;
}
// Cache the sphere position
Vector3d position = sphere.GetAxis(3);
// Find the distance from the plane
double dist = Vector3d.Dot(plane.Direction, position) - sphere.Radius - plane.Offset;
if (dist >= 0)
{
return;
}
// Create the contact - it has a normal in the plane direction.
var contact = data.GetContact();
contact.ContactNormal = plane.Direction;
contact.Penetration = -dist;
contact.ContactPoint = position - plane.Direction * (dist + sphere.Radius);
contact.SetBodyData(sphere.Body, null, data.Friction, data.Restitution);
}
public static bool SphereAndSphere(CollisionSphere one, CollisionSphere two)
{
// Find the vector between the objects
Vector3d midline = one.GetAxis(3) - two.GetAxis(3);
// See if it is large enough.
return(midline.SqrMagnitude < (one.Radius + two.Radius) * (one.Radius + two.Radius));
}
public static bool SphereAndHalfSpace(CollisionSphere sphere, CollisionPlane plane)
{
// Find the distance from the origin
double ballDistance = Vector3d.Dot(plane.Direction, sphere.GetAxis(3)) - sphere.Radius;
// Check for the intersection
return(ballDistance <= plane.Offset);
}
public static void SphereAndTruePlane(CollisionSphere sphere, CollisionPlane plane, CollisionData data)
{
// Make sure we have contacts
if (data.NoMoreContacts())
{
return;
}
// Cache the sphere position
Vector3d position = sphere.GetAxis(3);
// Find the distance from the plane
double centreDistance = Vector3d.Dot(plane.Direction, position) - plane.Offset;
// Check if we're within radius
if (centreDistance * centreDistance > sphere.Radius * sphere.Radius)
{
return;
}
// Check which side of the plane we're on
Vector3d normal = plane.Direction;
double penetration = -centreDistance;
if (centreDistance < 0)
{
normal *= -1;
penetration = -penetration;
}
penetration += sphere.Radius;
// Create the contact - it has a normal in the plane direction.
var contact = data.GetContact();
contact.ContactNormal = normal;
contact.Penetration = penetration;
contact.ContactPoint = position - plane.Direction * centreDistance;
contact.SetBodyData(sphere.Body, null, data.Friction, data.Restitution);
}
public static void BoxAndSphere(CollisionBox box, CollisionSphere sphere, CollisionData data)
{
// Make sure we have contacts
if (data.NoMoreContacts())
{
return;
}
// Transform the centre of the sphere into box coordinates
Vector3d centre = sphere.GetAxis(3);
Vector3d relCentre = box.Transform.TransformInverse(centre);
// Early out check to see if we can exclude the contact
if (Math.Abs(relCentre.x) - sphere.Radius > box.HalfSize.x ||
Math.Abs(relCentre.y) - sphere.Radius > box.HalfSize.y ||
Math.Abs(relCentre.z) - sphere.Radius > box.HalfSize.z)
{
return;
}
Vector3d closestPt = new Vector3d(0, 0, 0);
double dist;
// Clamp each coordinate to the box.
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;
// Check we're in contact
dist = (closestPt - relCentre).SqrMagnitude;
if (dist > sphere.Radius * sphere.Radius)
{
return;
}
// Compile the contact
Vector3d closestPtWorld = box.Transform.Transform(closestPt);
var contact = data.GetContact();
contact.ContactNormal = (closestPtWorld - centre).Normalized;
contact.ContactPoint = closestPtWorld;
contact.Penetration = sphere.Radius - Math.Sqrt(dist);
contact.SetBodyData(box.Body, sphere.Body, data.Friction, data.Restitution);
}
