private static void CalculateFaceFacePoints(CollisionFunctor cf, PolyhedronPart a, PolyhedronPart b, ref CollisionInfo ci)
{
int[] faceA = a.Face(ci.FeatureA.Index);
int[] faceB = b.Face(ci.FeatureB.Index);
Vector3 pa, pb, n;
a.World(faceA[0], out pa);
a.FaceNormal(ci.FeatureA.Index, out n);
Plane planeA = new Plane(pa, n);
b.World(faceB[0], out pb);
Plane planeB = new Plane(pb, ci.Normal);
// vertices of A contained in face of B
for (int i = 0; i < faceA.Length; i++)
{
a.World(faceA[i], out pa);
planeB.ClosestPointTo(ref pa, out pb);
if (b.IsPointOnFace(ci.FeatureB.Index, ref pb, true))
{
cf.WritePoint(ref pa, ref pb, ref ci.Normal);
}
}
// vertices of B contained in face of A
for (int i = 0; i < faceB.Length; i++)
{
b.World(faceB[i], out pb);
planeA.ClosestPointTo(ref pb, out pa);
if (a.IsPointOnFace(ci.FeatureA.Index, ref pa, true))
{
cf.WritePoint(ref pa, ref pb, ref ci.Normal);
}
}
// intersections of edges from both faces
Segment ea, eb;
for (int i = 0; i < faceA.Length; i++)
{
a.World(faceA[i == 0 ? faceA.Length - 1 : i - 1], out ea.P1);
a.World(faceA[i], out ea.P2);
for (int j = 0; j < faceB.Length; j++)
{
b.World(faceB[j == 0 ? faceB.Length - 1 : j - 1], out eb.P1);
b.World(faceB[j], out eb.P2);
float sa, sb;
Segment.ClosestPoints(ref ea, ref eb, out sa, out pa, out sb, out pb);
if (sa > 0f && sa < 1f && sb > 0f && sb < 1f)
{
cf.WritePoint(ref pa, ref pb, ref ci.Normal);
}
}
}
}
public static bool DoOverlapTest(CollisionFunctor cf, SpherePart a, PolyhedronPart b, Vector3 offset)
{
Vector3 pa, pb, normal, v, center;
float depth, r2 = a.World.Radius * a.World.Radius;
bool foundAny = false;
for (int i = 0; i < b.FaceCount; i++)
{
b.World(b.Face(i)[0], out v);
b.FaceNormal(i, out normal);
var plane = new Plane(v, normal);
Vector3.Add(ref a.World.Center, ref offset, out center);
Vector3.Subtract(ref center, ref v, out v);
Vector3.Dot(ref normal, ref v, out depth);
if (depth < 0f || depth - a.World.Radius >= Constants.Epsilon)
{
continue;
}
plane.ClosestPointTo(ref center, out pb);
if (b.IsPointOnFace(i, ref pb, true))
{
Vector3.Subtract(ref pb, ref center, out v);
if (v.LengthSquared() - r2 < Constants.Epsilon)
{
Vector3.Multiply(ref normal, -a.World.Radius, out pa);
Vector3.Add(ref a.World.Center, ref pa, out pa);
cf.WritePoint(ref pa, ref pb, ref normal);
return(true);
}
}
else
{
int[] face = b.Face(i);
for (int j = 0; j < face.Length; j++)
{
float s;
Segment edge;
b.World(face[j == 0 ? face.Length - 1 : j - 1], out edge.P1);
b.World(face[j], out edge.P2);
edge.ClosestPointTo(ref center, out s, out pb);
Vector3.Subtract(ref center, ref pb, out normal);
if (normal.LengthSquared() - r2 < Constants.Epsilon)
{
normal.Normalize();
Vector3.Multiply(ref normal, -a.World.Radius, out pa);
Vector3.Add(ref a.World.Center, ref pa, out pa);
cf.WritePoint(ref pa, ref pb, ref normal);
foundAny = true;
}
}
}
}
return(foundAny);
}
public static bool DoOverlapTest(CollisionFunctor cf, SpherePart a, PolyhedronPart b, Vector3 offset)
{
Vector3 pa, pb, normal, v, center;
float depth, r2 = a.World.Radius * a.World.Radius;
bool foundAny = false;
for (int i = 0; i < b.FaceCount; i++)
{
b.World(b.Face(i)[0], out v);
b.FaceNormal(i, out normal);
var plane = new Plane(v, normal);
Vector3.Add(ref a.World.Center, ref offset, out center);
Vector3.Subtract(ref center, ref v, out v);
Vector3.Dot(ref normal, ref v, out depth);
if (depth < 0f || depth - a.World.Radius >= Constants.Epsilon)
continue;
plane.ClosestPointTo(ref center, out pb);
if (b.IsPointOnFace(i, ref pb, true))
{
Vector3.Subtract(ref pb, ref center, out v);
if (v.LengthSquared() - r2 < Constants.Epsilon)
{
Vector3.Multiply(ref normal, -a.World.Radius, out pa);
Vector3.Add(ref a.World.Center, ref pa, out pa);
cf.WritePoint(ref pa, ref pb, ref normal);
return true;
}
}
else
{
int[] face = b.Face(i);
for (int j = 0; j < face.Length; j++)
{
float s;
Segment edge;
b.World(face[j == 0 ? face.Length - 1 : j - 1], out edge.P1);
b.World(face[j], out edge.P2);
edge.ClosestPointTo(ref center, out s, out pb);
Vector3.Subtract(ref center, ref pb, out normal);
if (normal.LengthSquared() - r2 < Constants.Epsilon)
{
normal.Normalize();
Vector3.Multiply(ref normal, -a.World.Radius, out pa);
Vector3.Add(ref a.World.Center, ref pa, out pa);
cf.WritePoint(ref pa, ref pb, ref normal);
foundAny = true;
}
}
}
}
return foundAny;
}
private static void CalculateEdgeFacePoints(CollisionFunctor cf, PolyhedronPart a, PolyhedronPart b, ref CollisionInfo ci)
{
Vector3 pa, pb;
Segment ea, eb, eab;
int[] edgeA = a.Edge(ci.FeatureA.Index);
a.World(edgeA[0], out ea.P1);
a.World(edgeA[1], out ea.P2);
int[] faceB = b.Face(ci.FeatureB.Index);
b.World(faceB[0], out pb);
var planeB = new Plane(pb, ci.Normal);
planeB.ClosestPointTo(ref ea.P1, out eab.P1);
planeB.ClosestPointTo(ref ea.P2, out eab.P2);
int count = 0;
if (b.IsPointOnFace(ci.FeatureB.Index, ref eab.P1, true))
{
count++;
cf.WritePoint(ref ea.P1, ref eab.P1, ref ci.Normal);
}
if (b.IsPointOnFace(ci.FeatureB.Index, ref eab.P2, true))
{
count++;
cf.WritePoint(ref ea.P2, ref eab.P2, ref ci.Normal);
}
for (int i = 0; i < faceB.Length && count < 2; i++)
{
b.World(faceB[i == 0 ? faceB.Length - 1 : i - 1], out eb.P1);
b.World(faceB[i], out eb.P2);
float sa, sb;
Segment.ClosestPoints(ref ea, ref eb, out sa, out pa, out sb, out pb);
if (sa > 0f && sa < 1f && sb > 0f && sb < 1f)
{
count++;
cf.WritePoint(ref pa, ref pb, ref ci.Normal);
}
}
}
private static void CalculateFaceEdgePoints(CollisionFunctor cf, PolyhedronPart a, MeshPart b, ref Triangle tri, ref CollisionInfo ci)
{
Vector3 pa, pb;
Segment ea, eba, eb;
int[] faceA = a.Face(ci.FeatureA.Index);
a.World(faceA[0], out pa);
Plane planeA = new Plane(pa, ci.Normal);
tri.Edge(ci.FeatureB.Index, out eb);
planeA.ClosestPointTo(ref eb.P1, out eba.P1);
planeA.ClosestPointTo(ref eb.P2, out eba.P2);
int count = 0;
if (a.IsPointOnFace(ci.FeatureA.Index, ref eba.P1, true))
{
count++;
cf.WritePoint(ref eba.P1, ref eb.P1, ref ci.Normal);
}
if (a.IsPointOnFace(ci.FeatureA.Index, ref eba.P2, true))
{
count++;
cf.WritePoint(ref eba.P2, ref eb.P2, ref ci.Normal);
}
for (int i = 0; i < faceA.Length && count < 2; i++)
{
a.World(faceA[i == 0 ? faceA.Length - 1 : i - 1], out ea.P1);
a.World(faceA[i], out ea.P2);
float sa, sb;
Segment.ClosestPoints(ref ea, ref eb, out sa, out pa, out sb, out pb);
if (sa > 0f && sa < 1f && sb > 0f && sb < 1f)
{
count++;
cf.WritePoint(ref pa, ref pb, ref ci.Normal);
}
}
}
private static void CalculateFaceFacePoints(CollisionFunctor cf, PolyhedronPart a, MeshPart b, ref Triangle tri, ref CollisionInfo ci)
{
int[] faceA = a.Face(ci.FeatureA.Index);
Vector3 pa, pb, n;
a.World(faceA[0], out pa);
a.FaceNormal(ci.FeatureA.Index, out n);
Plane planeA = new Plane(pa, n);
Plane planeB = new Plane(tri.V1, tri.Normal);
// vertices of A contained in face of B
for (int i = 0; i < faceA.Length; i++)
{
a.World(faceA[i], out pa);
planeB.ClosestPointTo(ref pa, out pb);
if (tri.Contains(ref pb))
{
cf.WritePoint(ref pa, ref pb, ref ci.Normal);
}
}
for (int i = 1; i <= 3; i++)
{
tri.Vertex(i, out pb);
planeA.ClosestPointTo(ref pb, out pa);
if (a.IsPointOnFace(ci.FeatureA.Index, ref pa, true))
{
cf.WritePoint(ref pa, ref pb, ref ci.Normal);
}
}
// intersection of edges from both faces
Segment ea, eb;
for (int i = 0; i < faceA.Length; i++)
{
a.World(faceA[i == 0 ? faceA.Length - 1 : i - 1], out ea.P1);
a.World(faceA[i], out ea.P2);
for (int j = 1; j <= 3; j++)
{
tri.Edge(j, out eb);
float sa, sb;
Segment.ClosestPoints(ref ea, ref eb, out sa, out pa, out sb, out pb);
if (sa > 0f && sa < 1f && sb > 0f && sb < 1f)
{
cf.WritePoint(ref pa, ref pb, ref ci.Normal);
}
}
}
}
private static void CalculateFaceFacePoints(CollisionFunctor cf, PolyhedronPart a, PolyhedronPart b, ref CollisionInfo ci)
{
int[] faceA = a.Face(ci.FeatureA.Index);
int[] faceB = b.Face(ci.FeatureB.Index);
Vector3 pa, pb, n;
a.World(faceA[0], out pa);
a.FaceNormal(ci.FeatureA.Index, out n);
Plane planeA = new Plane(pa, n);
b.World(faceB[0], out pb);
Plane planeB = new Plane(pb, ci.Normal);
// vertices of A contained in face of B
for (int i = 0; i < faceA.Length; i++)
{
a.World(faceA[i], out pa);
planeB.ClosestPointTo(ref pa, out pb);
if(b.IsPointOnFace(ci.FeatureB.Index, ref pb, true))
{
cf.WritePoint(ref pa, ref pb, ref ci.Normal);
}
}
// vertices of B contained in face of A
for (int i = 0; i < faceB.Length; i++)
{
b.World(faceB[i], out pb);
planeA.ClosestPointTo(ref pb, out pa);
if (a.IsPointOnFace(ci.FeatureA.Index, ref pa, true))
{
cf.WritePoint(ref pa, ref pb, ref ci.Normal);
}
}
// intersections of edges from both faces
Segment ea, eb;
for (int i = 0; i < faceA.Length; i++)
{
a.World(faceA[i == 0 ? faceA.Length - 1 : i - 1], out ea.P1);
a.World(faceA[i], out ea.P2);
for (int j = 0; j < faceB.Length; j++)
{
b.World(faceB[j == 0 ? faceB.Length - 1 : j - 1], out eb.P1);
b.World(faceB[j], out eb.P2);
float sa, sb;
Segment.ClosestPoints(ref ea, ref eb, out sa, out pa, out sb, out pb);
if (sa > 0f && sa < 1f && sb > 0f && sb < 1f)
{
cf.WritePoint(ref pa, ref pb, ref ci.Normal);
}
}
}
}
private static void CalculateFaceEdgePoints(CollisionFunctor cf, PolyhedronPart a, PolyhedronPart b, ref CollisionInfo ci)
{
Vector3 pa, pb;
Segment ea, eb, eba;
int[] edgeB = b.Edge(ci.FeatureB.Index);
b.World(edgeB[0], out eb.P1);
b.World(edgeB[1], out eb.P2);
int[] faceA = a.Face(ci.FeatureA.Index);
a.World(faceA[0], out pa);
var planeA = new Plane(pa, ci.Normal);
planeA.ClosestPointTo(ref eb.P1, out eba.P1);
planeA.ClosestPointTo(ref eb.P2, out eba.P2);
int count = 0;
if (a.IsPointOnFace(ci.FeatureA.Index, ref eba.P1, true))
{
count++;
cf.WritePoint(ref eba.P1, ref eb.P1, ref ci.Normal);
}
if (a.IsPointOnFace(ci.FeatureA.Index, ref eba.P2, true))
{
count++;
cf.WritePoint(ref eba.P2, ref eb.P2, ref ci.Normal);
}
for(int i = 0; i < faceA.Length && count < 2; i++)
{
a.World(faceA[i == 0 ? faceA.Length - 1 : i - 1], out ea.P1);
a.World(faceA[i], out ea.P2);
float sa, sb;
Segment.ClosestPoints(ref ea, ref eb, out sa, out pa, out sb, out pb);
if (sa > 0f && sa < 1f && sb > 0f && sb < 1f)
{
count++;
cf.WritePoint(ref pa, ref pb, ref ci.Normal);
}
}
}
public static bool DoOverlapTest(CollisionFunctor cf, CapsulePart a, PolyhedronPart b, Vector3 offset)
{
Vector3 v, normal;
float ax1, ax2, bx, r2 = a.World.Radius * a.World.Radius;
Segment capa;
Vector3.Add(ref a.World.P1, ref offset, out capa.P1);
Vector3.Add(ref a.World.P2, ref offset, out capa.P2);
float curDepth, finalDepth = float.MaxValue;
int faceIdx = -1;
// find the face with the least penetration depth along its normal
for (int i = 0; i < b.FaceCount; i++)
{
b.World(b.Face(i)[0], out v);
b.FaceNormal(i, out normal);
Vector3.Dot(ref normal, ref v, out bx);
Vector3.Dot(ref normal, ref capa.P1, out ax1);
Vector3.Dot(ref normal, ref capa.P2, out ax2);
bx += a.World.Radius;
curDepth = Math.Max(bx - ax1, bx - ax2);
if (curDepth < 0f)
{
return(false);
}
if (curDepth < finalDepth)
{
faceIdx = i;
finalDepth = curDepth;
}
}
bool got1 = false, got2 = false;
Vector3 pa, pb, pa1, pa2, pb1, pb2;
pa1 = pa2 = pb1 = pb2 = Vector3.Zero;
var face = b.Face(faceIdx);
b.World(face[0], out v);
b.FaceNormal(faceIdx, out normal);
var plane = new Plane(v, normal);
Vector3.Dot(ref normal, ref v, out bx);
bx += a.World.Radius;
// determine if either capsule point is inside the face
pa1 = capa.P1;
plane.ClosestPointTo(ref pa1, out pb1);
Vector3.Dot(ref normal, ref pa1, out ax1);
got1 = ax1 - bx < Constants.Epsilon && b.IsPointOnFace(faceIdx, ref pb1, true);
pa2 = capa.P2;
plane.ClosestPointTo(ref pa2, out pb2);
Vector3.Dot(ref normal, ref pa2, out ax1);
if (ax1 - bx < Constants.Epsilon && b.IsPointOnFace(faceIdx, ref pb2, true))
{
if (got1)
{
got2 = true;
}
else
{
got1 = true;
pa1 = pa2;
pb1 = pb2;
}
}
// if one capsule point is inside the face but one is not, try to generate a second point on an edge
if (got1 ^ got2)
{
float sbPrev = float.NaN;
int edgePrev = -1;
for (int i = 0; i < face.Length; i++)
{
float dist, sa, sb;
Segment edge;
b.World(face[i == 0 ? face.Length - 1 : i - 1], out edge.P1);
b.World(face[i], out edge.P2);
Segment.ClosestPoints(ref capa, ref edge, out sa, out pa, out sb, out pb);
Vector3.DistanceSquared(ref pa, ref pb, out dist);
if (dist - r2 < Constants.Epsilon && sa >= Constants.Epsilon && sa < 1 - Constants.Epsilon)
{
if (i == face.Length - 1 && edgePrev == 0 && sb == 1f)
{
continue;
}
if (!got2 || (edgePrev == i - 1 && sbPrev == 1f))
{
pa2 = pa; pb2 = pb; got2 = true;
}
sbPrev = sb;
edgePrev = i;
}
}
}
else if (!got1 && !got2)
{
// neither point is inside the face, so try all edges
float sbPrev = float.NaN, edgePrev = float.NaN;
for (int i = 0; i < face.Length; i++)
{
float dist, sa, sb;
Segment edge;
b.World(face[i == 0 ? face.Length - 1 : i - 1], out edge.P1);
b.World(face[i], out edge.P2);
Segment.ClosestPoints(ref capa, ref edge, out sa, out pa, out sb, out pb);
Vector3.DistanceSquared(ref pa, ref pb, out dist);
if (dist - r2 < Constants.Epsilon && sb > Constants.Epsilon)
{
if (i == face.Length - 1 && edgePrev == 0 && sb == 1f)
{
continue;
}
if (!got1 || (edgePrev == i - 1 && sbPrev == 1f))
{
pa1 = pa; pb1 = pb; got1 = true;
}
else if (!got2)
{
pa2 = pa; pb2 = pb; got2 = true;
}
sbPrev = sb;
edgePrev = i;
}
}
// if only one edge was crossed, create a new normal instead of using the face normal
if (got1 ^ got2)
{
Vector3.Subtract(ref pa1, ref pb1, out normal);
normal.Normalize();
}
}
// write out points
if (got1 || got2)
{
Vector3.Multiply(ref normal, -a.World.Radius, out v);
if (got1)
{
Vector3.Add(ref pa1, ref v, out pa1);
Vector3.Subtract(ref pa1, ref offset, out pa1);
cf.WritePoint(ref pa1, ref pb1, ref normal);
}
if (got2)
{
Vector3.Add(ref pa2, ref v, out pa2);
Vector3.Subtract(ref pa2, ref offset, out pa2);
cf.WritePoint(ref pa2, ref pb2, ref normal);
}
}
return(got1 || got2);
}
public static bool DoOverlapTest(CollisionFunctor cf, CapsulePart a, PolyhedronPart b, Vector3 offset)
{
Vector3 v, normal;
float ax1, ax2, bx, r2 = a.World.Radius * a.World.Radius;
Segment capa;
Vector3.Add(ref a.World.P1, ref offset, out capa.P1);
Vector3.Add(ref a.World.P2, ref offset, out capa.P2);
float curDepth, finalDepth = float.MaxValue;
int faceIdx = -1;
// find the face with the least penetration depth along its normal
for (int i = 0; i < b.FaceCount; i++)
{
b.World(b.Face(i)[0], out v);
b.FaceNormal(i, out normal);
Vector3.Dot(ref normal, ref v, out bx);
Vector3.Dot(ref normal, ref capa.P1, out ax1);
Vector3.Dot(ref normal, ref capa.P2, out ax2);
bx += a.World.Radius;
curDepth = Math.Max(bx - ax1, bx - ax2);
if (curDepth < 0f)
return false;
if (curDepth < finalDepth)
{
faceIdx = i;
finalDepth = curDepth;
}
}
bool got1 = false, got2 = false;
Vector3 pa, pb, pa1, pa2, pb1, pb2;
pa1 = pa2 = pb1 = pb2 = Vector3.Zero;
var face = b.Face(faceIdx);
b.World(face[0], out v);
b.FaceNormal(faceIdx, out normal);
var plane = new Plane(v, normal);
Vector3.Dot(ref normal, ref v, out bx);
bx += a.World.Radius;
// determine if either capsule point is inside the face
pa1 = capa.P1;
plane.ClosestPointTo(ref pa1, out pb1);
Vector3.Dot(ref normal, ref pa1, out ax1);
got1 = ax1 - bx < Constants.Epsilon && b.IsPointOnFace(faceIdx, ref pb1, true);
pa2 = capa.P2;
plane.ClosestPointTo(ref pa2, out pb2);
Vector3.Dot(ref normal, ref pa2, out ax1);
if (ax1 - bx < Constants.Epsilon && b.IsPointOnFace(faceIdx, ref pb2, true))
{
if (got1)
got2 = true;
else
{
got1 = true;
pa1 = pa2;
pb1 = pb2;
}
}
// if one capsule point is inside the face but one is not, try to generate a second point on an edge
if (got1 ^ got2)
{
float sbPrev = float.NaN;
int edgePrev = -1;
for (int i = 0; i < face.Length; i++)
{
float dist, sa, sb;
Segment edge;
b.World(face[i == 0 ? face.Length - 1 : i - 1], out edge.P1);
b.World(face[i], out edge.P2);
Segment.ClosestPoints(ref capa, ref edge, out sa, out pa, out sb, out pb);
Vector3.DistanceSquared(ref pa, ref pb, out dist);
if (dist - r2 < Constants.Epsilon && sa >= Constants.Epsilon && sa < 1 - Constants.Epsilon)
{
if (i == face.Length - 1 && edgePrev == 0 && sb == 1f) continue;
if (!got2 || (edgePrev == i - 1 && sbPrev == 1f)) { pa2 = pa; pb2 = pb; got2 = true; }
sbPrev = sb;
edgePrev = i;
}
}
}
else if (!got1 && !got2)
{
// neither point is inside the face, so try all edges
float sbPrev = float.NaN, edgePrev = float.NaN;
for (int i = 0; i < face.Length; i++)
{
float dist, sa, sb;
Segment edge;
b.World(face[i == 0 ? face.Length - 1 : i - 1], out edge.P1);
b.World(face[i], out edge.P2);
Segment.ClosestPoints(ref capa, ref edge, out sa, out pa, out sb, out pb);
Vector3.DistanceSquared(ref pa, ref pb, out dist);
if (dist - r2 < Constants.Epsilon && sb > Constants.Epsilon)
{
if (i == face.Length - 1 && edgePrev == 0 && sb == 1f) continue;
if (!got1 || (edgePrev == i - 1 && sbPrev == 1f)) { pa1 = pa; pb1 = pb; got1 = true; }
else if (!got2) { pa2 = pa; pb2 = pb; got2 = true; }
sbPrev = sb;
edgePrev = i;
}
}
// if only one edge was crossed, create a new normal instead of using the face normal
if (got1 ^ got2)
{
Vector3.Subtract(ref pa1, ref pb1, out normal);
normal.Normalize();
}
}
// write out points
if (got1 || got2)
{
Vector3.Multiply(ref normal, -a.World.Radius, out v);
if (got1)
{
Vector3.Add(ref pa1, ref v, out pa1);
Vector3.Subtract(ref pa1, ref offset, out pa1);
cf.WritePoint(ref pa1, ref pb1, ref normal);
}
if (got2)
{
Vector3.Add(ref pa2, ref v, out pa2);
Vector3.Subtract(ref pa2, ref offset, out pa2);
cf.WritePoint(ref pa2, ref pb2, ref normal);
}
}
return got1 || got2;
}
private static void CalculateFaceFacePoints(CollisionFunctor cf, PolyhedronPart a, MeshPart b, ref Triangle tri, ref CollisionInfo ci)
{
int[] faceA = a.Face(ci.FeatureA.Index);
Vector3 pa, pb, n;
a.World(faceA[0], out pa);
a.FaceNormal(ci.FeatureA.Index, out n);
Plane planeA = new Plane(pa, n);
Plane planeB = new Plane(tri.V1, tri.Normal);
// vertices of A contained in face of B
for (int i = 0; i < faceA.Length; i++)
{
a.World(faceA[i], out pa);
planeB.ClosestPointTo(ref pa, out pb);
if (tri.Contains(ref pb))
{
cf.WritePoint(ref pa, ref pb, ref ci.Normal);
}
}
for (int i = 1; i <= 3; i++)
{
tri.Vertex(i, out pb);
planeA.ClosestPointTo(ref pb, out pa);
if (a.IsPointOnFace(ci.FeatureA.Index, ref pa, true))
{
cf.WritePoint(ref pa, ref pb, ref ci.Normal);
}
}
// intersection of edges from both faces
Segment ea, eb;
for (int i = 0; i < faceA.Length; i++)
{
a.World(faceA[i == 0 ? faceA.Length - 1 : i - 1], out ea.P1);
a.World(faceA[i], out ea.P2);
for (int j = 1; j <= 3; j++)
{
tri.Edge(j, out eb);
float sa, sb;
Segment.ClosestPoints(ref ea, ref eb, out sa, out pa, out sb, out pb);
if (sa > 0f && sa < 1f && sb > 0f && sb < 1f)
{
cf.WritePoint(ref pa, ref pb, ref ci.Normal);
}
}
}
}
