public static void DebugDrawManifold(NativeManifold manifold, Color color = default)
{
if (!manifold.IsCreated || manifold.Points.Length == 0)
{
return;
}
if (color == default)
{
color = UnityColors.Blue.ToOpacity(0.3f);
}
for (int i = 0; i < manifold.Points.Length; i++)
{
var start = manifold.Points[i];
if (manifold.Points.Length >= 2)
{
var end = i > 0 ? manifold.Points[i - 1] : manifold.Points[manifold.Points.Length - 1];
Debug.DrawLine(start.position, end.position, color);
}
DebugDrawer.DrawSphere(start.position, 0.02f, color.ToOpacity(0.8f));
}
DebugDrawer.DrawAAConvexPolygon(manifold.Points.ToArray().Select(cp => (Vector3)cp.position).ToArray(), color);
}
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);
}
}
private static unsafe void DrawTests(int threadIndex, float3 start, float3 end, NativeString512 text, DrawingMethods methods, NativeArray <float3> polygon)
{
float3 offset = Vector3.up * 0.05f + Vector3.left * 0.05f;
float3 center = (start + (end - start) / 2);
if (methods.Sphere)
{
DebugDrawer.DrawSphere(start, 0.75f, UnityColors.GhostDodgerBlue);
}
if (methods.RectangleWithOutline)
{
var size = 0.25f;
var points = stackalloc[]
{
center + offset + new float3(0, 0, 0),
center + offset + new float3(0, size, 0),
center + offset + new float3(0, size, size),
center + offset + new float3(0, 0, size)
};
DebugDrawer.DrawSolidRectangleWithOutline(points, UnityColors.LightYellow, UnityColors.Yellow);
}
if (methods.Polygon)
{
DebugDrawer.DrawAAConvexPolygon(polygon, center + (float3)Vector3.down * 0.25f, UnityColors.GhostDodgerBlue);
}
if (methods.Line)
{
DebugDrawer.DrawLine(start + offset, end + offset);
}
if (methods.Ray)
{
DebugDrawer.DrawRay(center, Vector3.up, UnityColors.MediumBlue);
}
if (methods.Cone)
{
DebugDrawer.DrawCone(center + (float3)Vector3.up * 0.5f, Vector3.up, UnityColors.DarkKhaki, 22.5f);
}
if (methods.Circle)
{
DebugDrawer.DrawCircle(center, Vector3.up, 0.25f, UnityColors.AliceBlue);
}
if (methods.DottedLine)
{
DebugDrawer.DrawDottedLine(start, end, Color.yellow);
}
if (methods.WireCube)
{
DebugDrawer.DrawWireCube(end, Vector3.one / 2, Color.yellow);
}
if (methods.DottedWireCube)
{
DebugDrawer.DrawDottedWireCube(end, Vector3.one, Color.black);
}
if (methods.Label)
{
DebugDrawer.DrawLabel(center + (float3)Vector3.down * 0.25f, text);
}
if (methods.Arrow)
{
DebugDrawer.DrawArrow(start + (float3)Vector3.up * 0.5f, Vector3.up, Color.blue);
}
if (methods.Log)
{
DebugDrawer.Log(threadIndex, text);
}
if (methods.LogWarning)
{
DebugDrawer.LogWarning(text);
}
if (methods.LogError)
{
DebugDrawer.LogError(text);
}
if (methods.Point)
{
DebugDrawer.DrawPoint(center + (float3)Vector3.forward, UnityColors.Lavender, 0.25f);
}
}
}
