private static CollisionInfo resolveVertexFaceBox(CollisionHull3D box1, CollisionHull3D box2, Vector3 toCenter, int bestCase, float bestOverlap, Vector3 halfExtends) { Vector3 normal = box1.GetComponent<Particle3D>().getObjectToWorld().GetColumn(bestCase); Vector4 tempCenter = new Vector4(toCenter.x,toCenter.y,toCenter.z); if(Vector4.Dot(box1.GetComponent<Particle3D>().getObjectToWorld().GetColumn(bestCase),tempCenter) > 0) { normal = normal * -1.0f; } Vector3 vertex = halfExtends; if (Vector4.Dot(box2.GetComponent<Particle3D>().getObjectToWorld().GetColumn(0), normal) < 0) { vertex.x = -vertex.x; } if (Vector4.Dot(box2.GetComponent<Particle3D>().getObjectToWorld().GetColumn(1), normal) < 0) { vertex.y = -vertex.y; } if (Vector4.Dot(box2.GetComponent<Particle3D>().getObjectToWorld().GetColumn(2), normal) < 0) { vertex.z = -vertex.z; } Debug.Log(vertex); Vector3 vertexPoint = box1.GetComponent<Particle3D>().getObjectToWorld().MultiplyPoint(vertex); return new CollisionInfo(box1, box2, normal, bestOverlap, vertexPoint); }
public static CollisionInfo SphereToPlaneCollision(CollisionHull3D a, CollisionHull3D b) { // Find the relative centre by transforming the center of the circle to the local space of the AABB Vector3 relativeCentre = b.GetComponent <Particle3D>().invTransformMatrix.MultiplyPoint(a.GetPosition()); Vector3 closestPointToCircle = new Vector3(Math.Max(b.GetMinimumCorner().x, Math.Min(relativeCentre.x, b.GetMaximumCorner().x)), Math.Max(b.GetMinimumCorner().y, Math.Min(relativeCentre.y, b.GetMaximumCorner().y)), Math.Max(b.GetMinimumCorner().z, Math.Min(relativeCentre.z, b.GetMaximumCorner().z))); // Calculate the distance between both colliders Vector3 distance = relativeCentre - closestPointToCircle; // Are the Radii less than or equal to the distance between both circles? if (Vector3.Dot(distance, distance) < a.GetDimensions().x *a.GetDimensions().x) { // If yes, then inform the parents of the complex shape object (if applicable) ReportCollisionToParent(a, b); } else { // If no, return nothing return(null); } float penetration = a.GetDimensions().x - Mathf.Sqrt(Vector3.Dot(distance, distance)); Vector3 closestPointWorld = b.GetComponent <Particle3D>().transformMatrix.MultiplyPoint(closestPointToCircle); // Return result return(new CollisionInfo(a, b, penetration, (closestPointWorld - a.GetPosition()).normalized, Vector3.zero, CollisionResolution3D.CalculateSeparatingVelocity(a, b, a.GetPosition(), closestPointWorld))); }
static protected CollisionInfo boxTest(CollisionHull3D box1, CollisionHull3D box2, Vector3 toCenter, Vector3 box1HalfExtends, Vector3 box2HalfExtends) { List <Vector3> allAxis = new List <Vector3>(); allAxis.Add(box1.GetComponent <Particle3D>().getObjectToWorld().GetColumn(0)); allAxis.Add(box1.GetComponent <Particle3D>().getObjectToWorld().GetColumn(1)); allAxis.Add(box1.GetComponent <Particle3D>().getObjectToWorld().GetColumn(2)); allAxis.Add(box2.GetComponent <Particle3D>().getObjectToWorld().GetColumn(0)); allAxis.Add(box2.GetComponent <Particle3D>().getObjectToWorld().GetColumn(1)); allAxis.Add(box2.GetComponent <Particle3D>().getObjectToWorld().GetColumn(2)); allAxis.Add(Vector3.Cross(box1.GetComponent <Particle3D>().getObjectToWorld().GetColumn(0), box2.GetComponent <Particle3D>().getObjectToWorld().GetColumn(0))); allAxis.Add(Vector3.Cross(box1.GetComponent <Particle3D>().getObjectToWorld().GetColumn(0), box2.GetComponent <Particle3D>().getObjectToWorld().GetColumn(1))); allAxis.Add(Vector3.Cross(box1.GetComponent <Particle3D>().getObjectToWorld().GetColumn(0), box2.GetComponent <Particle3D>().getObjectToWorld().GetColumn(2))); allAxis.Add(Vector3.Cross(box1.GetComponent <Particle3D>().getObjectToWorld().GetColumn(1), box2.GetComponent <Particle3D>().getObjectToWorld().GetColumn(0))); allAxis.Add(Vector3.Cross(box1.GetComponent <Particle3D>().getObjectToWorld().GetColumn(1), box2.GetComponent <Particle3D>().getObjectToWorld().GetColumn(1))); allAxis.Add(Vector3.Cross(box1.GetComponent <Particle3D>().getObjectToWorld().GetColumn(1), box2.GetComponent <Particle3D>().getObjectToWorld().GetColumn(2))); allAxis.Add(Vector3.Cross(box1.GetComponent <Particle3D>().getObjectToWorld().GetColumn(2), box2.GetComponent <Particle3D>().getObjectToWorld().GetColumn(0))); allAxis.Add(Vector3.Cross(box1.GetComponent <Particle3D>().getObjectToWorld().GetColumn(2), box2.GetComponent <Particle3D>().getObjectToWorld().GetColumn(1))); allAxis.Add(Vector3.Cross(box1.GetComponent <Particle3D>().getObjectToWorld().GetColumn(2), box2.GetComponent <Particle3D>().getObjectToWorld().GetColumn(2))); float bestOverlap = float.MaxValue; int bestCase = 0; //greater than 6, edge Vector3 bestAxis = Vector3.one; for (int i = 0; i < 15; i++) { Vector3 axis = allAxis[i]; if (axis.sqrMagnitude < 0.001) { continue; } axis = axis.normalized; float overlap = penetrationOnAxis(box1, box2, axis, toCenter, box1HalfExtends, box2HalfExtends); if (overlap < 0) { return(null); } if (overlap < bestOverlap) { bestOverlap = overlap; bestCase = i; bestAxis = allAxis[bestCase]; } } if (bestCase < 3) { return(resolveVertexFaceBox(box1, box2, toCenter, bestCase, bestOverlap)); } else if (bestCase < 6) { return(resolveVertexFaceBox(box2, box1, toCenter * -1.0f, bestCase - 3, bestOverlap)); } else { return(new CollisionInfo(box1, box2, bestAxis, bestOverlap)); } }
static private CollisionInfo resolveVertexFaceBox(CollisionHull3D box1, CollisionHull3D box2, Vector3 toCenter, int bestCase, float bestOverlap) { Vector3 normal = box1.GetComponent <Particle3D>().getObjectToWorld().GetColumn(bestCase); Vector4 tempCenter = new Vector4(toCenter.x, toCenter.y, toCenter.z); if (Vector4.Dot(box1.GetComponent <Particle3D>().getObjectToWorld().GetColumn(bestCase), tempCenter) > 0) { normal = normal * -1.0f; } return(new CollisionInfo(box1, box2, normal, bestOverlap)); }
// This function calculate Circle to ABB collisions public static CollisionInfo CircleToOBBCollision(CollisionHull3D a, CollisionHull3D b) { // Find the relative centre by transforming the center of the circle to the local space of the AABB Vector3 relativeCentre = b.GetComponent <Particle3D>().invTransformMatrix.MultiplyPoint(a.GetPosition()); Vector3 closestPointToCircle = new Vector3(Math.Max(b.GetMinimumCorner().x, Math.Min(relativeCentre.x, b.GetMaximumCorner().x)), Math.Max(b.GetMinimumCorner().y, Math.Min(relativeCentre.y, b.GetMaximumCorner().y)), Math.Max(b.GetMinimumCorner().z, Math.Min(relativeCentre.z, b.GetMaximumCorner().z))); Vector3 distance = relativeCentre - closestPointToCircle; float distanceSquared = Vector3.Dot(distance, distance); float penetration = a.GetDimensions().x - Mathf.Sqrt(distanceSquared); // Is the penetration a positive value if (penetration >= 0) { // If yes, then inform the parents of the complex shape object (if applicable) ReportCollisionToParent(a, b); } else { // If no, return nothing return(null); } // Return full details of the Collision list if the two collide return(new CollisionInfo(a, b, penetration, (closestPointToCircle - relativeCentre).normalized, closestPointToCircle, CollisionResolution3D.CalculateSeparatingVelocity(a, b, a.GetPosition(), b.GetPosition()))); }
private static float penetrationOnAxis(CollisionHull3D box1, CollisionHull3D box2, Vector3 axis, Vector3 toCenter, Vector3 box1HalfExtends, Vector3 box2HalfExtends) { float projectOne = transformToAxis(box1HalfExtends, box1.GetComponent<Particle3D>().getObjectToWorld(), axis); float projectTwo = transformToAxis(box2HalfExtends, box2.GetComponent<Particle3D>().getObjectToWorld(), axis); float distance = Mathf.Abs(Vector3.Dot(toCenter, axis)); return projectOne + projectTwo - distance; }
public CollisionInfo(CollisionHull3D shapeA, CollisionHull3D shapeB, Vector3 normal, float penetration) { RigidBodyA = shapeA.GetComponent <Particle3D>(); ShapeA = shapeA; RigidBodyB = shapeB.GetComponent <Particle3D>(); ShapeB = shapeB; RelativeVelocity = RigidBodyB.velocity - RigidBodyA.velocity; contacts[0].normal = normal; contacts[0].penetration = penetration; contacts[0].restitution = Mathf.Min(RigidBodyA.restitution, RigidBodyB.restitution); }
// This function calculates OBB to OBB colisions public static CollisionInfo OBBToOBBCollision(CollisionHull3D a, CollisionHull3D b) { List <float> overlaps = new List <float>(); List <Vector3> axes = new List <Vector3>(); // Get the transform values for each axis for each shape Vector3 x1 = a.GetComponent <Particle3D>().transformMatrix.GetColumn(0); Vector3 y1 = a.GetComponent <Particle3D>().transformMatrix.GetColumn(1); Vector3 z1 = a.GetComponent <Particle3D>().transformMatrix.GetColumn(2); Vector3 x2 = b.GetComponent <Particle3D>().transformMatrix.GetColumn(0); Vector3 y2 = b.GetComponent <Particle3D>().transformMatrix.GetColumn(1); Vector3 z2 = b.GetComponent <Particle3D>().transformMatrix.GetColumn(2); // Go through and check through all overlapping axes // Face/Face Object 1 overlaps.Add(CheckOBBAxis(a, b, x1)); axes.Add(x1); if (overlaps[overlaps.Count - 1] < 0) { return(null); } overlaps.Add(CheckOBBAxis(a, b, y1)); axes.Add(y1); if (overlaps[overlaps.Count - 1] < 0) { return(null); } overlaps.Add(CheckOBBAxis(a, b, z1)); axes.Add(z1); if (overlaps[overlaps.Count - 1] < 0) { return(null); } // Face/Face Object 2 overlaps.Add(CheckOBBAxis(a, b, x2)); axes.Add(x2); if (overlaps[overlaps.Count - 1] < 0) { return(null); } overlaps.Add(CheckOBBAxis(a, b, y2)); axes.Add(y2); if (overlaps[overlaps.Count - 1] < 0) { return(null); } overlaps.Add(CheckOBBAxis(a, b, z2)); axes.Add(z2); if (overlaps[overlaps.Count - 1] < 0) { return(null); } // Edge/Edge overlaps.Add(CheckOBBAxis(a, b, Vector3.Cross(x1, x2))); axes.Add(Vector3.Cross(x1, x2)); if (overlaps[overlaps.Count - 1] < 0) { return(null); } overlaps.Add(CheckOBBAxis(a, b, Vector3.Cross(x1, y2))); axes.Add(Vector3.Cross(x1, y2)); if (overlaps[overlaps.Count - 1] < 0) { return(null); } overlaps.Add(CheckOBBAxis(a, b, Vector3.Cross(x1, z2))); axes.Add(Vector3.Cross(x1, z2)); if (overlaps[overlaps.Count - 1] < 0) { return(null); } overlaps.Add(CheckOBBAxis(a, b, Vector3.Cross(y1, x2))); axes.Add(Vector3.Cross(y1, x2)); if (overlaps[overlaps.Count - 1] < 0) { return(null); } overlaps.Add(CheckOBBAxis(a, b, Vector3.Cross(y1, y2))); axes.Add(Vector3.Cross(y1, y2)); if (overlaps[overlaps.Count - 1] < 0) { return(null); } overlaps.Add(CheckOBBAxis(a, b, Vector3.Cross(y1, z2))); axes.Add(Vector3.Cross(y1, z2)); if (overlaps[overlaps.Count - 1] < 0) { return(null); } overlaps.Add(CheckOBBAxis(a, b, Vector3.Cross(z1, x2))); axes.Add(Vector3.Cross(z1, x2)); if (overlaps[overlaps.Count - 1] < 0) { return(null); } overlaps.Add(CheckOBBAxis(a, b, Vector3.Cross(z1, y2))); axes.Add(Vector3.Cross(z1, y2)); if (overlaps[overlaps.Count - 1] < 0) { return(null); } overlaps.Add(CheckOBBAxis(a, b, Vector3.Cross(z1, z2))); axes.Add(Vector3.Cross(z1, z2)); if (overlaps[overlaps.Count - 1] < 0) { return(null); } float bestOverlap = Mathf.Infinity; int bestIndex = 0; for (int i = 0; i < overlaps.Count; i++) { if (overlaps[i] < bestOverlap) { bestOverlap = overlaps[i]; bestIndex = i; } } if (bestIndex > 2) { Vector3 normal = axes[bestIndex]; Vector3 axis = axes[bestIndex]; if (Vector3.Dot(axis, (b.GetPosition() - a.GetPosition())) > 0) { axis *= -1; } Vector3 vertex = a.GetDimensions(); if (Vector3.Dot(a.GetComponent <Particle3D>().transformMatrix.GetColumn(0), normal) > 0) { vertex.x = -vertex.x; } if (Vector3.Dot(a.GetComponent <Particle3D>().transformMatrix.GetColumn(1), normal) > 0) { vertex.y = -vertex.y; } if (Vector3.Dot(a.GetComponent <Particle3D>().transformMatrix.GetColumn(2), normal) > 0) { vertex.z = -vertex.z; } vertex = a.GetComponent <Particle3D>().invTransformMatrix.MultiplyPoint(vertex); // If all axis are overlaping, then we have a collision ReportCollisionToParent(a, b); return(new CollisionInfo(a, b, CollisionResolution.GetFinalPenetration(overlaps), normal, vertex, CollisionResolution3D.CalculateSeparatingVelocity(a, b, a.GetPosition(), b.GetPosition()))); } else if (bestIndex > 5) { Vector3 normal = axes[bestIndex]; Vector3 axis = axes[bestIndex]; if (Vector3.Dot(axis, (a.GetPosition() - b.GetPosition())) > 0) { axis *= -1; } Vector3 vertex = b.GetDimensions(); if (Vector3.Dot(b.GetComponent <Particle3D>().transformMatrix.GetColumn(0), normal) > 0) { vertex.x = -vertex.x; } if (Vector3.Dot(b.GetComponent <Particle3D>().transformMatrix.GetColumn(1), normal) > 0) { vertex.y = -vertex.y; } if (Vector3.Dot(b.GetComponent <Particle3D>().transformMatrix.GetColumn(2), normal) > 0) { vertex.z = -vertex.z; } vertex = b.GetComponent <Particle3D>().invTransformMatrix.MultiplyPoint(vertex); // If all axis are overlaping, then we have a collision ReportCollisionToParent(a, b); return(new CollisionInfo(a, b, CollisionResolution.GetFinalPenetration(overlaps), normal, vertex, CollisionResolution3D.CalculateSeparatingVelocity(a, b, a.GetPosition(), b.GetPosition()))); } else { int index = bestIndex - 6; int oneIndex = index / 3; int twoIndex = index % 3; Vector3 oneAxis = axes[oneIndex]; Vector3 twoAxis = axes[twoIndex]; Vector3 axis = axes[bestIndex].normalized; if (Vector3.Dot(axis, b.GetPosition() - a.GetPosition()) > 0) { axis *= -1; } Vector3 pointOnOneEdge = a.GetDimensions(); Vector3 pointOnTwoEdge = b.GetDimensions(); if (oneIndex == 0) { pointOnOneEdge.x = 0; } else if (Vector2.Dot(x1, axis) > 0) { pointOnOneEdge.x = -pointOnOneEdge.x; } if (twoIndex == 3) { pointOnTwoEdge.x = 0; } else if (Vector2.Dot(x2, axis) > 0) { pointOnTwoEdge.x = -pointOnTwoEdge.x; } if (oneIndex == 1) { pointOnOneEdge.y = 0; } else if (Vector2.Dot(y1, axis) > 0) { pointOnOneEdge.y = -pointOnOneEdge.y; } if (twoIndex == 4) { pointOnTwoEdge.y = 0; } else if (Vector2.Dot(y2, axis) > 0) { pointOnTwoEdge.y = -pointOnTwoEdge.y; } if (oneIndex == 2) { pointOnOneEdge.z = 0; } else if (Vector2.Dot(z1, axis) > 0) { pointOnOneEdge.z = -pointOnOneEdge.z; } if (twoIndex == 5) { pointOnTwoEdge.z = 0; } else if (Vector2.Dot(z2, axis) > 0) { pointOnTwoEdge.z = -pointOnTwoEdge.z; } pointOnOneEdge = a.GetComponent <Particle3D>().invTransformMatrix.MultiplyPoint(pointOnOneEdge); pointOnTwoEdge = b.GetComponent <Particle3D>().invTransformMatrix.MultiplyPoint(pointOnTwoEdge); Vector3 contactPoint = GetCollisionPoint(pointOnOneEdge, oneAxis, a.GetDimensions().x, pointOnTwoEdge, twoAxis, b.GetDimensions().x, bestIndex > 2); ReportCollisionToParent(a, b); return(new CollisionInfo(a, b, CollisionResolution.GetFinalPenetration(overlaps), axis, contactPoint, CollisionResolution3D.CalculateSeparatingVelocity(a, b, a.GetPosition(), b.GetPosition()))); } }
public static float transformToOBBAxis(CollisionHull3D shape, Vector3 rotationAxis) { return(shape.GetDimensions().x *Mathf.Abs(Vector3.Dot(shape.GetComponent <Particle3D>().transformMatrix.GetColumn(0), rotationAxis)) + shape.GetDimensions().y *Mathf.Abs(Vector3.Dot(shape.GetComponent <Particle3D>().transformMatrix.GetColumn(1), rotationAxis)) + shape.GetDimensions().z *Mathf.Abs(Vector3.Dot(shape.GetComponent <Particle3D>().transformMatrix.GetColumn(2), rotationAxis))); }
// This function calculates OBB to OBB colisions public static CollisionInfo OBBToOBBCollision(CollisionHull3D a, CollisionHull3D b) { List <float> overlaps = new List <float>(); // Get the transform values for each axis for each shape Vector3 x1 = a.GetComponent <Particle3D>().transformMatrix.GetColumn(0); Vector3 y1 = a.GetComponent <Particle3D>().transformMatrix.GetColumn(1); Vector3 z1 = a.GetComponent <Particle3D>().transformMatrix.GetColumn(2); Vector3 x2 = b.GetComponent <Particle3D>().transformMatrix.GetColumn(0); Vector3 y2 = b.GetComponent <Particle3D>().transformMatrix.GetColumn(1); Vector3 z2 = b.GetComponent <Particle3D>().transformMatrix.GetColumn(2); // Go through and check through all overlapping axes overlaps.Add(CheckOBBAxis(a, b, x1)); if (overlaps[overlaps.Count - 1] < 0) { return(null); } overlaps.Add(CheckOBBAxis(a, b, y1)); if (overlaps[overlaps.Count - 1] < 0) { return(null); } overlaps.Add(CheckOBBAxis(a, b, z1)); if (overlaps[overlaps.Count - 1] < 0) { return(null); } overlaps.Add(CheckOBBAxis(a, b, x2)); if (overlaps[overlaps.Count - 1] < 0) { return(null); } overlaps.Add(CheckOBBAxis(a, b, y2)); if (overlaps[overlaps.Count - 1] < 0) { return(null); } overlaps.Add(CheckOBBAxis(a, b, z2)); if (overlaps[overlaps.Count - 1] < 0) { return(null); } overlaps.Add(CheckOBBAxis(a, b, Vector3.Cross(x1, x2))); if (overlaps[overlaps.Count - 1] < 0) { return(null); } overlaps.Add(CheckOBBAxis(a, b, Vector3.Cross(x1, y2))); if (overlaps[overlaps.Count - 1] < 0) { return(null); } overlaps.Add(CheckOBBAxis(a, b, Vector3.Cross(x1, z2))); if (overlaps[overlaps.Count - 1] < 0) { return(null); } overlaps.Add(CheckOBBAxis(a, b, Vector3.Cross(y1, x2))); if (overlaps[overlaps.Count - 1] < 0) { return(null); } overlaps.Add(CheckOBBAxis(a, b, Vector3.Cross(y1, y2))); if (overlaps[overlaps.Count - 1] < 0) { return(null); } overlaps.Add(CheckOBBAxis(a, b, Vector3.Cross(y1, z2))); if (overlaps[overlaps.Count - 1] < 0) { return(null); } overlaps.Add(CheckOBBAxis(a, b, Vector3.Cross(z1, x2))); if (overlaps[overlaps.Count - 1] < 0) { return(null); } overlaps.Add(CheckOBBAxis(a, b, Vector3.Cross(z1, y2))); if (overlaps[overlaps.Count - 1] < 0) { return(null); } overlaps.Add(CheckOBBAxis(a, b, Vector3.Cross(z1, z2))); if (overlaps[overlaps.Count - 1] < 0) { return(null); } // If all axis are overlaping, then we have a collision ReportCollisionToParent(a, b); return(new CollisionInfo(a, b, CollisionResolution.GetFinalPenetration(overlaps))); }