public static int BoxAndHalfSpace(CollisionBox box, CollisionPlane plane, ref CollisionData data)
{
if (data.ContactsLeft <= 0) return 0;
if (!IntersectionTests.BoxAndHalfSpace(box, plane))
{
return 0;
}
int contactsUsed = 0;
for (int i = 0; i < 8; i++)
{
Vector3 vertexPos = new Vector3(mults[i, 0], mults[i, 1], mults[i, 2]);
vertexPos *= (box.HalfSize);
vertexPos = box.Transform.Transform(vertexPos);
float vertexDistance = Vector3.Dot(vertexPos, plane.Normal);
if (vertexDistance <= plane.D)
{
Contact contact = new Contact();
contact.ContactPoint = plane.Normal;
contact.ContactPoint *= (vertexDistance - plane.D);
contact.ContactPoint = vertexPos;
contact.ContactNormal = plane.Normal;
contact.Penetration = plane.D - vertexDistance;
contact.SetData(box.Body, null, data.Friction, data.Restitution);
data.Contacts.Add(contact);
if (contactsUsed == data.ContactsLeft) return contactsUsed;
}
}
data.AddContacts(contactsUsed);
return contactsUsed;
}
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 bool BoxAndHalfSpace(CollisionBox box, CollisionPlane plane)
{
float projectedRadius = transformToAxis(box, plane.Normal);
float boxDistance = Vector3.Dot(plane.Normal, box.GetAxis(3)) - projectedRadius;
return boxDistance <= plane.D;
}
public static bool SphereAndHalfSpace(CollisionSphere sphere, CollisionPlane plane)
{
float ballDistance = Vector3.Dot(plane.Normal, sphere.GetAxis(3)) - sphere.Radius;
return ballDistance <= plane.D;
}
void generateContacts()
{
CollisionPlane plane = new CollisionPlane(new Vector3(0, 1, 0).ToVector3(), 0);
cdata.Reset(256);
cdata.Friction = 0.9f;
cdata.Restitution = 0.1f;
cdata.Tolerance = 0.1f;
for (int i = 0; i < bodies.Count; i++)
{
if (!cdata.HasMoreContacts) return;
CollisionDetector.BoxAndHalfSpace(bodies[i], plane, ref cdata);
}
}
