void OnDrawGizmos()
{
{
var s = 0.25f;
var com = MassProperties.CenterOfMass;
Gizmos.color = Color.white;
Gizmos.DrawLine(transform.TransformPoint(com + new float3(-s, 0, 0)), transform.TransformPoint(com + new float3(+s, 0, 0)));
Gizmos.DrawLine(transform.TransformPoint(com + new float3(0, -s, 0)), transform.TransformPoint(com + new float3(0, +s, 0)));
Gizmos.DrawLine(transform.TransformPoint(com + new float3(0, 0, -s)), transform.TransformPoint(com + new float3(0, 0, +s)));
}
if (UpdateMesh)
{
UpdateMesh = false;
UpdateMeshNow();
}
// Display faces.
if (ShowFaces && HullData.Faces != null)
{
MTransform trs = new MTransform(transform.rotation, transform.position);
Gizmos.color = Color.white;
foreach (Face face in HullData.Faces)
{
var offset = face.Plane.Normal * 0.0001f;
for (int i = face.NumVertices - 1, j = 0; j < face.NumVertices; i = j++)
{
var a = Hull.Vertices[HullData.FaceVertices[face.FirstVertex + i]].Position;
var b = Hull.Vertices[HullData.FaceVertices[face.FirstVertex + j]].Position;
a = Mul(trs, a + offset);
b = Mul(trs, b + offset);
Gizmos.DrawLine(a, b);
}
}
}
// Display triangles.
if (ShowTriangles)
{
MTransform trs = new MTransform(transform.rotation, transform.position);
Gizmos.color = Color.white;
for (int i = Hull.Triangles.GetFirstIndex(); i != -1; i = Hull.Triangles.GetNextIndex(i))
{
var a = Mul(trs, Hull.Vertices[Hull.Triangles[i].Vertex0].Position);
var b = Mul(trs, Hull.Vertices[Hull.Triangles[i].Vertex1].Position);
var c = Mul(trs, Hull.Vertices[Hull.Triangles[i].Vertex2].Position);
Gizmos.DrawLine(a, b);
Gizmos.DrawLine(b, c);
Gizmos.DrawLine(c, a);
}
}
// Display vertex normals.
if (ShowVertexNormals)
{
MTransform trs = new MTransform(transform.rotation, transform.position);
Gizmos.color = Color.white;
for (var vertex = Hull.Triangles.GetFirstIndex(); vertex != -1; vertex = Hull.Triangles.GetNextIndex(vertex))
{
var normal = math.mul(trs.Rotation, Hull.ComputeVertexNormal(vertex)) * 0.25f;
var start = Mul(trs, Hull.Vertices[vertex].Position);
Gizmos.DrawRay(start, normal);
}
}
// Display labels.
if (ShowLabels)
{
}
// Compute distance to every other convex hulls.
if (CollideOthers)
{
var thisId = GetInstanceID();
var cvxs = FindObjectsOfType <ConvexConvexDistanceTest>();
MTransform transformA = new MTransform(transform.rotation, transform.position);
float3[] verticesA = GetVertexArray();
foreach (var cvx in cvxs)
{
if (cvx.GetInstanceID() == thisId)
{
continue;
}
MTransform transformB = new MTransform(cvx.transform.rotation, cvx.transform.position);
float3[] verticesB = cvx.GetVertexArray();
MTransform btoA = Mul(Inverse(transformA), transformB);
ConvexConvexDistanceQueries.Result result;
fixed(float3 *va = verticesA)
{
fixed(float3 *vb = verticesB)
{
result = ConvexConvexDistanceQueries.ConvexConvex(va, verticesA.Length, vb, verticesB.Length, btoA, PenetrationHandling);
}
}
var from = Mul(transformA, result.ClosestPoints.PositionOnAinA);
var to = Mul(transformA, result.ClosestPoints.PositionOnBinA);
if (TraceQueryResults)
{
Debug.Log($"Iterations={result.Iterations}, plane={result.ClosestPoints.NormalInA}, distance={result.ClosestPoints.Distance}");
Debug.Log($"Features A = [{result.Simplex[0] >> 16}, {result.Simplex[1] >> 16}, {result.Simplex[2] >> 16}]");
Debug.Log($"Features B = [{result.Simplex[0] & 0xffff}, {result.Simplex[1] & 0xffff}, {result.Simplex[2] & 0xffff}]");
}
if (ShowManifold && Hull.Dimension == 3 && cvx.Hull.Dimension == 3)
{
DrawManifold(Experimental, result,
transformA, ref Hull, ref HullData,
transformB, ref cvx.Hull, ref cvx.HullData);
}
if (ShowProjection)
{
Gizmos.color = Color.white;
var trs = Mul(transformA, new MTransform(float3x3.identity, result.ClosestPoints.NormalInA * result.ClosestPoints.Distance));
{
var tv = stackalloc float3[Hull.Vertices.PeakCount];
for (var vertex = Hull.Vertices.GetFirstIndex(); vertex != -1; vertex = Hull.Vertices.GetNextIndex(vertex))
{
tv[vertex] = Mul(trs, Hull.Vertices[vertex].Position);
}
if (Hull.Dimension == 3)
{
for (var edge = Hull.GetFirstPrimaryEdge(); edge.IsValid; edge = Hull.GetNextPrimaryEdge(edge))
{
Gizmos.DrawLine(tv[Hull.StartVertex(edge)], tv[Hull.EndVertex(edge)]);
}
}
else if (Hull.Dimension >= 1)
{
for (int i = Hull.Vertices.PeakCount - 1, j = 0; j < Hull.Vertices.PeakCount; i = j++)
{
Gizmos.DrawLine(tv[i], tv[j]);
}
}
}
}
Gizmos.color = Color.red;
Gizmos.DrawSphere(from, 0.05f);
Gizmos.color = Color.green;
Gizmos.DrawSphere(to, 0.05f);
Gizmos.color = Color.white;
Gizmos.DrawLine(from, to);
if (ShowCso)
{
Gizmos.color = Color.yellow;
using (var cso = new ConvexHullBuilder(8192, 8192 * 2))
{
for (int i = 0; i < verticesA.Length; ++i)
{
for (int j = 0; j < verticesB.Length; ++j)
{
cso.AddPoint(verticesA[i] - Mul(btoA, verticesB[j]));
}
}
if (cso.Dimension == 2)
{
for (int n = cso.Vertices.PeakCount, i = n - 1, j = 0; j < n; i = j++)
{
Gizmos.DrawLine(cso.Vertices[i].Position, cso.Vertices[j].Position);
}
}
else if (cso.Dimension == 3)
{
foreach (var triangle in cso.Triangles.Elements)
{
Gizmos.DrawLine(cso.Vertices[triangle.Vertex0].Position, cso.Vertices[triangle.Vertex1].Position);
Gizmos.DrawLine(cso.Vertices[triangle.Vertex1].Position, cso.Vertices[triangle.Vertex2].Position);
Gizmos.DrawLine(cso.Vertices[triangle.Vertex2].Position, cso.Vertices[triangle.Vertex0].Position);
}
}
Gizmos.DrawLine(new float3(-0.1f, 0, 0), new float3(+0.1f, 0, 0));
Gizmos.DrawLine(new float3(0, -0.1f, 0), new float3(0, +0.1f, 0));
Gizmos.DrawLine(new float3(0, 0, -0.1f), new float3(0, 0, +0.1f));
}
}
}
}
// Draw vertices.
#if UNITY_EDITOR
GUIStyle labelStyle = new GUIStyle();
labelStyle.fontSize = 24;
#endif
Gizmos.color = Color.yellow;
for (int i = Hull.Vertices.GetFirstIndex(); i != -1; i = Hull.Vertices.GetNextIndex(i))
{
var w = transform.TransformPoint(Hull.Vertices[i].Position);
#if UNITY_EDITOR
if (ShowLabels)
{
Handles.color = Color.white;
Handles.Label(w, $"{i}:{Hull.Vertices[i].Cardinality}", labelStyle);
}
else
#endif
{
Gizmos.DrawSphere(w, 0.01f);
}
}
}