public int FindContacts(Primitive primOne, Primitive primTwo)
{
CollisionData data = new CollisionData();
data.contactsLeft = 1;
int numContacts = FindContacts(primOne, primTwo, data);
return numContacts;
}
public void IntersectionTests_BoxVsBox_Faces()
{
NoBody topBody = new NoBody(Matrix.CreateTranslation(new Vector3(1.5f, 2.0f, 0.25f)));
Box top = new Box(
topBody,
Matrix.Identity,
new Vector3(1f, 1f, 1f));
NoBody bottomBody =
new NoBody(Matrix.CreateTranslation(new Vector3(0f, 0f, 0f)));
Box bottom = new Box(
bottomBody,
Matrix.Identity,
new Vector3(1f, 1f, 1f));
CollisionData data = new CollisionData();
int contactsFound = IntersectionTests.boxAndBox(top, bottom, data);
Assert.AreEqual(1, contactsFound);
Contact contact = data.contacts[0];
Assert.AreEqual(new Vector3(1f, 1f, 1f), contact.Point);
Assert.AreEqual(Vector3.Up, contact.Normal);
Assert.AreEqual(0f, contact.Penetration);
}
public void IntersectionTests_BoxVsBox_NotTouching()
{
Vector3 leftPos = new Vector3(-100f, -100f, -100f);
IRigidBody leftBody = new NoBody(Matrix.CreateTranslation(leftPos));
Box left = new Box(leftBody, Matrix.Identity, new Vector3(1f, 1f, 1f));
Vector3 rightPos = new Vector3(100f, 100f, 100f);
IRigidBody rightBody = new NoBody(Matrix.CreateTranslation(rightPos));
Box right = new Box(rightBody, Matrix.Identity, new Vector3(1f, 1f, 1f));
CollisionData data = new CollisionData();
int contactsFound = IntersectionTests.boxAndBox(left, right, data);
Assert.AreEqual(0, contactsFound);
Assert.AreEqual(0, data.contacts.Count);
}
public static int edgeAndEdge(
Box left, Vector3 leftEdgeOne, Vector3 leftEdgeTwo,
Box right, Vector3 rightEdgeOne, Vector3 rightEdgeTwo,
CollisionData data)
{
//return 0; //TODO
Vector3 leftCenter = left.Body.Position;
Vector3 rightCenter = right.Body.Position;
Vector3 midLineRight = rightCenter - leftCenter;
float distCenters = midLineRight.Length();
Vector3 projectedLeftEdge = TrickyMath.Project(leftEdgeOne - leftCenter, midLineRight);
float distLeftCenterToEdge = projectedLeftEdge.Length();
Vector3 projectedRightEdge = TrickyMath.Project(rightEdgeOne - rightCenter, -midLineRight);
float distRightCenterToEdge = projectedRightEdge.Length();
if (distCenters > (distLeftCenterToEdge + distRightCenterToEdge))
{
return 0;
}
float distEdges = (distLeftCenterToEdge + distRightCenterToEdge) - distCenters;
Contact contact = new Contact();
contact.DiscoveryHint = ContactDiscoveryHint.BoxOnBox_Edge_Edge;
contact.Restitution = data.restitution;
contact.Friction = data.friction;
contact.Bodies[0] = left.Body;
contact.Bodies[1] = right.Body;
contact.Penetration = (distLeftCenterToEdge + distRightCenterToEdge) - distCenters;
contact.Point = leftCenter + (midLineRight * 0.5f);
Vector3 x = leftEdgeOne - leftEdgeTwo; ;
x.Normalize();
Vector3 y, z;
TrickyMath.MakeOrthonormalBasis(x, out y, out z);
Vector3 realY = y;
Vector3 normal = TrickyMath.VectorProduct(x, realY);
normal.Normalize();
contact.Normal = normal;
data.contacts.Add(contact);
return 1;
}
public static Contact checkEdgeAndEdge(
Box left, Vector3 leftEdgeOne, Vector3 leftEdgeTwo,
Box right, Vector3 rightEdgeOne, Vector3 rightEdgeTwo, float restitution, float friction)
{
return null;
CollisionData pointData = new CollisionData();
pointData.restitution = restitution;
pointData.friction = friction;
int pointContacts = IntersectionTests.edgeAndEdge(left, leftEdgeOne, leftEdgeTwo, right, rightEdgeOne, rightEdgeTwo, pointData);
if (pointContacts > 0)
{
return pointData.contacts[0];
}
return null;
}
public static int boxAndSphere(Box box, Sphere sph, CollisionData data)
{
Vector3 center = sph.Body.Position;
Vector3 relCenter = Vector3.Transform(center, Matrix.Invert(box.Transform));
double x = relCenter.X;
double y = relCenter.Y;
double z = relCenter.Z;
if (Math.Abs(x) - sph.Radius > box.HalfSizes.X
|| Math.Abs(y) - sph.Radius > box.HalfSizes.Y
|| Math.Abs(z) - sph.Radius > box.HalfSizes.Z)
{
return 0;
}
Vector3 closest = Vector3.Zero;
float dist;
dist = relCenter.X;
if (dist > box.HalfSizes.X) dist = box.HalfSizes.X;
if (dist < -box.HalfSizes.X) dist = -box.HalfSizes.X;
closest.X = dist;
dist = relCenter.Y;
if (dist > box.HalfSizes.Y) dist = box.HalfSizes.Y;
if (dist < -box.HalfSizes.Y) dist = -box.HalfSizes.Y;
closest.Y = dist;
dist = relCenter.Z;
if (dist > box.HalfSizes.Z) dist = box.HalfSizes.Z;
if (dist < -box.HalfSizes.Z) dist = -box.HalfSizes.Z;
closest.Z = dist;
dist = (closest - relCenter).Length();
if (dist > sph.Radius && !TrickyMath.AlmostEquals(0f, dist - sph.Radius)) return 0;
Vector3 closestWorld = Vector3.Transform(closest, box.Transform);
Contact contact = new Contact();
Vector3 normal = closestWorld - center;// (center - closestWorld);
normal.Normalize();
contact.Normal = normal;
Debug.Sanity(contact.Normal);
contact.Point = closestWorld;
contact.Penetration = sph.Radius - dist;// (float)Math.Sqrt((float)dist);
contact.Bodies[0] = box.Body;
contact.Bodies[1] = sph.Body;
contact.Restitution = data.restitution;
contact.Friction = data.friction;
data.contacts.Add(contact);
return 1;
}
//TODO - write unit tests!!!!!!
public static int boxAndPoint(Box box, Vector3 pointWorld, CollisionData data)
{
Vector3 pointLocal = Vector3.Transform(pointWorld, Matrix.Invert(box.Body.World * box.OffsetMatrix));
Vector3 halfSizes = box.HalfSizes;
Vector3 absPoint = new Vector3(
TrickyMath.Abs(pointLocal.X),
TrickyMath.Abs(pointLocal.Y),
TrickyMath.Abs(pointLocal.Z)
);
Vector3 check = halfSizes - absPoint;
if (check.X < 0 || check.Y < 0 || check.Z < 0)
{
return 0;
}
Vector3 depth = new Vector3(
TrickyMath.Abs(absPoint.X - halfSizes.X),
TrickyMath.Abs(absPoint.Y - halfSizes.Y),
TrickyMath.Abs(absPoint.Z - halfSizes.Z)
);
depth = new Vector3(
depth.X != 0f ? depth.X : float.MaxValue,
depth.Y != 0f ? depth.Y : float.MaxValue,
depth.Z != 0f ? depth.Z : float.MaxValue
);
if (depth.X <= depth.Y && depth.X <= depth.Z)
{
Contact contact = new Contact();
contact.DiscoveryHint = ContactDiscoveryHint.BoxOnBox_Point_Face;
Vector3 boxAxis = box.getAxis(0);
contact.Normal = boxAxis * (pointLocal.X > 0 ? -1f : 1f);
contact.Point = pointWorld;
contact.Penetration = TrickyMath.Abs(absPoint.X - halfSizes.X);
contact.Bodies[0] = box.Body;
contact.Bodies[1] = null;
contact.Restitution = data.restitution;
contact.Friction = data.friction;
data.contacts.Add(contact);
return 1;
}
else if (depth.Y <= depth.X && depth.Y <= depth.Z)
{
Contact contact = new Contact();
contact.DiscoveryHint = ContactDiscoveryHint.BoxOnBox_Point_Face;
Vector3 boxAxis = box.getAxis(1);
contact.Normal = boxAxis * (pointLocal.Y > 0 ? -1f : 1f);
contact.Point = pointWorld;
contact.Penetration = TrickyMath.Abs(absPoint.Y - halfSizes.Y);
contact.Bodies[0] = box.Body;
contact.Bodies[1] = null;
contact.Restitution = data.restitution;
contact.Friction = data.friction;
data.contacts.Add(contact);
return 1;
}
else if (depth.Z <= depth.X && depth.Z <= depth.Y)
{
Contact contact = new Contact();
contact.DiscoveryHint = ContactDiscoveryHint.BoxOnBox_Point_Face;
Vector3 boxAxis = box.getAxis(2);
contact.Normal = boxAxis * (pointLocal.Z > 0 ? -1f : 1f);
contact.Point = pointWorld;
contact.Penetration = TrickyMath.Abs(absPoint.Z - halfSizes.Z);
contact.Bodies[0] = box.Body;
contact.Bodies[1] = null;
contact.Restitution = data.restitution;
contact.Friction = data.friction;
data.contacts.Add(contact);
return 1;
}
else
{
return 0;
}
}
public static int sphereAndSphere(Sphere one, Sphere two, CollisionData data)
{
//make sure we have contacts
if (data.contactsLeft <= 0) return 0;
// Cache the sphere positions
Vector3 positionOne = one.Body.Position;
Vector3 positionTwo = two.Body.Position;
if (positionOne == positionTwo)
{
throw new ArgumentException("Two bodies with the same position are unsupported....TODO");
}
// find the vector between the objects
Vector3 midline = positionOne - positionTwo;
float size = midline.Length();
//see if it is large enough
if (size <= 0.0f || size > one.Radius + two.Radius)
{
return 0;
}
// we manually create the normal, because we have the size at hand;
Vector3 normal = midline * (1f / size);
Contact contact = new Contact();
contact.Normal = normal;
contact.Point = positionTwo + midline * 0.5f;
contact.Penetration = (one.Radius + two.Radius - size);
//write the appropriate data
contact.Bodies[0] = one.Body;
contact.Bodies[1] = two.Body;
contact.Restitution = data.restitution;
contact.Friction = data.friction;
data.contacts.Add(contact);
return 1;
}
private static void fillPointFaceBoxBox(Box one, Box two, Vector3 toCenter, CollisionData data, int best, float penetration)
{
Contact contact = new Contact();
Vector3 normal = one.getAxis(best);
if (Vector3.Dot(normal, toCenter) > 0)
{
normal *= -1f;
}
Vector3 vertex = two.HalfSizes;
float oneDot = Vector3.Dot(two.getAxis(0), normal);
if (oneDot < 0)
{
vertex *= new Vector3(-1f, 0f, 0f);
}
float twoDot = Vector3.Dot(two.getAxis(1), normal);
if (twoDot < 0)
{
vertex *= new Vector3(0f, -1f, 0f);
}
float threeDot = Vector3.Dot(two.getAxis(2), normal);
if (threeDot < 0)
{
vertex *= new Vector3(0f, 0f, -1f);
}
contact.DiscoveryHint = ContactDiscoveryHint.BoxOnBox_Point_Face;
contact.Normal = normal;
contact.Penetration = penetration;
contact.Point = Vector3.Transform(vertex, two.Transform);
contact.Bodies[0] = one.Body;
contact.Bodies[1] = two.Body;
contact.Restitution = data.restitution;
contact.Friction = data.friction;
data.contacts.Add(contact);
}
public void IntersectionTests_BoxVsPlane()
{
IRigidBody planeBody = new NoBody();
Box box;
Forever.Physics.Collide.Plane plane;
CollisionData data;
Vector3 planeNormal = Vector3.Up;
// Plane is x-z plane for all tests
//////// unit box is sitting at {0,1,0} (surface touching)
box = new Box(new NoBody( Matrix.CreateTranslation(new Vector3(0f, 1f, 0f))), Matrix.Identity, new Vector3(1f, 1f, 1f));
plane = new Forever.Physics.Collide.Plane(planeBody, Vector3.Zero, planeNormal);
data = new CollisionData();
int contactsFound = IntersectionTests.boxAndPlane(box, plane, data);
Assert.AreEqual(4, contactsFound);
Contact contact1 = data.contacts[0];
Contact contact2 = data.contacts[1];
Contact contact3 = data.contacts[2];
Contact contact4 = data.contacts[3];
Assert.AreEqual(new Vector3(-1f, 0f, -1f), contact1.Point);
Assert.AreEqual(planeNormal, contact1.Normal);
Assert.AreEqual(0, contact1.Penetration);
Assert.AreEqual(new Vector3(-1f, 0f, 1f), contact2.Point);
Assert.AreEqual(planeNormal, contact2.Normal);
Assert.AreEqual(0, contact2.Penetration);
Assert.AreEqual(new Vector3(1f, 0f, -1f), contact3.Point);
Assert.AreEqual(planeNormal, contact3.Normal);
Assert.AreEqual(0, contact3.Penetration);
Assert.AreEqual(new Vector3(1f, 0f, 1f), contact4.Point);
Assert.AreEqual(planeNormal, contact4.Normal);
Assert.AreEqual(0, contact4.Penetration);
//////// unit box is centered at origin
box = new Box(new NoBody(Matrix.CreateTranslation(new Vector3(0f, 0f, 0f))), Matrix.Identity, new Vector3(1f, 1f, 1f));
plane = new Forever.Physics.Collide.Plane(planeBody, Vector3.Zero, planeNormal);
data = new CollisionData();
contactsFound = IntersectionTests.boxAndPlane(box, plane, data);
Assert.AreEqual(4, contactsFound);
contact1 = data.contacts[0];
contact2 = data.contacts[1];
contact3 = data.contacts[2];
contact4 = data.contacts[3];
Assert.AreEqual(new Vector3(-1f, 0f, -1f), contact1.Point);
Assert.AreEqual(planeNormal, contact1.Normal);
Assert.AreEqual(1f, contact1.Penetration);
Assert.AreEqual(new Vector3(-1f, 0f, 1f), contact2.Point);
Assert.AreEqual(planeNormal, contact2.Normal);
Assert.AreEqual(1f, contact2.Penetration);
Assert.AreEqual(new Vector3(1f, 0f, -1f), contact3.Point);
Assert.AreEqual(planeNormal, contact3.Normal);
Assert.AreEqual(1f, contact3.Penetration);
Assert.AreEqual(new Vector3(1f, 0f, 1f), contact4.Point);
Assert.AreEqual(planeNormal, contact4.Normal);
Assert.AreEqual(1f, contact4.Penetration);
//// unit box centered at y=1 and rotated 45 degrees on each axis
float angle = (float)Math.PI * 0.25f;
Matrix boxMatrix = Matrix.CreateFromYawPitchRoll(angle, angle, angle) * Matrix.CreateTranslation(new Vector3(0f, 1f, 0f));
box = new Box(new NoBody(boxMatrix), Matrix.Identity, new Vector3(1f, 1f, 1f));
plane = new Forever.Physics.Collide.Plane(planeBody, Vector3.Zero, planeNormal);
data = new CollisionData();
contactsFound = IntersectionTests.boxAndPlane(box, plane, data);
Assert.AreEqual(1, contactsFound);
contact1 = data.contacts[0];
Assert.True(
TrickyMath.AlmostEquals(0.7071067f, contact1.Penetration)
);
Assert.True(
TrickyMath.CloseEnoughToBeSame(
new Vector3(-0.2071069f, 0f, -0.2071068f),
contact1.Point));
////unit box centered at y=sqrt(2) and rotated 45 degrees on each axis
boxMatrix = Matrix.CreateFromYawPitchRoll(angle, angle, angle) * Matrix.CreateTranslation(new Vector3(0f, TrickyMath.Sqrt(2), 0f));
box = new Box(new NoBody(boxMatrix), Matrix.Identity, new Vector3(1f, 1f, 1f));
plane = new Forever.Physics.Collide.Plane(planeBody, Vector3.Zero, planeNormal);
data = new CollisionData();
contactsFound = IntersectionTests.boxAndPlane(box, plane, data);
Assert.AreEqual(1, contactsFound);
contact1 = data.contacts[0];
Assert.True(
TrickyMath.AlmostEquals(0.292893142f,contact1.Penetration)
);
Assert.True(
TrickyMath.CloseEnoughToBeSame(
new Vector3(-0.2071069f, 0f, -0.2071068f),
contact1.Point
)
);
Assert.AreEqual(Vector3.UnitY, contact1.Normal);
}
public void IntersectionTests_SphereVsSphere_SurfaceCollision()
{
Vector3 sphereOnePosition = Vector3.Zero;
IRigidBody sphereOneBody = new NoBody(Matrix.CreateTranslation(sphereOnePosition));
float sphereOneRadius = 0.5f;
Matrix sphereOneMatrix = Matrix.Identity;
Sphere sphereOne = new Sphere(sphereOneBody, sphereOneMatrix, sphereOneRadius);
Vector3 sphereTwoPosition = Vector3.UnitX;
IRigidBody sphereTwoBody = new NoBody(Matrix.CreateTranslation(sphereTwoPosition));
float sphereTwoRadius = 0.5f;
Matrix sphereTwoMatrix = Matrix.Identity;
Sphere sphereTwo = new Sphere(sphereTwoBody, sphereTwoMatrix, sphereTwoRadius);
CollisionData data = new CollisionData();
Assert.AreEqual(1, IntersectionTests.sphereAndSphere(sphereOne, sphereTwo, data), "How many contacts?");
Contact contact;
Assert.NotNull(data.contacts, "Contacts Container");
Assert.AreEqual(1, data.contacts.Count, "One Contact Expected");
contact = data.contacts[0];
Assert.AreEqual(0, contact.Friction, "0 friction");
Assert.AreEqual(1f, contact.Restitution, "1 restitution");
Assert.AreEqual(0, contact.Penetration, "0 penetration (surface collision)");
Assert.AreEqual(new Vector3(0.5f, 0f, 0f), contact.Point, "expected contact point");
Assert.AreEqual(-Vector3.UnitX, contact.Normal, "expected contact normal");
//Assymetric
data = new CollisionData();
IntersectionTests.sphereAndSphere(sphereTwo, sphereOne, data);
Assert.NotNull(data.contacts, "Contacts Container");
Assert.AreEqual(1, data.contacts.Count, "One Contact Expected");
contact = data.contacts[0];
Assert.AreEqual(0, contact.Friction, "0 friction");
Assert.AreEqual(1, contact.Restitution, "1 restitution");
Assert.AreEqual(0, contact.Penetration, "0 penetration (surface collision)");
Assert.AreEqual(new Vector3(0.5f, 0f, 0f), contact.Point, "expected contact point");
Assert.AreEqual(Vector3.UnitX, contact.Normal, "expected contact normal");
}
public void IntersectionTests_SphereVsSphere_BodyPointIntersection()
{
IRigidBody sphereOneBody = new NoBody();
float sphereOneRadius = 1f;
Matrix sphereOneMatrix = Matrix.Identity;
Sphere sphereOne = new Sphere(sphereOneBody, sphereOneMatrix, sphereOneRadius);
IRigidBody sphereTwoBody = new NoBody();
float sphereTwoRadius = 1f;
Matrix sphereTwoMatrix = Matrix.Identity;
Sphere sphereTwo = new Sphere(sphereTwoBody, sphereTwoMatrix, sphereTwoRadius);
CollisionData data = new CollisionData();
IntersectionTests.sphereAndSphere(sphereOne, sphereTwo, data);
}
public void IntersectionTests_SphereVsSphere_NoIntersection()
{
IRigidBody sphereOneBody = new NoBody(Matrix.CreateTranslation(Vector3.UnitX * -2f));
float sphereOneRadius = 1f;
Matrix sphereOneMatrix = Matrix.Identity;
Sphere sphereOne = new Sphere(sphereOneBody, sphereOneMatrix, sphereOneRadius);
IRigidBody sphereTwoBody = new NoBody(Matrix.CreateTranslation(Vector3.UnitX * 2f));
float sphereTwoRadius = 1f;
Matrix sphereTwoMatrix = Matrix.Identity;
Sphere sphereTwo = new Sphere(sphereTwoBody, sphereTwoMatrix, sphereTwoRadius);
CollisionData data = new CollisionData();
Assert.AreEqual(0, IntersectionTests.sphereAndSphere(sphereOne, sphereTwo, data));
}
public void IntersectionTests_SphereVsPlane()
{
Matrix sphereMatrix;
float sphereRadius = 1f;
Forever.Physics.Collide.Plane plane;
Forever.Physics.Collide.Sphere sphere;
CollisionData data;
int contactsFound;
Contact contact1;
// plane is always the x-z plane for this test method
Vector3 planeNormal = Vector3.Up;
plane = new Forever.Physics.Collide.Plane(new NoBody(), Vector3.Zero, planeNormal);
// unit sphere centered at y=1 (one point of surface touching)
sphereMatrix = Matrix.CreateTranslation(Vector3.UnitY);
sphere = new Sphere( new NoBody(sphereMatrix), Matrix.Identity, sphereRadius);
data = new CollisionData();
contactsFound = IntersectionTests.sphereAndHalfSpace(sphere, plane, data);
Assert.AreEqual(1, contactsFound);
contact1 = data.contacts[0];
Assert.AreEqual(Vector3.Zero, contact1.Point);
Assert.AreEqual(0f, contact1.Penetration);
Assert.AreEqual(planeNormal, contact1.Normal);
// unit sphere centered at y=0.9 (.1 penetration)
sphereMatrix = Matrix.CreateTranslation(Vector3.UnitY * 0.9f);
sphere = new Sphere(new NoBody(sphereMatrix), Matrix.Identity, sphereRadius);
data = new CollisionData();
contactsFound = IntersectionTests.sphereAndHalfSpace(sphere, plane, data);
Assert.AreEqual(1, contactsFound);
contact1 = data.contacts[0];
Assert.AreEqual(Vector3.Zero, contact1.Point);
Assert.True(
TrickyMath.AlmostEquals(0.1f, contact1.Penetration)
);
Assert.AreEqual(planeNormal, contact1.Normal);
}
public void IntersectionTests_BoxVsSphere_NoIntersection()
{
float sphereRadius = 1f;
CollisionData data;
int contactsFound;
Contact contact1;
Forever.Physics.Collide.Sphere sphere;
Forever.Physics.Collide.Box box;
// no intersection
sphere = new Sphere(
new NoBody( Matrix.CreateTranslation(new Vector3(-10000f, -10000f, -10000f))),
Matrix.Identity,
sphereRadius
);
box = new Box(
new NoBody(Matrix.CreateTranslation(new Vector3(10000f, 10000f, 10000f))),
Matrix.Identity,
new Vector3(1f, 1f, 1f)
);
data = new CollisionData();
contactsFound = IntersectionTests.boxAndSphere(box, sphere, data);
Assert.AreEqual(0, contactsFound);
}
public void IntersectionTests_BoxVsSphere_BoxFace()
{
//box face intersection
float sphereRadius = 1f;
CollisionData data;
int contactsFound;
Contact contact1;
Forever.Physics.Collide.Sphere sphere;
Forever.Physics.Collide.Box box;
sphere = new Sphere(
new NoBody(Matrix.CreateTranslation(Vector3.Up)),
Matrix.Identity,
sphereRadius);
box = new Box(
new NoBody(Matrix.CreateTranslation(Vector3.Down * 0.95f)),
Matrix.Identity,
new Vector3(1f, 1f, 1f));
data = new CollisionData();
contactsFound = IntersectionTests.boxAndSphere(box, sphere, data);
Assert.AreEqual(1, contactsFound);
contact1 = data.contacts[0];
Assert.True(
TrickyMath.AlmostEquals(0.05f, contact1.Penetration)
);
Assert.True(
TrickyMath.CloseEnoughToBeSame(new Vector3(0f, 0.05f, 0f), contact1.Point)
);
Assert.AreEqual(Vector3.Down, contact1.Normal);
}
//dispatch
public static int primAndPrim(Primitive one, Primitive two, CollisionData data)
{
if (one.PrimType == CollideType.Sphere
&& two.PrimType == CollideType.Sphere)
{
return sphereAndSphere((Sphere)one, (Sphere)two, data);
}
if (one.PrimType == CollideType.Sphere
&& two.PrimType == CollideType.Plane)
{
return sphereAndHalfSpace((Sphere)one, (Plane)two, data);
}
if (one.PrimType == CollideType.Plane
&& two.PrimType == CollideType.Sphere)
{
return sphereAndHalfSpace((Sphere)two, (Plane)one, data);
}
if (one.PrimType == CollideType.Box
&& two.PrimType == CollideType.Sphere)
{
return boxAndSphere((Box)one, (Sphere)two, data);
}
if (one.PrimType == CollideType.Sphere
&& two.PrimType == CollideType.Box)
{
return boxAndSphere((Box)two, (Sphere)one, data);
}
if (one.PrimType == CollideType.Box
&& two.PrimType == CollideType.Plane)
{
return boxAndPlane((Box)one, (Plane)two, data);
}
if (one.PrimType == CollideType.Plane
&& two.PrimType == CollideType.Box)
{
return boxAndPlane((Box)two, (Plane)one, data);
}
if (one.PrimType == CollideType.Box && two.PrimType == CollideType.Box)
{
return boxAndBox((Box)one, (Box)two, data);
}
throw new Exception("I don't know wtf that is!");
}
public static int boxAndBoxBetter(Box left, Box right, CollisionData data)
{
Vector3[] leftWorldVerts = left.WorldVerts();
Vector3[] rightWorldVerts = right.WorldVerts();
foreach (Vector3 leftWorldVert in leftWorldVerts)
{
Contact contact = IntersectionTests.checkBoxAndPoint(right, leftWorldVert, data.restitution, data.friction);
if (contact != null)
{
contact.Bodies[1] = left.Body;
data.contacts.Add(contact);
}
}
if (data.contacts.Count > 0)
{
return data.contacts.Count;
}
foreach (Vector3 rightWorldVert in rightWorldVerts)
{
Contact contact = IntersectionTests.checkBoxAndPoint(left, rightWorldVert, data.restitution, data.friction);
if (contact != null)
{
contact.Bodies[1] = right.Body;
data.contacts.Add(contact);
}
}
// If any point/face contacts get precedent over edge/edge
if (data.contacts.Count > 0)
{
return data.contacts.Count;
}
List<Vector3[]> leftEdges = left.WorldEdges();
List<Vector3[]> rightEdges = right.WorldEdges();
foreach (Vector3[] leftEdge in leftEdges)
{
foreach (Vector3[] rightEdge in rightEdges)
{
Contact contact = checkEdgeAndEdge(left, leftEdge[0], leftEdge[1], right, rightEdge[0], rightEdge[1], data.restitution, data.friction);
if (contact != null)
{
data.contacts.Add(contact);
}
}
}
return data.contacts.Count;
}
public static int sphereAndHalfSpace(Sphere sphere, Plane plane, CollisionData data)
{
Vector3 pos = sphere.Body.Position;
//d = p * L - l
float ballDist = Vector3.Dot(plane.Normal, pos) - (sphere.Radius - plane.Offset);
if (ballDist > 0) return 0;
Contact contact = new Contact();
contact.Normal = plane.Normal;
contact.Penetration = -ballDist;
contact.Point = pos - (plane.Normal * (ballDist + sphere.Radius));
contact.Bodies[0] = sphere.Body;
contact.Bodies[1] = null;
contact.Restitution = data.restitution;
contact.Friction = data.friction;
data.contacts.Add(contact);
return 1;
}
public int FindContacts(Primitive primOne, Primitive primTwo, CollisionData data)
{
return IntersectionTests.primAndPrim(primOne, primTwo, data);
}
private static Contact checkBoxAndPoint(Box box, Vector3 point, float restitution, float friction)
{
CollisionData pointData = new CollisionData();
pointData.restitution = restitution;
pointData.friction = friction;
int pointContacts = IntersectionTests.boxAndPoint(box, point, pointData);
if (pointContacts > 0)
{
return pointData.contacts[0];
}
return null;
}
public void IntersectionTests_BoxVsPoint()
{
IRigidBody body = new NoBody();
Box box;
Vector3 point;
CollisionData data;
Contact contact;
box = new Box(body, Matrix.Identity, new Vector3(1f, 1f, 1f));
////////////////////////// point@+,+,+
point = new Vector3(0.75f, 1f, 1f);
data = new CollisionData();
data.contactsLeft = 1;
Assert.AreEqual(1, IntersectionTests.boxAndPoint(box, point, data));
contact = data.contacts[0];
Assert.AreEqual(Vector3.Left, contact.Normal);
Assert.AreEqual(0.25f, contact.Penetration);
Assert.AreEqual(1, data.contacts.Count);
Assert.AreEqual(point, contact.Point);
Assert.Null(contact.Bodies[1]);
///////////////////////////// point@center when box is translated
Vector3 translatedPos = Vector3.UnitY * 10f;
body = new NoBody(Matrix.CreateTranslation(translatedPos));
box = new Box(body, Matrix.Identity, new Vector3(1f, 1f, 1f));
point = translatedPos;
data = new CollisionData();
data.contactsLeft = 1;
Assert.AreEqual(1, IntersectionTests.boxAndPoint(box, point, data));
contact = data.contacts[0];
Assert.AreEqual(1f, contact.Penetration);
Assert.AreEqual(1, data.contacts.Count);
Assert.AreEqual(point, contact.Point);
Assert.AreEqual(Vector3.Right, contact.Normal);
Assert.Null(contact.Bodies[1]);
/////////////////////////////////////point@+,+,+ when box is translated
point = translatedPos + Vector3.Right * 0.75f;
data = new CollisionData();
data.contactsLeft = 1;
Assert.AreEqual(1, IntersectionTests.boxAndPoint(box, point, data));
contact = data.contacts[0];
Assert.AreEqual(0.25f, contact.Penetration);
Assert.AreEqual(1, data.contacts.Count);
Assert.AreEqual(point, contact.Point);
Vector3 normal = Vector3.Left;
Assert.AreEqual(normal, contact.Normal);
Assert.Null(contact.Bodies[1]);
//////////////////////////////////////////point@tip(+,+,+)
body = new NoBody();
box = new Box(body, Matrix.Identity, new Vector3(1f, 1f, 1f));
point = new Vector3(1f, 1f, 1f);
data = new CollisionData();
data.contactsLeft = 1;
Assert.AreEqual(1, IntersectionTests.boxAndPoint(box, point, data));
contact = data.contacts[0];
Assert.AreEqual(0f, contact.Penetration);
Assert.AreEqual(1, data.contacts.Count);
Assert.AreEqual(point, contact.Point);
Assert.AreEqual(Vector3.Left, contact.Normal);
Assert.Null(contact.Bodies[1]);
/////////////////////////////////////////point@tip(-,-,-)
point = new Vector3(-1f, -1f, -1f);
data = new CollisionData();
data.contactsLeft = 1;
Assert.AreEqual(1, IntersectionTests.boxAndPoint(box, point, data));
contact = data.contacts[0];
Assert.AreEqual(0f, contact.Penetration);
Assert.AreEqual(1, data.contacts.Count);
Assert.AreEqual(point, contact.Point);
Assert.AreEqual(Vector3.Right, contact.Normal);
Assert.Null(contact.Bodies[1]);
/////////////////////////////////////////point@middle of nowhere
point = new Vector3(10000f, 10000f, 10000f);
data = new CollisionData();
data.contactsLeft = 1;
Assert.AreEqual(0, IntersectionTests.boxAndPoint(box, point, data));
/////////////////////////////////////////point@+,0,0 after box is rotated
float rotY = (float)Math.PI / 4f; //45 degrees
point = new Vector3(1.001f, 0f, 0f);
body = new NoBody(Matrix.CreateRotationY(rotY));
box = new Box(body, Matrix.Identity, new Vector3(1f, 1f, 1f));
data = new CollisionData();
Assert.AreEqual(1, IntersectionTests.boxAndPoint(box, point, data));
Assert.AreEqual(1, data.contacts.Count);
contact = data.contacts[0];
Assert.True(
TrickyMath.CloseEnoughToBeSame(contact.Normal, new Vector3(-0.7071068f, 0, 0.7071068f) )
);
/////////////////////////////////////////////////////point@+,0,0 after box is rotated other direction
point = new Vector3(1.001f, 0f, 0f);
body = new NoBody(Matrix.CreateRotationY(-rotY));
box = new Box(body, Matrix.Identity, new Vector3(1f, 1f, 1f));
data = new CollisionData();
Assert.AreEqual(1, IntersectionTests.boxAndPoint(box, point, data));
Assert.AreEqual(1, data.contacts.Count);
contact = data.contacts[0];
Assert.True(
TrickyMath.CloseEnoughToBeSame(contact.Normal, new Vector3(-0.7071068f, 0, -0.7071068f))
);
}
public static int boxAndBox(Box one, Box two, CollisionData data)
{
Vector3 toCenter = two.Body.Position - one.Body.Position;
float pen = float.MaxValue;
int best = int.MaxValue;
if (!tryAxis(one, two, one.getAxis(0), toCenter, 0, ref pen, ref best)) return 0;
if (!tryAxis(one, two, one.getAxis(1), toCenter, 1, ref pen, ref best)) return 0;
if (!tryAxis(one, two, one.getAxis(2), toCenter, 2, ref pen, ref best)) return 0;
if (!tryAxis(one, two, one.getAxis(0), toCenter, 3, ref pen, ref best)) return 0;
if (!tryAxis(one, two, one.getAxis(1), toCenter, 4, ref pen, ref best)) return 0;
if (!tryAxis(one, two, one.getAxis(2), toCenter, 5, ref pen, ref best)) return 0;
int bestSingleAxis = best;
if (!tryAxis(one, two, TrickyMath.VectorProduct(one.getAxis(0), two.getAxis(0)), toCenter, 6, ref pen, ref best)) return 0;
if (!tryAxis(one, two, TrickyMath.VectorProduct(one.getAxis(0), two.getAxis(1)), toCenter, 7, ref pen, ref best)) return 0;
if (!tryAxis(one, two, TrickyMath.VectorProduct(one.getAxis(0), two.getAxis(2)), toCenter, 8, ref pen, ref best)) return 0;
if (!tryAxis(one, two, TrickyMath.VectorProduct(one.getAxis(1), two.getAxis(0)), toCenter, 9, ref pen, ref best)) return 0;
if (!tryAxis(one, two, TrickyMath.VectorProduct(one.getAxis(1), two.getAxis(1)), toCenter, 10, ref pen, ref best)) return 0;
if (!tryAxis(one, two, TrickyMath.VectorProduct(one.getAxis(1), two.getAxis(2)), toCenter, 11, ref pen, ref best)) return 0;
if (!tryAxis(one, two, TrickyMath.VectorProduct(one.getAxis(2), two.getAxis(0)), toCenter, 12, ref pen, ref best)) return 0;
if (!tryAxis(one, two, TrickyMath.VectorProduct(one.getAxis(2), two.getAxis(1)), toCenter, 13, ref pen, ref best)) return 0;
if (!tryAxis(one, two, TrickyMath.VectorProduct(one.getAxis(2), two.getAxis(2)), toCenter, 14, ref pen, ref best)) return 0;
if (best == int.MaxValue)
{
throw new Exception("Found no good axis");
}
if (best < 3)
{
fillPointFaceBoxBox(one, two, toCenter, data, best, pen);
return 1;
}
else if (best < 6)
{
// We've got a vertex of box one on a face of box two.
// We use the same algorithm as above, but swap around
// one and two (and therefore also the vector between their
// centres).
fillPointFaceBoxBox(two, one, toCenter * -1.0f, data, best - 3, pen);
return 1;
}
else
{
best -= 6; //move this back to talk about a cardinal axis
int oneAxisIndex = best / 3;
int twoAxisIndex = best % 3;
Vector3 oneAxis = one.getAxis(oneAxisIndex);
Vector3 twoAxis = two.getAxis(twoAxisIndex);
Vector3 axis = TrickyMath.VectorProduct(oneAxis, twoAxis);
axis.Normalize();
if (Vector3.Dot(axis, toCenter) > 0)
{
axis *= -1f;
}
float[] ptOnOneEdge = TrickyMath.Vector3ToFloatArray(one.HalfSizes);
float[] ptOnTwoEdge = TrickyMath.Vector3ToFloatArray(two.HalfSizes);
for (int i = 0; i < 3; i++)
{
if (i == oneAxisIndex)
{
ptOnOneEdge[i] = 0f;
}
else if (Vector3.Dot(one.getAxis(i), axis) > 0)
{
ptOnOneEdge[i] *= -1f;
}
if (i == twoAxisIndex)
{
ptOnTwoEdge[i] = 0;
}
else if (Vector3.Dot(two.getAxis(i), axis) < 0)
{
ptOnTwoEdge[i] *= -1f;
}
}
Vector3 vertexOnOneEdge = Vector3.Transform(
TrickyMath.FloatArrayToVector3(ptOnOneEdge),
one.Transform
);
Vector3 vertexOnTwoEdge = Vector3.Transform(
TrickyMath.FloatArrayToVector3(ptOnTwoEdge),
two.Transform
);
float[] oneHalfSizes = TrickyMath.Vector3ToFloatArray(one.HalfSizes);
float[] twoHalfSizes = TrickyMath.Vector3ToFloatArray(two.HalfSizes);
Vector3 vertex = contactPoint(
vertexOnOneEdge, oneAxis, oneHalfSizes[oneAxisIndex],
vertexOnTwoEdge, twoAxis, twoHalfSizes[twoAxisIndex],
bestSingleAxis > 2
);
Contact contact = new Contact();
contact.DiscoveryHint = ContactDiscoveryHint.BoxOnBox_Edge_Edge;
contact.Penetration = pen;
contact.Normal = axis;
contact.Point = vertex;
contact.Bodies[0] = one.Body;
contact.Bodies[1] = two.Body;
contact.Restitution = data.restitution;
contact.Friction = data.friction;
data.contacts.Add(contact);
return 1;
}
return 0;
}
public void IntersectionTests_BoxVsSphere_BoxEdge_ZeroPenetration()
{
//box edge intersection
float sphereRadius = 1f;
CollisionData data;
int contactsFound;
Contact contact1;
Forever.Physics.Collide.Sphere sphere;
Forever.Physics.Collide.Box box;
box = new Box(
new NoBody(), //box centered at origin
Matrix.Identity,
new Vector3(1f, 1f, 1f)
);
Vector3 spherePos = new Vector3(0f, 1f, 1f);
spherePos.Normalize();
spherePos = new Vector3(0f, 1f, 1f) + spherePos;
sphere = new Sphere(
new NoBody(
Matrix.CreateTranslation(spherePos)
),
Matrix.Identity,
sphereRadius);
data = new CollisionData();
contactsFound = IntersectionTests.boxAndSphere(box, sphere, data);
Assert.AreEqual(1, contactsFound);
contact1 = data.contacts[0];
Assert.True(
TrickyMath.AlmostEquals(0f, contact1.Penetration)
);
Assert.AreEqual(new Vector3(0f, 1f, 1f), contact1.Point);
Assert.True(
TrickyMath.CloseEnoughToBeSame(
new Vector3(0f, -0.7071068f, -0.7071068f),
contact1.Normal)
);
}
private CollisionData ManageCollisions(CollisionDetector detect, ICollideable left, ICollideable right, float duration)
{
CollisionData data = new CollisionData();
data.restitution = this.Restitution;
data.friction = this.Friction;
detect.FindContacts(left.GeometryData.Prim, right.GeometryData.Prim, data);
return data;
}
/// <summary>
/// danger! This works for ContactDemo, but there is one hang up. The call to Math.Abs() should not
/// be needed. It isn't in the orange book text. However, we only get negative numbers from that Dot.
/// </summary>
/// <param name="box"></param>
/// <param name="plane"></param>
/// <param name="data"></param>
/// <returns></returns>
public static int boxAndPlane(Box box, Plane plane, CollisionData data)
{
if (data.contactsLeft <= 0) return 0;
int contacts_found = 0;
// p dot L < l
Vector3[] verts = box.WorldVerts();
foreach (Vector3 vert in verts)
{
//float vertexDist = (float)Math.Abs( (double)Vector3.Dot(plane.Normal, vert));
float vertexDist = Vector3.Dot(-plane.Normal, vert);
if (vertexDist >= plane.Offset + data.tolerance)
{
// the contact point is halfway between the vertex and the plane
// we multiply the direction by half the spearation distance
// and add the vertex location
Contact contact = new Contact();
contact.DiscoveryHint = ContactDiscoveryHint.BoxOnPlane_Corner;
contact.Bodies[0] = box.Body;
contact.Bodies[1] = null;
contact.Point = plane.ClosestPoint(vert);
contact.Normal = plane.Normal;
contact.Penetration = vertexDist - plane.Offset;//TrickyMathHelper.Abs(plane.Offset - vertexDist);
contact.Restitution = data.restitution;
contact.Friction = data.friction;
data.contacts.Add(contact);
contacts_found++;
}
}
return contacts_found;
}