public static bool sphereAndHalfSpace(
SphereRigid sphere,
CollisionPlane plane,
ref CollisionData data
)
{
// Make sure we have contacts
if (data.contactsLeft <= 0)
{
return(false);
}
// Cache the sphere position
Vector3 position = sphere.PositionCenterEngine;
// Find the distance from the plane
float ballDistance =
Vector3.Dot(plane.Direction, position) -
sphere.Radius - plane.Offset;
if (ballDistance >= 0)
{
return(false);
}
Contact c = new Contact();
c.ContactNormal = plane.Direction;
c.Penetration = -ballDistance;
c.ContactPoint =
position - plane.Direction * (ballDistance + sphere.Radius);
c.SetBodyData(sphere, null,
data.friction, data.restitution);
c.ContactToWorld.M11 = plane.Direction.X;
c.ContactToWorld.M12 = -plane.Direction.Y;
c.ContactToWorld.M21 = plane.Direction.Y;
c.ContactToWorld.M22 = plane.Direction.X;
data.contacts.Add(c);
sphere.SetCanSleep(true);
data.addContacts(1);
return(true);
}
static bool IntersectionTestboxAndHalfSpace(
BoxRigid box,
CollisionPlane plane
)
{
// Work out the projected radius of the box onto the plane direction
float projectedRadius = transformToAxis(box, plane.Direction);
// Work out how far the box is from the origin
float boxDistance =
Vector3.Dot(plane.Direction,
box.PositionCenterEngine) -
projectedRadius;
// Check for the intersection
return(boxDistance <= plane.Offset);
}
public static bool boxAndHalfSpace(
BoxRigid box,
CollisionPlane plane,
ref CollisionData data
)
{
// Make sure we have contacts
if (data.contactsLeft <= 0)
{
return(false);
}
// Check for intersection
if (!IntersectionTestboxAndHalfSpace(box, plane))
{
return(false);
}
bool b = false;
int contactsUsed = 0;
for (int i = 0; i < 4; i++)
{
Vector3 vertexPos = ((BoxRigid)box).vertices[i].Position;
// Calculate the distance from the plane
float vertexDistance = Vector3.Dot(vertexPos, plane.Direction);
// Compare this to the plane's distance
if (vertexDistance <= plane.Offset)
{
b = true;
// 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.
Contact c = new Contact();
c.ContactPoint = plane.Direction;
c.ContactPoint *= (0.5f * (plane.Offset - vertexDistance));
c.ContactPoint = vertexPos;
c.ContactNormal = plane.Direction;
c.Penetration = plane.Offset - vertexDistance;
c.Particle[0] = box;
c.Particle[1] = null;
c.Friction = data.friction;
c.Restitution = data.restitution;
c.Friction = StaticData.FrictionTable[(int)plane.Material][(int)box.GetMaterial()];
c.Restitution = StaticData.RestitutionTable[(int)plane.Material][(int)box.GetMaterial()];
c.ContactToWorld.M11 = plane.Direction.X;
c.ContactToWorld.M12 = -plane.Direction.Y;
c.ContactToWorld.M21 = plane.Direction.Y;
c.ContactToWorld.M22 = plane.Direction.X;
data.contacts.Add(c);
box.SetCanSleep(true);
data.index++;
data.contactsLeft--;
data.contactCount++;
contactsUsed++;
}
}
return(b);
}