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; }