private static void CalculateEdgeVertexPoint(CollisionFunctor cf, PolyhedronPart a, MeshPart b, ref Triangle tri, ref CollisionInfo ci)
{
Vector3 pa, pb;
Segment ea;
float sa;
int[] edgeA = a.Edge(ci.FeatureA.Index);
a.World(edgeA[0], out ea.P1);
a.World(edgeA[1], out ea.P2);
tri.Vertex(ci.FeatureB.Index, out pb);
ea.ClosestPointTo(ref pb, out sa, out pa);
cf.WritePoint(ref pa, ref pb, ref ci.Normal);
}
private static void CalculateVertexEdgePoint(CollisionFunctor cf, PolyhedronPart a, PolyhedronPart b, ref CollisionInfo ci)
{
Vector3 pa, pb;
Segment eb;
float sb;
int[] edgeB = b.Edge(ci.FeatureB.Index);
b.World(edgeB[0], out eb.P1);
b.World(edgeB[1], out eb.P2);
a.World(ci.FeatureA.Index, out pa);
eb.ClosestPointTo(ref pa, out sb, out pb);
cf.WritePoint(ref pa, ref pb, ref ci.Normal);
}
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 CalculateEdgeFacePoints(CollisionFunctor cf, PolyhedronPart a, MeshPart b, ref Triangle tri, ref CollisionInfo ci)
{
Vector3 pa, pb;
Segment ea, eb, eab;
int[] edge = a.Edge(ci.FeatureA.Index);
a.World(edge[0], out ea.P1);
a.World(edge[1], out ea.P2);
Plane planeB = new Plane(tri.V1, tri.Normal);
planeB.ClosestPointTo(ref ea.P1, out eab.P1);
planeB.ClosestPointTo(ref ea.P2, out eab.P2);
int count = 0;
if (tri.Contains(ref eab.P1))
{
count++;
cf.WritePoint(ref ea.P1, ref eab.P1, ref ci.Normal);
}
if (tri.Contains(ref eab.P2))
{
count++;
cf.WritePoint(ref ea.P2, ref eab.P2, ref ci.Normal);
}
for (int i = 1; i <= 3 && count < 2; i++)
{
tri.Edge(i, 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)
{
count++;
cf.WritePoint(ref pa, ref pb, ref ci.Normal);
}
}
}
private static void CalculateVertexEdgePoint(CollisionFunctor cf, PolyhedronPart a, PolyhedronPart b, ref CollisionInfo ci)
{
Vector3 pa, pb;
Segment eb;
float sb;
int[] edgeB = b.Edge(ci.FeatureB.Index);
b.World(edgeB[0], out eb.P1);
b.World(edgeB[1], out eb.P2);
a.World(ci.FeatureA.Index, out pa);
eb.ClosestPointTo(ref pa, out sb, out pb);
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);
}
}
}
private static void CalculateEdgeFacePoints(CollisionFunctor cf, PolyhedronPart a, MeshPart b, ref Triangle tri, ref CollisionInfo ci)
{
Vector3 pa, pb;
Segment ea, eb, eab;
int[] edge = a.Edge(ci.FeatureA.Index);
a.World(edge[0], out ea.P1);
a.World(edge[1], out ea.P2);
Plane planeB = new Plane(tri.V1, tri.Normal);
planeB.ClosestPointTo(ref ea.P1, out eab.P1);
planeB.ClosestPointTo(ref ea.P2, out eab.P2);
int count = 0;
if (tri.Contains(ref eab.P1))
{
count++;
cf.WritePoint(ref ea.P1, ref eab.P1, ref ci.Normal);
}
if (tri.Contains(ref eab.P2))
{
count++;
cf.WritePoint(ref ea.P2, ref eab.P2, ref ci.Normal);
}
for (int i = 1; i <= 3 && count < 2; i++)
{
tri.Edge(i, 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)
{
count++;
cf.WritePoint(ref pa, ref pb, ref ci.Normal);
}
}
}
private static void CalculateEdgeEdgePoints(CollisionFunctor cf, PolyhedronPart a, MeshPart b, ref Triangle tri, ref CollisionInfo ci)
{
Vector3 pa, pb;
Segment ea, eb;
int[] edgeA = a.Edge(ci.FeatureA.Index);
a.World(edgeA[0], out ea.P1);
a.World(edgeA[1], out ea.P2);
tri.Edge(ci.FeatureB.Index, out eb);
float sa, sb;
Segment.ClosestPoints(ref ea, ref eb, out sa, out pa, out sb, out pb);
cf.WritePoint(ref pa, ref pb, ref ci.Normal);
}