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 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 BoxAndHalfSpace(CollisionBox box, CollisionPlane plane, CollisionData data)
{
// Make sure we have contacts
if (data.NoMoreContacts())
{
return;
}
// Check for intersection
if (!IntersectionTests.BoxAndHalfSpace(box, plane))
{
return;
}
// We have an intersection, so find the intersection points. We can make
// do with only checking vertices. If the box is resting on a plane
// or on an edge, it will be reported as four or two contact points.
// Go through each combination of + and - for each half-size
int[,] mults = new int[, ]
{
{ 1, 1, 1 }, { -1, 1, 1 }, { 1, -1, 1 }, { -1, -1, 1 },
{ 1, 1, -1 }, { -1, 1, -1 }, { 1, -1, -1 }, { -1, -1, -1 }
};
for (int i = 0; i < 8; i++)
{
// Calculate the position of each vertex
Vector3d vertexPos = new Vector3d(mults[i, 0], mults[i, 1], mults[i, 2]);
vertexPos *= box.HalfSize;
vertexPos = box.Transform.Transform(vertexPos);
// Calculate the distance from the plane
double vertexDistance = Vector3d.Dot(vertexPos, plane.Direction);
// Compare this to the plane's distance
if (vertexDistance <= plane.Offset)
{
// Create the contact data.
// The contact point is halfway between the vertex and the
// plane - we multiply the direction by half the separation
// distance and add the vertex location.
var contact = data.GetContact();
contact.ContactPoint = plane.Direction;
contact.ContactPoint *= (vertexDistance - plane.Offset);
contact.ContactPoint += vertexPos;
contact.ContactNormal = plane.Direction;
contact.Penetration = plane.Offset - vertexDistance;
contact.SetBodyData(box.Body, null, data.Friction, data.Restitution);
if (!data.HasMoreContacts())
{
return;
}
}
}
}
public static bool BoxAndHalfSpace(CollisionBox box, CollisionPlane plane)
{
// Work out the projected radius of the box onto the plane direction
double projectedRadius = TransformToAxis(box, plane.Direction);
// Work out how far the box is from the origin
double boxDistance = Vector3d.Dot(plane.Direction, box.GetAxis(3)) - projectedRadius;
// Check for the intersection
return(boxDistance <= 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);
}
