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;
}
}
}