static List <int> GetPerVertexFaces(int vertex, ref HullFaceData data) { var set = new List <int>(); VertexFaces vf = data.VertexFaces[vertex]; for (int j = 0; j < vf.NumFaces; ++j) { set.Add(data.FaceIndices[vf.FirstFace + j]); } return(set); }
static void DrawManifold(bool useExerimentalMethod, ConvexConvexDistanceQueries.Result distance, MTransform trsA, ref ConvexHullBuilder hullA, ref HullFaceData dataA, MTransform trsB, ref ConvexHullBuilder hullB, ref HullFaceData dataB) { MTransform btoA = Mul(Inverse(trsA), trsB); MTransform atoB = Mul(Inverse(trsB), trsA); Face faceA = SelectBestFace(dataA.Faces, distance.ClosestPoints.NormalInA); Face faceB = SelectBestFace(dataB.Faces, math.mul(atoB.Rotation, -distance.ClosestPoints.NormalInA)); if (useExerimentalMethod) { var legacyFaceA = faceA; var legacyFaceB = faceB; // Experimental method. // Extract faces sub-set of A. { float bestDot = -2; var normal = distance.ClosestPoints.NormalInA; for (int i = 0; i < distance.SimplexDimension; ++i) { int vi = distance.SimplexVertexA(i); for (int j = 0; j < dataA.VertexFaces[vi].NumFaces; ++j) { var k = dataA.FaceIndices[dataA.VertexFaces[vi].FirstFace + j]; var face = dataA.Faces[k]; var d = math.dot(face.Plane.Normal, normal); if (d > bestDot) { faceA = face; bestDot = d; } } } } // Extract faces sub-set of B. { float bestDot = -2; var normal = math.mul(atoB.Rotation, -distance.ClosestPoints.NormalInA); for (int i = 0; i < distance.SimplexDimension; ++i) { int vi = distance.SimplexVertexB(i); for (int j = 0; j < dataB.VertexFaces[vi].NumFaces; ++j) { var k = dataB.FaceIndices[dataB.VertexFaces[vi].FirstFace + j]; var face = dataB.Faces[k]; var d = math.dot(face.Plane.Normal, normal); if (d > bestDot) { faceB = face; bestDot = d; } } } } if (legacyFaceA.FirstVertex != faceA.FirstVertex || legacyFaceB.FirstVertex != faceB.FirstVertex) { Debug.LogError("Different face set found."); } } var facePlaneAinA = faceA.Plane; var facePlaneBinA = TransformPlane(btoA, faceB.Plane); /*drawFace(trsA, ref hullA, faceA, dataA.faceVertices, 0.01f, Color.yellow); * drawFace(trsB, ref hullB, faceB, dataB.faceVertices, 0.01f, Color.yellow);*/ const float eps = 1e-6f; var va = new List <float3>(); var vb = new List <float3>(); for (int i = 0; i < faceA.NumVertices; ++i) { va.Add(hullA.Vertices[dataA.FaceVertices[faceA.FirstVertex + i]].Position); } for (int i = 0; i < faceB.NumVertices; ++i) { vb.Add(Mul(btoA, hullB.Vertices[dataB.FaceVertices[faceB.FirstVertex + i]].Position)); } var vt = new List <float3>(); for (int ai = va.Count - 1, aj = 0; aj < va.Count; ai = aj++) { var plane = new float4(math.normalize(math.cross(distance.ClosestPoints.NormalInA, va[ai] - va[aj])), 0); plane.w = -math.dot(plane.xyz, va[ai]); if (vb.Count > 1) { int bi = vb.Count - 1; float d2Pi = DistanceToPlaneEps(plane, vb[bi], eps); for (int bj = 0; bj < vb.Count; bi = bj++) { var d2Pj = DistanceToPlaneEps(plane, vb[bj], eps); if ((d2Pi * d2Pj) < 0) { float isec = d2Pi / (d2Pi - d2Pj); vt.Add(vb[bi] + (vb[bj] - vb[bi]) * isec); } if (d2Pj <= 0) { vt.Add(vb[bj]); } d2Pi = d2Pj; } } var temp = vb; vb = vt; vt = temp; vt.Clear(); } if (vb.Count == 0) { vb.Add(distance.ClosestPoints.PositionOnBinA); } { GUIStyle labelStyle = new GUIStyle(); labelStyle.fontSize = 16; var projectionInvDen = 1 / math.dot(distance.ClosestPoints.NormalInA, facePlaneAinA.Normal); int i = vb.Count - 1; var piOnB = vb[i]; var piD = math.dot(facePlaneAinA, new float4(piOnB, 1)) * projectionInvDen; var piOnA = piOnB - distance.ClosestPoints.NormalInA * piD; for (int j = 0; j < vb.Count; i = j++) { #if UNITY_EDITOR var center = Mul(trsA, (piOnA + piOnB) / 2); Handles.Label(center, $"{piD}", labelStyle); #endif var pjOnB = vb[j]; var pjD = math.dot(facePlaneAinA, new float4(pjOnB, 1)) * projectionInvDen; var pjOnA = pjOnB - distance.ClosestPoints.NormalInA * pjD; Gizmos.DrawLine(Mul(trsA, piOnB), Mul(trsA, pjOnB)); Gizmos.DrawLine(Mul(trsA, piOnA), Mul(trsA, pjOnA)); Gizmos.DrawLine(Mul(trsA, pjOnB), Mul(trsA, pjOnA)); piOnB = pjOnB; piOnA = pjOnA; piD = pjD; } } }