public void Box_PointOnFace() { using (var box = CreateBox()) { HullDrawingUtility.DrawBasicHull(box, RigidTransform.identity, Color.black, 100); var rotateEveryAxis45 = Quaternion.Euler((float3)45); // up and rotated position var insideUnitBox = new RigidTransform(rotateEveryAxis45, new float3(0.2f, 1.34f, 0.1f)); var outsideUnitBox = new RigidTransform(rotateEveryAxis45, new float3(0.2f, 1.38f, 0.1f)); Assert.IsTrue(HullCollision.IsColliding(insideUnitBox, box, RigidTransform.identity, box)); Assert.IsFalse(HullCollision.IsColliding(outsideUnitBox, box, RigidTransform.identity, box)); HullDrawingUtility.DrawBasicHull(box, insideUnitBox, Color.blue, 100); HullDrawingUtility.DrawBasicHull(box, outsideUnitBox, Color.black, 100); // down and rotated position insideUnitBox = new RigidTransform(rotateEveryAxis45, new float3(0.2f, -1.34f, 0.1f)); outsideUnitBox = new RigidTransform(rotateEveryAxis45, new float3(0.2f, -1.38f, 0.1f)); Assert.IsTrue(HullCollision.IsColliding(insideUnitBox, box, RigidTransform.identity, box)); Assert.IsFalse(HullCollision.IsColliding(outsideUnitBox, box, RigidTransform.identity, box)); HullDrawingUtility.DrawBasicHull(box, insideUnitBox, Color.blue, 100); HullDrawingUtility.DrawBasicHull(box, outsideUnitBox, Color.black, 100); } }
public static HullCollision OBBOBBCollision(OBBHull boxHull1, OBBHull boxHull2) { Particle2D A = boxHull1.GetComponent <Particle2D>(); Particle2D B = boxHull2.GetComponent <Particle2D>(); Vector3[] shape1Corners; Vector3[] shape2Corners; shape1Corners = new Vector3[4]; shape2Corners = new Vector3[4]; Vector3[] normals = new Vector3[4]; float[] shape1MinMax = new float[2]; float[] shape2MinMax = new float[2]; shape1Corners = getRotatedCorners(boxHull1); shape2Corners = getRotatedCorners(boxHull2); normals[0] = getUpNormal(-boxHull1.currentRotation); normals[1] = getRightNormal(-boxHull1.currentRotation); normals[2] = getUpNormal(-boxHull2.currentRotation); normals[3] = getRightNormal(-boxHull2.currentRotation); HullCollision col = new HullCollision(); col.a = boxHull1; col.b = boxHull2; Vector3 range = (boxHull2.transform.position + boxHull2.offset) - (boxHull1.transform.position + boxHull1.offset); //float xOverlap = boxHull1.halfX + boxHull2.halfX - Mathf.Abs(range.x); //float yOverlap = boxHull1.halfY + boxHull2.halfY - Mathf.Abs(range.y); //TRANSPORTATION //col.penetration = new Vector3(xOverlap, yOverlap); //Vector3 closingVel = A.velocity - B.velocity; Vector3 closingVel = B.velocity - A.velocity; col.closingVelocity = closingVel; HullCollision.Contact con0 = new HullCollision.Contact(); con0.point = new Vector3(Mathf.Clamp(range.x, -boxHull1.halfX, boxHull1.halfX), Mathf.Clamp(range.y, -boxHull1.halfY, boxHull1.halfY)); con0.restitution = Mathf.Min(boxHull1.restitution, boxHull2.restitution); con0.normal = range.normalized; for (int i = 0; i < normals.Length; i++) { //Debug.Log("testing corner" + i); shape1MinMax = SatTest(normals[i], shape1Corners); shape2MinMax = SatTest(normals[i], shape2Corners); if (!Overlap(shape1MinMax[0], shape1MinMax[1], shape2MinMax[0], shape2MinMax[1])) { //Debug.Log("falure"); col.status = false; return(col); } } col.status = true; col.contacts[0] = con0; return(col); }
public static HullCollision CircleAABBCollision(CircleHull circleHull, AABBHull boxHull) { Particle2D A = boxHull.GetComponent <Particle2D>(); Particle2D B = circleHull.GetComponent <Particle2D>(); Vector3 closestPoint = new Vector3(0.0f, 0.0f); Vector3 range = (circleHull.transform.position + circleHull.offset) - (boxHull.transform.position + boxHull.offset); closestPoint = new Vector3(Mathf.Clamp(range.x, -boxHull.halfX, boxHull.halfX), Mathf.Clamp(range.y, -boxHull.halfY, boxHull.halfY)); HullCollision col = new HullCollision(); col.a = boxHull; col.b = circleHull; Vector3 closingVel = B.velocity - A.velocity; Vector3 penetration = range - (closestPoint - circleHull.transform.position + circleHull.offset); col.closingVelocity = closingVel; col.penetration = penetration; HullCollision.Contact con0 = new HullCollision.Contact(); con0.point = closestPoint; con0.restitution = Mathf.Min(boxHull.restitution, circleHull.restitution); Vector3 collisionNormal = new Vector3(); if ((range - closestPoint).magnitude - circleHull.radius < 0) { if (con0.point.x == boxHull.halfX)//added mathf { collisionNormal = new Vector3(1.0f, 0.0f); } if (con0.point.x == -boxHull.halfX)//added mathf { collisionNormal = new Vector3(-1.0f, 0.0f); } if (con0.point.y == boxHull.halfY) { collisionNormal = new Vector3(0.0f, 1.0f); } if (con0.point.y == -boxHull.halfY) { collisionNormal = new Vector3(0.0f, -1.0f); } con0.normal = collisionNormal; col.status = true; col.contacts[0] = con0; } else { col.status = false; } return(col); }
public static HullCollision CircleOBBCollision(CircleHull circleHull, OBBHull OBBHull) { Particle2D A = circleHull.GetComponent <Particle2D>(); Particle2D B = OBBHull.GetComponent <Particle2D>(); Vector3[] OBBCorners; OBBCorners = new Vector3[2];//was 4 Vector3[] normals = new Vector3[2]; float[] OBBMinMax = new float[2]; float[] circleMinMax = new float[2]; OBBCorners = getRotatedCorners(OBBHull); normals[0] = getUpNormal(-OBBHull.currentRotation); normals[1] = getRightNormal(-OBBHull.currentRotation); //normals[2] = getUpNormal(-OBBHull2.currentRotation); //normals[3] = getRightNormal(-boxHull2.currentRotation); HullCollision col = new HullCollision(); col.a = circleHull; col.b = OBBHull; Vector3 range = (OBBHull.transform.position + OBBHull.offset) - (circleHull.transform.position + circleHull.offset); Vector3 rotatedRange = getRotatedPoint(range, new Vector3(0.0f, 0.0f), -OBBHull.currentRotation);// 2 circleHull.transform.position Vector3 point = new Vector3(Mathf.Clamp(rotatedRange.x, -OBBHull.halfX, OBBHull.halfX), Mathf.Clamp(rotatedRange.y, -OBBHull.halfY, OBBHull.halfY)); //Debug.Log("range " + range); //Debug.Log("rotrange " + rotatedRange); //float xOverlap = boxHull1.halfX + boxHull2.halfX - Mathf.Abs(range.x); //float yOverlap = boxHull1.halfY + boxHull2.halfY - Mathf.Abs(range.y); //col.penetration = new Vector3(xOverlap, yOverlap); Vector3 closingVel = B.velocity - A.velocity; col.closingVelocity = closingVel; HullCollision.Contact con0 = new HullCollision.Contact(); con0.point = new Vector3(Mathf.Clamp(range.x, -OBBHull.halfX, OBBHull.halfX), Mathf.Clamp(range.y, -OBBHull.halfY, OBBHull.halfY)); con0.restitution = Mathf.Min(OBBHull.restitution, circleHull.restitution); con0.normal = range.normalized; //Debug.Log("point " + point); col.status = false; if ((rotatedRange - point).magnitude - circleHull.radius < 0) { col.status = true; col.contacts[0] = con0; } return(col); }
void VolumeBoundChecks() { Hull3D hull1; Hull3D hull2; Vector3 range; float radialSum; float dot; for (int i = 0; i < allColliders.Count; i++) { allColliders[i].gameObject.GetComponent <Renderer>().material.color = Color.red; if (i != allColliders.Count - 1) // if i is not the last collider in the array { for (int j = i + 1; j < allColliders.Count; j++) // compare it with all colliders after it { HullCollision col = new HullCollision(); hull1 = allColliders[i]; hull2 = allColliders[j]; col.a = hull1; col.b = hull2; /* * if (hull1.transform.position.magnitude <= hull2.transform.position.magnitude) * { * col.a = hull1; * col.b = hull2; * } * else * { * col.a = hull2; * col.b = hull1; * } */ range = (col.b.transform.position - col.a.transform.position); radialSum = hull1.boundingVolumeRadius + hull2.boundingVolumeRadius; if (radialSum * radialSum >= range.sqrMagnitude) // if the radial distance is greater than the actual distance { col.closingVelocity = col.a.GetComponent <Particle3D>().velocity - col.b.GetComponent <Particle3D>().velocity; dot = Vector3.Dot(range.normalized, col.closingVelocity.normalized); //if(dot<=0) // maybe remove if (!CheckIfCollisionsContains(hull1, hull2)) { Collisions.Add(col); } } } } } }
public void Box_CornerOnCorner() { using (var box = CreateBox()) { var insideUnitRangePosition = new float3(0.99f, 0.99f, 0.99f); var outsideUnitRangePosition = new float3(1.01f, 1.01f, 1.01f); var insideUnitRange = new RigidTransform(float3x3.identity, insideUnitRangePosition); var outsideUnitRange = new RigidTransform(float3x3.identity, outsideUnitRangePosition); Assert.IsTrue(HullCollision.IsColliding(insideUnitRange, box, RigidTransform.identity, box)); Assert.IsFalse(HullCollision.IsColliding(outsideUnitRange, box, RigidTransform.identity, box)); } }
public void Box_EdgeSeparation() { using (var box = CreateBox()) { // These two boxes are positioned so that both face SAT checks would show no separation, // a working SAT edge test is required to catch the false positive. var boxTransformA = new RigidTransform(Quaternion.Euler(-24.357f, -4.779f, -32.115f), new float3(-0.089f, -0.821f, -2.233f)); var boxTransformB = new RigidTransform(Quaternion.Euler(55.943f, 21.207f, 47.057f), new float3(-0.207f, -0.06f, -1.256f)); Assert.IsFalse(HullCollision.IsColliding(boxTransformA, box, boxTransformB, box)); HullDrawingUtility.DrawBasicHull(box, boxTransformA, Color.blue, 100); HullDrawingUtility.DrawBasicHull(box, boxTransformB, Color.black, 100); } }
public void Box_EdgeOnEdge() { using (var box = CreateBox()) { HullDrawingUtility.DrawBasicHull(box, RigidTransform.identity, Color.black, 100); var rotateEveryAxis45 = Quaternion.Euler((float3)45); var insideUnitBox = new RigidTransform(rotateEveryAxis45, new float3(0.2f, 1.1f, -0.80f)); var outsideUnitBox = new RigidTransform(rotateEveryAxis45, new float3(0.184f, 1.123f, -0.816f)); Assert.IsTrue(HullCollision.IsColliding(insideUnitBox, box, RigidTransform.identity, box)); Assert.IsFalse(HullCollision.IsColliding(outsideUnitBox, box, RigidTransform.identity, box)); HullDrawingUtility.DrawBasicHull(box, insideUnitBox, Color.blue, 100); HullDrawingUtility.DrawBasicHull(box, outsideUnitBox, Color.black, 100); } }
public void Box_FaceOnFace() { using (var box = CreateBox()) { var testTransform = new RigidTransform(); testTransform.pos = Vector3.down * 0.99f; Assert.IsTrue(HullCollision.IsColliding(testTransform, box, RigidTransform.identity, box)); testTransform.pos = Vector3.down * 1.01f; Assert.IsFalse(HullCollision.IsColliding(testTransform, box, RigidTransform.identity, box)); testTransform.pos = Vector3.up * 0.99f; Assert.IsTrue(HullCollision.IsColliding(testTransform, box, RigidTransform.identity, box)); testTransform.pos = Vector3.up * 1.01f; Assert.IsFalse(HullCollision.IsColliding(testTransform, box, RigidTransform.identity, box)); testTransform.pos = Vector3.back * 0.99f; Assert.IsTrue(HullCollision.IsColliding(testTransform, box, RigidTransform.identity, box)); testTransform.pos = Vector3.back * 1.01f; Assert.IsFalse(HullCollision.IsColliding(testTransform, box, RigidTransform.identity, box)); testTransform.pos = Vector3.forward * 0.99f; Assert.IsTrue(HullCollision.IsColliding(testTransform, box, RigidTransform.identity, box)); testTransform.pos = Vector3.forward * 1.01f; Assert.IsFalse(HullCollision.IsColliding(testTransform, box, RigidTransform.identity, box)); testTransform.pos = Vector3.right * 0.99f; Assert.IsTrue(HullCollision.IsColliding(testTransform, box, RigidTransform.identity, box)); testTransform.pos = Vector3.right * 1.01f; Assert.IsFalse(HullCollision.IsColliding(testTransform, box, RigidTransform.identity, box)); testTransform.pos = Vector3.left * 0.99f; Assert.IsTrue(HullCollision.IsColliding(testTransform, box, RigidTransform.identity, box)); testTransform.pos = Vector3.left * 1.01f; Assert.IsFalse(HullCollision.IsColliding(testTransform, box, RigidTransform.identity, box)); } }
public static HullCollision CircleCircleCollision(CircleHull circleHull1, CircleHull circleHull2) { // *IMPORTANT* for circle and square the collision only wirks with obejct1 - object 2 and not viceversa, must be a prob in clollision resolution Vector3 c1Offset = circleHull1.offset; Vector3 c2Offset = circleHull2.offset; Vector3 range = (circleHull2.transform.position + c2Offset) - (circleHull1.transform.position + c1Offset); // make sure offsets arent screwing things up float overlap = (circleHull2.radius + circleHull1.radius) - range.magnitude; HullCollision col = new HullCollision(); col.a = circleHull1; col.b = circleHull2; col.penetration = range * overlap; HullCollision.Contact con0 = new HullCollision.Contact(); con0.point = (range.normalized * circleHull1.radius); con0.point += new Vector3(circleHull1.transform.position.x, circleHull1.transform.position.y); con0.normal = range.normalized; con0.restitution = Mathf.Min(circleHull1.restitution, circleHull2.restitution); col.contacts[0] = con0; Particle2D c1 = circleHull1.GetComponentInParent <Particle2D>(); Particle2D c2 = circleHull2.GetComponentInParent <Particle2D>(); Vector3 closingVel = c2.velocity - c1.velocity; // started as c1 -c2 col.closingVelocity = closingVel; if (overlap > 0) { col.status = true; return(col); } else { col.status = false; return(col); } }
private void HandleHullCollisions() { for (int i = 0; i < Transforms.Count; ++i) { var tA = Transforms[i]; if (tA == null) { continue; } var hullA = Hulls[tA.GetInstanceID()].Hull; var transformA = new RigidTransform(tA.rotation, tA.position); HullDrawingUtility.DrawDebugHull(hullA, transformA, HullDrawingOptions); if (LogClosestPoint) { var sw3 = System.Diagnostics.Stopwatch.StartNew(); var result3 = HullCollision.ClosestPoint(transformA, hullA, 0); sw3.Stop(); var sw4 = System.Diagnostics.Stopwatch.StartNew(); var result4 = HullOperations.ClosestPoint.Invoke(transformA, hullA, 0); sw4.Stop(); if (DrawClosestPoint) { DebugDrawer.DrawSphere(result4, 0.1f, Color.blue); DebugDrawer.DrawLine(result4, Vector3.zero, Color.blue); } Debug.Log($"ClosestPoint between '{tA.name}' and world zero took: {sw3.Elapsed.TotalMilliseconds:N4}ms (Normal), {sw4.Elapsed.TotalMilliseconds:N4}ms (Burst)"); } for (int j = i + 1; j < Transforms.Count; j++) { var tB = Transforms[j]; if (tB == null) { continue; } if (!tA.hasChanged && !tB.hasChanged) { continue; } var hullB = Hulls[tB.GetInstanceID()].Hull; var transformB = new RigidTransform(tB.rotation, tB.position); HullDrawingUtility.DrawDebugHull(hullB, transformB, HullDrawingOptions); DrawHullCollision(tA.gameObject, tB.gameObject, transformA, hullA, transformB, hullB); if (LogCollisions) { var sw1 = System.Diagnostics.Stopwatch.StartNew(); var result1 = HullCollision.IsColliding(transformA, hullA, transformB, hullB); sw1.Stop(); var sw2 = System.Diagnostics.Stopwatch.StartNew(); var result2 = HullOperations.IsColliding.Invoke(transformA, hullA, transformB, hullB); sw2.Stop(); Debug.Assert(result1 == result2); Debug.Log($"Collisions between '{tA.name}'/'{tB.name}' took: {sw1.Elapsed.TotalMilliseconds:N4}ms (Normal), {sw2.Elapsed.TotalMilliseconds:N4}ms (Burst)"); } } } if (LogCollisions) { TestBatchCollision(); } }
public void DrawHullCollision(GameObject a, GameObject b, RigidTransform t1, NativeHull hull1, RigidTransform t2, NativeHull hull2) { var collision = HullCollision.GetDebugCollisionInfo(t1, hull1, t2, hull2); if (collision.IsColliding) { if (DrawIntersection) // Visualize all faces of the intersection { HullIntersection.DrawNativeHullHullIntersection(t1, hull1, t2, hull2); } if (DrawContact || LogContact) // Visualize the minimal contact calcluation for physics { //var manifold = HullOperations.GetContact.Invoke(t1, hull1, t2, hull2); var sw1 = System.Diagnostics.Stopwatch.StartNew(); var tmp = new NativeManifold(Allocator.Persistent); var normalResult = HullIntersection.NativeHullHullContact(ref tmp, t1, hull1, t2, hull2); sw1.Stop(); tmp.Dispose(); var sw2 = System.Diagnostics.Stopwatch.StartNew(); var burstResult = HullOperations.TryGetContact.Invoke(out NativeManifold manifold, t1, hull1, t2, hull2); sw2.Stop(); if (LogContact) { Debug.Log($"GetContact between '{a.name}'/'{b.name}' took: {sw1.Elapsed.TotalMilliseconds:N4}ms (Normal), {sw2.Elapsed.TotalMilliseconds:N4}ms (Burst)"); } if (DrawContact && burstResult) { // Do something with manifold HullDrawingUtility.DebugDrawManifold(manifold); //var points = manifold.Points; for (int i = 0; i < manifold.Length; i++) { var point = manifold[i]; DebugDrawer.DrawSphere(point.Position, 0.02f); DebugDrawer.DrawArrow(point.Position, manifold.Normal * 0.2f); var penentrationPoint = point.Position + manifold.Normal * point.Distance; DebugDrawer.DrawLabel(penentrationPoint, $"{point.Distance:N2}"); HullDrawingUtility.DrawEdge(point.Id.FeaturePair.InEdge1, t1, hull1); HullDrawingUtility.DrawEdge(point.Id.FeaturePair.OutEdge1, t1, hull1); HullDrawingUtility.DrawEdge(point.Id.FeaturePair.InEdge2, t1, hull1); HullDrawingUtility.DrawEdge(point.Id.FeaturePair.OutEdge2, t1, hull1); HullDrawingUtility.DrawEdge(point.Id.FeaturePair.InEdge1, t2, hull2); HullDrawingUtility.DrawEdge(point.Id.FeaturePair.OutEdge1, t2, hull2); HullDrawingUtility.DrawEdge(point.Id.FeaturePair.InEdge2, t2, hull2); HullDrawingUtility.DrawEdge(point.Id.FeaturePair.OutEdge2, t2, hull2); DebugDrawer.DrawDottedLine(point.Position, penentrationPoint); } manifold.Dispose(); } } if (DrawIsCollided) { DebugDrawer.DrawSphere(t1.pos, 0.1f, UnityColors.GhostDodgerBlue); DebugDrawer.DrawSphere(t2.pos, 0.1f, UnityColors.GhostDodgerBlue); } } if (DrawClosestFace) { var color1 = collision.Face1.Distance > 0 ? UnityColors.Red.ToOpacity(0.3f) : UnityColors.Yellow.ToOpacity(0.3f); HullDrawingUtility.DrawFaceWithOutline(collision.Face1.Index, t1, hull1, color1, UnityColors.Black); var color2 = collision.Face2.Distance > 0 ? UnityColors.Red.ToOpacity(0.3f) : UnityColors.Yellow.ToOpacity(0.3f); HullDrawingUtility.DrawFaceWithOutline(collision.Face2.Index, t2, hull2, color2, UnityColors.Black); } }
public static void ResolveCollision(HullCollision col) { Particle2D A = col.a.GetComponent <Particle2D>(); Particle2D B = col.b.GetComponent <Particle2D>(); float invAMass; float invBMass; if (A.mass == 0) { invAMass = 0; } else { invAMass = 1 / A.mass; } if (B.mass == 0) { invBMass = 0; } else { invBMass = 1 / B.mass; } float velAlongNormal = Vector3.Dot(col.closingVelocity, col.contacts[0].normal); //Debug.Log("velAlongNormal " + velAlongNormal); if (velAlongNormal > 0) { return; // > makes square work properly } //Debug.Log(velAlongNormal); // restitustion float e = col.contacts[0].restitution; // impulse scalar float j = -(1 + e) * velAlongNormal; j /= invAMass + invBMass; //Debug.Log(j); Vector3 impulse = j * col.contacts[0].normal; //Debug.Log(impulse); //A.velocity = new Vector3(0.0f, 0.0f, 0.0f); //B.velocity = new Vector3(0.0f, 0.0f, 0.0f); A.velocity -= invAMass * impulse; B.velocity += invBMass * impulse; // Positional Correction if (col.status) { float percent = 0.2f; float slop = 0.01f; Vector3 correction = Mathf.Max(velAlongNormal - slop, 0) / (invAMass + invBMass) * percent * col.contacts[0].normal; A.position += invAMass * correction; // started - B.position -= invBMass * correction; // started + } }
public static HullCollision AABBAABBCollision(AABBHull boxHull1, AABBHull boxHull2) { Vector3 min0, max0, min1, max1; Vector3 b1Offset = boxHull1.offset; Vector3 b2Offset = boxHull2.offset; Particle2D A = boxHull1.GetComponent <Particle2D>(); Particle2D B = boxHull2.GetComponent <Particle2D>(); min0 = boxHull1.transform.position - new Vector3(boxHull1.halfX, boxHull1.halfY) + b1Offset; max0 = boxHull1.transform.position + new Vector3(boxHull1.halfX, boxHull1.halfY) + b1Offset; min1 = boxHull2.transform.position - new Vector3(boxHull2.halfX, boxHull2.halfY) + b2Offset; max1 = boxHull2.transform.position + new Vector3(boxHull2.halfX, boxHull2.halfY) + b2Offset; Vector3 range = (boxHull2.transform.position + b2Offset) - (boxHull1.transform.position + b1Offset); // make sure offsets arent screwing things up HullCollision col = new HullCollision(); col.a = boxHull1; col.b = boxHull2; float xOverlap = boxHull1.halfX + boxHull2.halfX - Mathf.Abs(range.x); float yOverlap = boxHull1.halfY + boxHull2.halfY - Mathf.Abs(range.y); //TRANSPORTATION col.penetration = new Vector3(xOverlap, yOverlap); //Vector3 closingVel = A.velocity - B.velocity; Vector3 closingVel = B.velocity - A.velocity; col.closingVelocity = closingVel; HullCollision.Contact con0 = new HullCollision.Contact(); con0.point = new Vector3(Mathf.Clamp(range.x, -boxHull1.halfX, boxHull1.halfX), Mathf.Clamp(range.y, -boxHull1.halfY, boxHull1.halfY)); con0.restitution = Mathf.Min(boxHull1.restitution, boxHull2.restitution); if (max0.x >= min1.x && max1.x >= min0.x) { if (max0.y >= min1.y && max1.y >= min0.y) { Vector3 collisionNormal = new Vector3(); if (con0.point.x == boxHull1.halfX)//added mathf { collisionNormal = new Vector3(1.0f, 0.0f); } if (con0.point.x == -boxHull1.halfX)//added mathf { collisionNormal = new Vector3(-1.0f, 0.0f); } if (con0.point.y == boxHull1.halfY) { collisionNormal = new Vector3(0.0f, 1.0f); } if (con0.point.y == -boxHull1.halfY) { collisionNormal = new Vector3(0.0f, -1.0f); } con0.normal = collisionNormal; col.status = true; } } else { col.status = false; } col.contacts[0] = con0; return(col); }