internal static float transformToAxis(CollisionBox box, Vector3 axis) { return box.HalfSize.X * MathHelper.Abs(Vector3.Dot(axis, box.GetAxis(0))) + box.HalfSize.Y * MathHelper.Abs(Vector3.Dot(axis, box.GetAxis(1))) + box.HalfSize.Z * MathHelper.Abs(Vector3.Dot(axis, box.GetAxis(2))); }
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 BoxAndBox(CollisionBox one, CollisionBox two) { Vector3 toCenter = two.GetAxis(3) - one.GetAxis(3); return ( // Check on box one's axes first overlapOnAxis(one, two, one.GetAxis(0), toCenter) && overlapOnAxis(one, two,one.GetAxis(1), toCenter) && overlapOnAxis(one, two,one.GetAxis(2), toCenter) && // And on two's overlapOnAxis(one, two,two.GetAxis(0), toCenter) && overlapOnAxis(one, two,two.GetAxis(1), toCenter) && overlapOnAxis(one, two,two.GetAxis(2), toCenter) && // Now on the cross products overlapOnAxis(one, two,Vector3.Cross(one.GetAxis(0), two.GetAxis(0)), toCenter) && overlapOnAxis(one, two,Vector3.Cross(one.GetAxis(0), two.GetAxis(1)), toCenter) && overlapOnAxis(one, two,Vector3.Cross(one.GetAxis(0), two.GetAxis(2)), toCenter) && overlapOnAxis(one, two,Vector3.Cross(one.GetAxis(1), two.GetAxis(0)), toCenter) && overlapOnAxis(one, two,Vector3.Cross(one.GetAxis(1), two.GetAxis(1)), toCenter) && overlapOnAxis(one, two,Vector3.Cross(one.GetAxis(1), two.GetAxis(2)), toCenter) && overlapOnAxis(one, two,Vector3.Cross(one.GetAxis(2), two.GetAxis(0)), toCenter) && overlapOnAxis(one, two,Vector3.Cross(one.GetAxis(2), two.GetAxis(1)), toCenter) && overlapOnAxis(one, two,Vector3.Cross(one.GetAxis(2), two.GetAxis(2)), toCenter) ); }
static void fillPointFaceBoxBox(CollisionBox one, CollisionBox two, Vector3 toCentre, CollisionData data, int best, float pen) { Contact contact = new Contact(); Vector3 normal = one.GetAxis(best); if (Vector3.Dot(one.GetAxis(best), toCentre) > 0) { normal = normal * -1.0f; } Vector3 vertex = two.HalfSize; if (Vector3.Dot(two.GetAxis(0), normal) < 0) vertex.X = -vertex.X; if (Vector3.Dot(two.GetAxis(1), normal) < 0) vertex.Y = -vertex.Y; if (Vector3.Dot(two.GetAxis(2), normal) < 0) vertex.Z = -vertex.Z; contact.ContactNormal = normal; contact.Penetration = pen; contact.ContactPoint = Vector3.Transform(vertex, two.Transform); contact.SetData(one.Body, two.Body, data.Friction, data.Restitution); data.Contacts.Add(contact); }
public static int BoxAndPoint(CollisionBox box, Vector3 point, ref CollisionData data) { Vector3 relPt = box.Transform.TransformInverse(point); Vector3 normal = new Vector3(); float min_depth = box.HalfSize.X - MathHelper.Abs(relPt.X); if (min_depth < 0) return 0; normal = box.GetAxis(0) * ((relPt.X < 0) ? -1 : 1); float depth = box.HalfSize.Y - MathHelper.Abs(relPt.Y); if (depth < 0) return 0; else if (depth < min_depth) { min_depth = depth; normal = box.GetAxis(1) * ((relPt.Y < 0) ? -1 : 1); } depth = box.HalfSize.Z - MathHelper.Abs(relPt.Z); if (depth < 0) return 0; else if (depth < min_depth) { min_depth = depth; normal = box.GetAxis(2) * ((relPt.Z < 0) ? -1 : 1); } Contact contact = new Contact(); contact.ContactNormal = normal; contact.ContactPoint = point; contact.Penetration = min_depth; contact.SetData(box.Body, null, data.Friction, data.Restitution); data.Contacts.Add(contact); data.AddContacts(1); return 1; }
public static int BoxAndBox(CollisionBox one, CollisionBox two, ref CollisionData data) { Vector3 toCenter = two.GetAxis(3) - one.GetAxis(3); float pen = float.MaxValue; int best = 0xffffff; CHECK_OVERLAP(one.GetAxis(0), 0, one, two, toCenter, ref pen, ref best); CHECK_OVERLAP(one.GetAxis(1), 1, one, two, toCenter, ref pen, ref best); CHECK_OVERLAP(one.GetAxis(2), 2, one, two, toCenter, ref pen, ref best); CHECK_OVERLAP(one.GetAxis(0), 3, one, two, toCenter, ref pen, ref best); CHECK_OVERLAP(one.GetAxis(1), 4, one, two, toCenter, ref pen, ref best); CHECK_OVERLAP(one.GetAxis(2), 5, one, two, toCenter, ref pen, ref best); int bestSingleAxis = best; CHECK_OVERLAP(Vector3.Cross(one.GetAxis(0), two.GetAxis(0)), 6, one, two, toCenter, ref pen, ref best); CHECK_OVERLAP(Vector3.Cross(one.GetAxis(0), two.GetAxis(1)), 7, one, two, toCenter, ref pen, ref best); CHECK_OVERLAP(Vector3.Cross(one.GetAxis(0), two.GetAxis(2)), 8, one, two, toCenter, ref pen, ref best); CHECK_OVERLAP(Vector3.Cross(one.GetAxis(1), two.GetAxis(0)), 9, one, two, toCenter, ref pen, ref best); CHECK_OVERLAP(Vector3.Cross(one.GetAxis(1), two.GetAxis(1)), 10, one, two, toCenter, ref pen, ref best); CHECK_OVERLAP(Vector3.Cross(one.GetAxis(1), two.GetAxis(2)), 11, one, two, toCenter, ref pen, ref best); CHECK_OVERLAP(Vector3.Cross(one.GetAxis(2), two.GetAxis(0)), 12, one, two, toCenter, ref pen, ref best); CHECK_OVERLAP(Vector3.Cross(one.GetAxis(2), two.GetAxis(1)), 13, one, two, toCenter, ref pen, ref best); CHECK_OVERLAP(Vector3.Cross(one.GetAxis(2), two.GetAxis(2)), 14, one, two, toCenter, ref pen, ref best); if (best == 0xffffff) throw new Exception("same, same"); if (best < 3) { fillPointFaceBoxBox(one, two, toCenter, data, best, pen); data.AddContacts(1); } else if (best < 6) { fillPointFaceBoxBox(two, one, toCenter * -1.0f, data, best - 3, pen); data.AddContacts(1); return 1; } else { best -= 6; int oneAxisIndex = best / 3; int twoAxisIndex = best % 3; Vector3 oneAxis = one.GetAxis(oneAxisIndex); Vector3 twoAxis = two.GetAxis(twoAxisIndex); Vector3 axis = Vector3.Cross(oneAxis, twoAxis); axis.Normalize(); if (Vector3.Dot(axis, toCenter) > 0) axis = axis * -1.0f; Vector3 ptOnOneEdge = one.HalfSize; Vector3 ptOnTwoEdge = two.HalfSize; for (int i = 0; i < 3; i++) { if (i == oneAxisIndex) ptOnOneEdge[i] = 0; else if (Vector3.Dot(one.GetAxis(i), axis) > 0) ptOnOneEdge[i] = -ptOnOneEdge[i]; if (i == twoAxisIndex) ptOnTwoEdge[i] = 0; else if (Vector3.Dot(two.GetAxis(i), axis) < 0) ptOnTwoEdge[i] = -ptOnTwoEdge[i]; } ptOnOneEdge = Vector3.Transform(ptOnOneEdge, one.Transform); ptOnTwoEdge = Vector3.Transform(ptOnTwoEdge, two.Transform); Vector3 vertex = contactPoint( ptOnOneEdge, oneAxis, one.HalfSize[oneAxisIndex], ptOnTwoEdge, twoAxis, two.HalfSize[twoAxisIndex], bestSingleAxis > 2 ); Contact contact = new Contact(); contact.Penetration = pen; contact.ContactNormal = axis; contact.ContactPoint = vertex; contact.SetData(one.Body, two.Body, data.Friction, data.Restitution); data.Contacts.Add(contact); data.AddContacts(1); return 1; } return 0; }