public static unsafe void QueryEdgeDistance(out EdgeQueryResult result, RigidTransform transform1, NativeHull hull1, RigidTransform transform2, NativeHull hull2) { // Perform computations in the local space of the second hull. RigidTransform transform = math.mul(math.inverse(transform2), transform1); float3 C1 = transform.pos; result.Distance = -float.MaxValue; result.Index1 = -1; result.Index2 = -1; for (int i = 0; i < hull1.EdgeCount; i += 2) { NativeHalfEdge *edge1 = hull1.GetEdgePtr(i); NativeHalfEdge *twin1 = hull1.GetEdgePtr(i + 1); Debug.Assert(edge1->Twin == i + 1 && twin1->Twin == i); float3 P1 = math.transform(transform, hull1.GetVertex(edge1->Origin)); float3 Q1 = math.transform(transform, hull1.GetVertex(twin1->Origin)); float3 E1 = Q1 - P1; float3 U1 = math.rotate(transform, hull1.GetPlane(edge1->Face).Normal); float3 V1 = math.rotate(transform, hull1.GetPlane(twin1->Face).Normal); for (int j = 0; j < hull2.EdgeCount; j += 2) { NativeHalfEdge *edge2 = hull2.GetEdgePtr(j); NativeHalfEdge *twin2 = hull2.GetEdgePtr(j + 1); Debug.Assert(edge2->Twin == j + 1 && twin2->Twin == j); float3 P2 = hull2.GetVertex(edge2->Origin); float3 Q2 = hull2.GetVertex(twin2->Origin); float3 E2 = Q2 - P2; float3 U2 = hull2.GetPlane(edge2->Face).Normal; float3 V2 = hull2.GetPlane(twin2->Face).Normal; if (IsMinkowskiFace(U1, V1, -E1, -U2, -V2, -E2)) { float distance = Project(P1, E1, P2, E2, C1); if (distance > result.Distance) { result.Index1 = i; result.Index2 = j; result.Distance = distance; } } } } }
/// <summary> /// Populates a list with transformed face vertices. /// </summary> public static unsafe void ComputeFaceClippingPolygon(ref NativeList <ClipVertex> output, int faceIndex, RigidTransform t, NativeHull hull) { Debug.Assert(output.IsCreated); NativeFace * face = hull.GetFacePtr(faceIndex); NativePlane plane = hull.GetPlane(faceIndex); NativeHalfEdge *start = hull.GetEdgePtr(face->Edge); NativeHalfEdge *current = start; do { NativeHalfEdge *twin = hull.GetEdgePtr(current->Twin); float3 vertex = hull.GetVertex(current->Origin); float3 P = math.transform(t, vertex); ClipVertex clipVertex; clipVertex.featurePair.InEdge1 = -1; clipVertex.featurePair.OutEdge1 = -1; clipVertex.featurePair.InEdge2 = (sbyte)current->Next; clipVertex.featurePair.OutEdge2 = (sbyte)twin->Twin; clipVertex.position = P; clipVertex.hull2local = vertex; clipVertex.plane = plane; output.Add(clipVertex); current = hull.GetEdgePtr(current->Next); } while (current != start); }
public static unsafe void b3CreateEdgeContact(ref NativeManifold output, EdgeQueryResult input, RigidTransform transform1, NativeHull hull1, RigidTransform transform2, NativeHull hull2) { Debug.Assert(output.IsCreated); ContactPoint cp = default; if (input.Index1 < 0 || input.Index2 < 0) { return; } NativeHalfEdge *edge1 = hull1.GetEdgePtr(input.Index1); NativeHalfEdge *twin1 = hull1.GetEdgePtr(edge1->Twin); float3 P1 = math.transform(transform1, hull1.GetVertex(edge1->Origin)); float3 Q1 = math.transform(transform1, hull1.GetVertex(twin1->Origin)); float3 E1 = Q1 - P1; NativeHalfEdge *edge2 = hull2.GetEdgePtr(input.Index2); NativeHalfEdge *twin2 = hull2.GetEdgePtr(edge2->Twin); float3 P2 = math.transform(transform1, hull2.GetVertex(edge2->Origin)); float3 Q2 = math.transform(transform1, hull2.GetVertex(twin2->Origin)); float3 E2 = Q2 - P2; float3 normal = math.normalize(math.cross(Q1 - P1, Q2 - P2)); float3 C2C1 = transform2.pos - transform1.pos; if (math.dot(normal, C2C1) < 0) { // Flip output.Normal = -normal; cp.Id.FeaturePair.InEdge1 = (sbyte)input.Index2; cp.Id.FeaturePair.OutEdge1 = (sbyte)(input.Index2 + 1); cp.Id.FeaturePair.InEdge2 = (sbyte)(input.Index1 + 1); cp.Id.FeaturePair.OutEdge2 = (sbyte)input.Index1; } else { output.Normal = normal; cp.Id.FeaturePair.InEdge1 = (sbyte)input.Index1; cp.Id.FeaturePair.OutEdge1 = (sbyte)(input.Index1 + 1); cp.Id.FeaturePair.InEdge2 = (sbyte)(input.Index2 + 1); cp.Id.FeaturePair.OutEdge2 = (sbyte)input.Index2; } // Compute the closest points between the two edges (center point of penetration) ClosestPointsSegmentSegment(P1, Q1, P2, Q2, out float3 C1, out float3 C2); float3 position = 0.5f * (C1 + C2); //// the closest points on each hull //cp.positionOnTarget = Math3d.ProjectPointOnLineSegment(P2, Q2, C2); //cp.positionOnSource = Math3d.ProjectPointOnLineSegment(P1, Q1, C1); cp.Penetration = C1 - C2; cp.Position = position; cp.Distance = input.Distance; output.Add(cp); }
public static void DrawDebugHull(NativeHull hull, RigidTransform t, DebugHullFlags options = DebugHullFlags.All, Color BaseColor = default) { if (options == DebugHullFlags.None) { return; } if (BaseColor == default) { BaseColor = Color.yellow; } float faceExplosionDistance = (options & DebugHullFlags.ExplodeFaces) != 0 ? 0.3f : 0; // Iterate each twin pair at the same time. for (int j = 0; j < hull.edgeCount; j = j + 2) { var edge = hull.GetEdge(j); var twin = hull.GetEdge(j + 1); var edgePlane = edge.face != -1 ? hull.GetPlane(edge.face) : new NativePlane(); var twinPlane = twin.face != -1 ? hull.GetPlane(twin.face) : new NativePlane(); var rotatedEdgeNormal = math.rotate(t, edgePlane.Normal); var rotatedTwinNormal = math.rotate(t, twinPlane.Normal); var edgeVertex1 = math.transform(t, hull.GetVertex(edge.origin)); var twinVertex1 = math.transform(t, hull.GetVertex(twin.origin)); var edgeVertex2 = math.transform(t, hull.GetVertex(edge.origin)); var twinVertex2 = math.transform(t, hull.GetVertex(twin.origin)); if ((options & DebugHullFlags.Outline) != 0) { Debug.DrawLine(edgeVertex1 + rotatedEdgeNormal * faceExplosionDistance, twinVertex1 + rotatedEdgeNormal * faceExplosionDistance, BaseColor); Debug.DrawLine(edgeVertex2 + rotatedTwinNormal * faceExplosionDistance, twinVertex2 + rotatedTwinNormal * faceExplosionDistance, BaseColor); } if ((options & DebugHullFlags.EdgeLinks) != 0) { Debug.DrawLine((edgeVertex1 + twinVertex1) / 2 + rotatedEdgeNormal * faceExplosionDistance, (edgeVertex2 + twinVertex2) / 2 + rotatedTwinNormal * faceExplosionDistance, Color.gray); } } if ((options & DebugHullFlags.PlaneNormals) != 0) { hull.IterateFaces((int index, ref NativePlane plane, ref NativeHalfEdge firstEdge) => { var tPlane = plane.Transform(t); DebugDrawer.DebugArrow(tPlane.Position, tPlane.Rotation * 0.2f, BaseColor); }); } if ((options & DebugHullFlags.Indices) != 0) { var dupeCheck = new HashSet <Vector3>(); for (int i = 0; i < hull.vertexCount; i++) { // Offset the label if multiple verts are on the same position. var v = math.transform(t, hull.GetVertex(i)); var offset = dupeCheck.Contains(v) ? (float3)Vector3.forward * 0.05f : 0; DebugDrawer.DrawLabel(v + offset, i.ToString()); dupeCheck.Add(v); } } if ((options & DebugHullFlags.FaceWinding) != 0) { for (int i = 0; i < hull.faceCount; i++) { var face = hull.GetFace(i); var plane = hull.GetPlane(i); var tPlane = t * plane; var edge = hull.GetEdge(face.edge); var startOrigin = edge.origin; do { var nextEdge = hull.GetEdge(edge.next); var startVert = math.transform(t, hull.GetVertex(edge.origin)); var endVert = math.transform(t, hull.GetVertex(nextEdge.origin)); var center = (endVert + startVert) / 2; var dir = math.normalize(endVert - startVert); var insetDir = math.normalize(math.cross(tPlane.Normal, dir)); if ((options & DebugHullFlags.ExplodeFaces) != 0) { DebugDrawer.DebugArrow(center + tPlane.Normal * faceExplosionDistance, dir * 0.2f, Color.black); } else { DebugDrawer.DebugArrow(center + tPlane.Normal * faceExplosionDistance + insetDir * 0.1f, dir * 0.2f, Color.black); } edge = nextEdge; } while (edge.origin != startOrigin); } } }