private static void CalculateVertexVertexPoint(CollisionFunctor cf, PolyhedronPart a, MeshPart b, ref Triangle tri, ref CollisionInfo ci)
{
Vector3 pa, pb;
a.World(ci.FeatureA.Index, out pa);
tri.Vertex(ci.FeatureB.Index, out pb);
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);
}
}
}
}
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 CalculateVertexFacePoint(CollisionFunctor cf, PolyhedronPart a, MeshPart b, ref Triangle tri, ref CollisionInfo ci)
{
Vector3 pa, pb;
a.World(ci.FeatureA.Index, out pa);
var plane = new Plane(tri.V1, ci.Normal);
plane.ClosestPointTo(ref pa, out pb);
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;
}
private static void CalculateFaceVertexPoint(CollisionFunctor cf, PolyhedronPart a, PolyhedronPart b, ref CollisionInfo ci)
{
Vector3 pa, pb;
b.World(ci.FeatureB.Index, out pb);
a.World(a.Face(ci.FeatureA.Index)[0], out pa);
var plane = new Plane(pa, ci.Normal);
plane.ClosestPointTo(ref pb, out pa);
cf.WritePoint(ref pa, ref pb, ref ci.Normal);
}
private static void CalculateContactPoints(CollisionFunctor cf, PolyhedronPart a, PolyhedronPart b, ref CollisionInfo ci)
{
if (ci.FeatureA.Type == PolyhedronFeatureType.None || ci.FeatureB.Type == PolyhedronFeatureType.None)
{
System.Diagnostics.Debug.WriteLine("Unhandled collision case!");
}
// calculate contact manifold
if (ci.FeatureB.Type == PolyhedronFeatureType.Vertex)
{
if (ci.FeatureA.Type == PolyhedronFeatureType.Vertex)
{
CalculateVertexVertexPoint(cf, a, b, ref ci);
}
else if (ci.FeatureA.Type == PolyhedronFeatureType.Edge)
{
CalculateEdgeVertexPoint(cf, a, b, ref ci);
}
else if (ci.FeatureA.Type == PolyhedronFeatureType.Face)
{
CalculateFaceVertexPoint(cf, a, b, ref ci);
}
}
else if (ci.FeatureB.Type == PolyhedronFeatureType.Edge)
{
if (ci.FeatureA.Type == PolyhedronFeatureType.Vertex)
{
CalculateVertexEdgePoint(cf, a, b, ref ci);
}
else if (ci.FeatureA.Type == PolyhedronFeatureType.Edge)
{
CalculateEdgeEdgePoints(cf, a, b, ref ci);
}
else if (ci.FeatureA.Type == PolyhedronFeatureType.Face)
{
CalculateFaceEdgePoints(cf, a, b, ref ci);
}
}
else if (ci.FeatureB.Type == PolyhedronFeatureType.Face)
{
if (ci.FeatureA.Type == PolyhedronFeatureType.Vertex)
{
CalculateVertexFacePoint(cf, a, b, ref ci);
}
else if (ci.FeatureA.Type == PolyhedronFeatureType.Edge)
{
CalculateEdgeFacePoints(cf, a, b, ref ci);
}
else if (ci.FeatureA.Type == PolyhedronFeatureType.Face)
{
CalculateFaceFacePoints(cf, a, b, ref ci);
}
}
}
private static void CalculateVertexEdgePoint(CollisionFunctor cf, PolyhedronPart a, MeshPart b, ref Triangle tri, ref CollisionInfo ci)
{
Vector3 pa, pb;
Segment eb;
float sb;
tri.Edge(ci.FeatureB.Index, out eb);
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 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 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 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);
}
public void Initialize(CollisionFunctor cf, PolyhedronPart a, MeshPart b, Vector3 delta)
{
_cf = cf;
_a = a;
_b = b;
_delta = delta;
_useSweptTest = _delta != Vector3.Zero;
Depth = float.MaxValue;
a.Center(out _center);
// transform bounding box to body space
b.BoundingBox(out BoundingBox);
AlignedBox.Transform(ref BoundingBox, ref b.TransformInverse, out BoundingBox);
}
private static void CalculateContactPoints(CollisionFunctor cf, PolyhedronPart a, MeshPart b, ref Triangle tri, ref CollisionInfo ci)
{
if (ci.FeatureB.Type == TriangleFeatureType.Vertex)
{
if (ci.FeatureA.Type == PolyhedronFeatureType.Vertex)
{
CalculateVertexVertexPoint(cf, a, b, ref tri, ref ci);
}
else if (ci.FeatureA.Type == PolyhedronFeatureType.Edge)
{
CalculateEdgeVertexPoint(cf, a, b, ref tri, ref ci);
}
else if (ci.FeatureA.Type == PolyhedronFeatureType.Face)
{
CalculateFaceVertexPoint(cf, a, b, ref tri, ref ci);
}
}
else if (ci.FeatureB.Type == TriangleFeatureType.Edge)
{
if (ci.FeatureA.Type == PolyhedronFeatureType.Vertex)
{
CalculateVertexEdgePoint(cf, a, b, ref tri, ref ci);
}
else if (ci.FeatureA.Type == PolyhedronFeatureType.Edge)
{
CalculateEdgeEdgePoints(cf, a, b, ref tri, ref ci);
}
else if (ci.FeatureA.Type == PolyhedronFeatureType.Face)
{
CalculateFaceEdgePoints(cf, a, b, ref tri, ref ci);
}
}
else if (ci.FeatureB.Type == TriangleFeatureType.Face)
{
if (ci.FeatureA.Type == PolyhedronFeatureType.Vertex)
{
CalculateVertexFacePoint(cf, a, b, ref tri, ref ci);
}
else if (ci.FeatureA.Type == PolyhedronFeatureType.Edge)
{
CalculateEdgeFacePoints(cf, a, b, ref tri, ref ci);
}
else if (ci.FeatureA.Type == PolyhedronFeatureType.Face)
{
CalculateFaceFacePoints(cf, a, b, ref tri, ref ci);
}
}
}
private static void DoOverlapTest(CollisionFunctor cf, PolyhedronPart a, PlanePart b, float offset)
{
Vector3 normalNeg;
Vector3.Negate(ref b.Plane.Normal, out normalNeg);
int vIndex = a.ExtremeVertex(ref normalNeg).Index;
float ax, bx;
Vector3 p = b.Plane.P;
if (offset > 0f)
{
Vector3.Multiply(ref b.Plane.Normal, offset, out p);
Vector3.Add(ref b.Plane.P, ref p, out p);
}
Vector3.Dot(ref b.Plane.Normal, ref p, out bx);
for (int i = 0; i < a.FaceCount; i++)
{
var face = a.Face(i);
bool skip = true;
for (int j = 0; j < face.Length; j++)
{
if (face[j] == vIndex)
{
skip = false;
break;
}
}
if (skip)
{
continue;
}
for (int j = 0; j < face.Length; j++)
{
Vector3 pa, pb;
a.World(face[j], out pa);
Vector3.Dot(ref b.Plane.Normal, ref pa, out ax);
if (ax - bx < Constants.Epsilon)
{
b.Plane.ClosestPointTo(ref pa, out pb);
cf.WritePoint(ref pa, ref pb, ref b.Plane.Normal);
}
}
}
}
private static void DoOverlapTest(CollisionFunctor cf, PolyhedronPart a, PlanePart b, float offset)
{
Vector3 normalNeg;
Vector3.Negate(ref b.Plane.Normal, out normalNeg);
int vIndex = a.ExtremeVertex(ref normalNeg).Index;
float ax, bx;
Vector3 p = b.Plane.P;
if(offset > 0f)
{
Vector3.Multiply(ref b.Plane.Normal, offset, out p);
Vector3.Add(ref b.Plane.P, ref p, out p);
}
Vector3.Dot(ref b.Plane.Normal, ref p, out bx);
for (int i = 0; i < a.FaceCount; i++)
{
var face = a.Face(i);
bool skip = true;
for (int j = 0; j < face.Length; j++)
{
if (face[j] == vIndex)
{
skip = false;
break;
}
}
if (skip)
continue;
for (int j = 0; j < face.Length; j++)
{
Vector3 pa, pb;
a.World(face[j], out pa);
Vector3.Dot(ref b.Plane.Normal, ref pa, out ax);
if (ax - bx < Constants.Epsilon)
{
b.Plane.ClosestPointTo(ref pa, out pb);
cf.WritePoint(ref pa, ref pb, ref b.Plane.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 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 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);
}
}
}
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 CalculateVertexEdgePoint(CollisionFunctor cf, PolyhedronPart a, MeshPart b, ref Triangle tri, ref CollisionInfo ci)
{
Vector3 pa, pb;
Segment eb;
float sb;
tri.Edge(ci.FeatureB.Index, out eb);
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 CalculateFaceVertexPoint(CollisionFunctor cf, PolyhedronPart a, MeshPart b, ref Triangle tri, ref CollisionInfo ci)
{
Vector3 pa, pb;
tri.Vertex(ci.FeatureB.Index, out pb);
a.World(a.Face(ci.FeatureA.Index)[0], out pa);
var plane = new Plane(pa, ci.Normal);
plane.ClosestPointTo(ref pb, out pa);
cf.WritePoint(ref pa, ref pb, ref ci.Normal);
}
private static void OverlapTest(CollisionFunctor cf, PolyhedronPart a, MeshPart b, ref Triangle tri)
{
CollisionInfo cur = new CollisionInfo(), final;
Vector3 v;
float d;
// axis: face normal of B
cur.Normal = tri.Normal;
Vector3.Negate(ref cur.Normal, out cur.NormalNeg);
cur.FeatureA = a.ExtremeVertex(ref cur.NormalNeg);
cur.FeatureA.X = -cur.FeatureA.X;
cur.FeatureB.Type = TriangleFeatureType.Face;
Vector3.Dot(ref cur.Normal, ref tri.V1, out cur.FeatureB.X);
cur.Depth = cur.FeatureB.X - cur.FeatureA.X;
if (cur.Depth <= -Constants.Epsilon)
{
return;
}
final = cur;
// axes: face normals of A
for (int i = 0; i < a.FaceCount; i++)
{
a.FaceNormal(i, out cur.NormalNeg);
Vector3.Negate(ref cur.NormalNeg, out cur.Normal);
a.World(a.Face(i)[0], out v);
cur.FeatureA = new PolyhedronFeature(PolyhedronFeatureType.Face, i, 0f);
Vector3.Dot(ref cur.Normal, ref v, out cur.FeatureA.X);
cur.FeatureB = tri.ExtremeVertex(ref cur.Normal);
cur.Depth = cur.FeatureB.X - cur.FeatureA.X;
if (cur.Depth <= -Constants.Epsilon)
{
return;
}
else if (cur.Depth < final.Depth)
{
final = cur;
}
}
// crossed edges from A and B
Vector3 centerA, centerB;
a.Center(out centerA);
tri.Center(out centerB);
for (int i = 0; i < a.EdgeVectorCount; i++)
{
for (int j = 1; j <= 3; j++)
{
// calculate normal from the two edge vectors
Vector3 eva, evb;
a.EdgeVector(i, out eva);
tri.EdgeVector(j, out evb);
Vector3.Cross(ref eva, ref evb, out cur.Normal);
if (cur.Normal.LengthSquared() < Constants.Epsilon)
{
continue;
}
cur.Normal.Normalize();
float ca, cb;
Vector3.Dot(ref cur.Normal, ref centerA, out ca);
Vector3.Dot(ref cur.Normal, ref centerB, out cb);
if (ca < cb)
{
cur.NormalNeg = cur.Normal;
Vector3.Negate(ref cur.NormalNeg, out cur.Normal);
}
else
{
Vector3.Negate(ref cur.Normal, out cur.NormalNeg);
}
// skip this normal if it's close to one we already have
Vector3.Dot(ref cur.Normal, ref final.Normal, out d);
if (Math.Abs(1f - d) < Constants.Epsilon)
{
continue;
}
cur.FeatureA = a.ExtremeVertex(ref cur.NormalNeg);
cur.FeatureA.X = -cur.FeatureA.X;
cur.FeatureB = tri.ExtremeVertex(ref cur.Normal);
cur.Depth = cur.FeatureB.X - cur.FeatureA.X;
if (cur.Depth <= -Constants.Epsilon)
{
return;
}
else if (final.Depth - cur.Depth >= Constants.Epsilon)
{
final = cur;
}
}
}
if (final.FeatureA.Type == PolyhedronFeatureType.Vertex)
{
final.FeatureA = a.ExtremeFeature(ref final.NormalNeg, -final.FeatureB.X);
final.FeatureA.X = -final.FeatureA.X;
}
if (final.FeatureB.Type == TriangleFeatureType.Vertex)
{
final.FeatureB = tri.ExtremeFeature(ref final.Normal, final.FeatureA.X);
}
// make sure the normal points outward
Vector3.Dot(ref tri.Normal, ref final.Normal, out d);
if (d < 0f)
{
Vector3.Multiply(ref tri.Normal, -2f * d, out v);
Vector3.Add(ref final.Normal, ref v, out final.Normal);
}
CalculateContactPoints(cf, a, b, ref tri, ref final);
}
private static void OverlapTest(CollisionFunctor cf, PolyhedronPart a, MeshPart b, ref Triangle tri)
{
CollisionInfo cur = new CollisionInfo(), final;
Vector3 v;
float d;
// axis: face normal of B
cur.Normal = tri.Normal;
Vector3.Negate(ref cur.Normal, out cur.NormalNeg);
cur.FeatureA = a.ExtremeVertex(ref cur.NormalNeg);
cur.FeatureA.X = -cur.FeatureA.X;
cur.FeatureB.Type = TriangleFeatureType.Face;
Vector3.Dot(ref cur.Normal, ref tri.V1, out cur.FeatureB.X);
cur.Depth = cur.FeatureB.X - cur.FeatureA.X;
if (cur.Depth <= -Constants.Epsilon)
return;
final = cur;
// axes: face normals of A
for (int i = 0; i < a.FaceCount; i++)
{
a.FaceNormal(i, out cur.NormalNeg);
Vector3.Negate(ref cur.NormalNeg, out cur.Normal);
a.World(a.Face(i)[0], out v);
cur.FeatureA = new PolyhedronFeature(PolyhedronFeatureType.Face, i, 0f);
Vector3.Dot(ref cur.Normal, ref v, out cur.FeatureA.X);
cur.FeatureB = tri.ExtremeVertex(ref cur.Normal);
cur.Depth = cur.FeatureB.X - cur.FeatureA.X;
if (cur.Depth <= -Constants.Epsilon)
return;
else if (cur.Depth < final.Depth)
final = cur;
}
// crossed edges from A and B
Vector3 centerA, centerB;
a.Center(out centerA);
tri.Center(out centerB);
for (int i = 0; i < a.EdgeVectorCount; i++)
{
for (int j = 1; j <= 3; j++)
{
// calculate normal from the two edge vectors
Vector3 eva, evb;
a.EdgeVector(i, out eva);
tri.EdgeVector(j, out evb);
Vector3.Cross(ref eva, ref evb, out cur.Normal);
if (cur.Normal.LengthSquared() < Constants.Epsilon)
continue;
cur.Normal.Normalize();
float ca, cb;
Vector3.Dot(ref cur.Normal, ref centerA, out ca);
Vector3.Dot(ref cur.Normal, ref centerB, out cb);
if (ca < cb)
{
cur.NormalNeg = cur.Normal;
Vector3.Negate(ref cur.NormalNeg, out cur.Normal);
}
else
Vector3.Negate(ref cur.Normal, out cur.NormalNeg);
// skip this normal if it's close to one we already have
Vector3.Dot(ref cur.Normal, ref final.Normal, out d);
if (Math.Abs(1f - d) < Constants.Epsilon)
continue;
cur.FeatureA = a.ExtremeVertex(ref cur.NormalNeg);
cur.FeatureA.X = -cur.FeatureA.X;
cur.FeatureB = tri.ExtremeVertex(ref cur.Normal);
cur.Depth = cur.FeatureB.X - cur.FeatureA.X;
if (cur.Depth <= -Constants.Epsilon)
return;
else if (final.Depth - cur.Depth >= Constants.Epsilon)
final = cur;
}
}
if (final.FeatureA.Type == PolyhedronFeatureType.Vertex)
{
final.FeatureA = a.ExtremeFeature(ref final.NormalNeg, -final.FeatureB.X);
final.FeatureA.X = -final.FeatureA.X;
}
if (final.FeatureB.Type == TriangleFeatureType.Vertex)
{
final.FeatureB = tri.ExtremeFeature(ref final.Normal, final.FeatureA.X);
}
// make sure the normal points outward
Vector3.Dot(ref tri.Normal, ref final.Normal, out d);
if (d < 0f)
{
Vector3.Multiply(ref tri.Normal, -2f * d, out v);
Vector3.Add(ref final.Normal, ref v, out final.Normal);
}
CalculateContactPoints(cf, a, b, ref tri, ref final);
}
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);
}
private static void CalculateContactPoints(CollisionFunctor cf, PolyhedronPart a, MeshPart b, ref Triangle tri, ref CollisionInfo ci)
{
if (ci.FeatureB.Type == TriangleFeatureType.Vertex)
{
if (ci.FeatureA.Type == PolyhedronFeatureType.Vertex)
CalculateVertexVertexPoint(cf, a, b, ref tri, ref ci);
else if (ci.FeatureA.Type == PolyhedronFeatureType.Edge)
CalculateEdgeVertexPoint(cf, a, b, ref tri, ref ci);
else if (ci.FeatureA.Type == PolyhedronFeatureType.Face)
CalculateFaceVertexPoint(cf, a, b, ref tri, ref ci);
}
else if (ci.FeatureB.Type == TriangleFeatureType.Edge)
{
if (ci.FeatureA.Type == PolyhedronFeatureType.Vertex)
CalculateVertexEdgePoint(cf, a, b, ref tri, ref ci);
else if (ci.FeatureA.Type == PolyhedronFeatureType.Edge)
CalculateEdgeEdgePoints(cf, a, b, ref tri, ref ci);
else if (ci.FeatureA.Type == PolyhedronFeatureType.Face)
CalculateFaceEdgePoints(cf, a, b, ref tri, ref ci);
}
else if (ci.FeatureB.Type == TriangleFeatureType.Face)
{
if (ci.FeatureA.Type == PolyhedronFeatureType.Vertex)
CalculateVertexFacePoint(cf, a, b, ref tri, ref ci);
else if (ci.FeatureA.Type == PolyhedronFeatureType.Edge)
CalculateEdgeFacePoints(cf, a, b, ref tri, ref ci);
else if (ci.FeatureA.Type == PolyhedronFeatureType.Face)
CalculateFaceFacePoints(cf, a, b, ref tri, ref ci);
}
}
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 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 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 CalculateVertexFacePoint(CollisionFunctor cf, PolyhedronPart a, PolyhedronPart b, ref CollisionInfo ci)
{
Vector3 pa, pb;
a.World(ci.FeatureA.Index, out pa);
b.World(b.Face(ci.FeatureB.Index)[0], out pb);
var plane = new Plane(pb, ci.Normal);
plane.ClosestPointTo(ref pa, out pb);
cf.WritePoint(ref pa, ref pb, ref ci.Normal);
}
private static void CalculateContactPoints(CollisionFunctor cf, PolyhedronPart a, PolyhedronPart b, ref CollisionInfo ci)
{
if (ci.FeatureA.Type == PolyhedronFeatureType.None || ci.FeatureB.Type == PolyhedronFeatureType.None)
//System.Diagnostics.Debug.WriteLine("Unhandled collision case!");
// calculate contact manifold
if (ci.FeatureB.Type == PolyhedronFeatureType.Vertex)
{
if (ci.FeatureA.Type == PolyhedronFeatureType.Vertex)
CalculateVertexVertexPoint(cf, a, b, ref ci);
else if (ci.FeatureA.Type == PolyhedronFeatureType.Edge)
CalculateEdgeVertexPoint(cf, a, b, ref ci);
else if (ci.FeatureA.Type == PolyhedronFeatureType.Face)
CalculateFaceVertexPoint(cf, a, b, ref ci);
}
else if (ci.FeatureB.Type == PolyhedronFeatureType.Edge)
{
if (ci.FeatureA.Type == PolyhedronFeatureType.Vertex)
CalculateVertexEdgePoint(cf, a, b, ref ci);
else if (ci.FeatureA.Type == PolyhedronFeatureType.Edge)
CalculateEdgeEdgePoints(cf, a, b, ref ci);
else if (ci.FeatureA.Type == PolyhedronFeatureType.Face)
CalculateFaceEdgePoints(cf, a, b, ref ci);
}
else if (ci.FeatureB.Type == PolyhedronFeatureType.Face)
{
if (ci.FeatureA.Type == PolyhedronFeatureType.Vertex)
CalculateVertexFacePoint(cf, a, b, ref ci);
else if (ci.FeatureA.Type == PolyhedronFeatureType.Edge)
CalculateEdgeFacePoints(cf, a, b, ref ci);
else if (ci.FeatureA.Type == PolyhedronFeatureType.Face)
CalculateFaceFacePoints(cf, a, b, ref ci);
}
}
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 void Initialize(CollisionFunctor cf, PolyhedronPart a, MeshPart b, Vector3 delta)
{
_cf = cf;
_a = a;
_b = b;
_delta = delta;
_useSweptTest = _delta != Vector3.Zero;
Depth = float.MaxValue;
a.Center(out _center);
// transform bounding box to body space
b.BoundingBox(out BoundingBox);
AlignedBox.Transform(ref BoundingBox, ref b.TransformInverse, out BoundingBox);
}
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 bool SweptTest(CollisionFunctor cf, PolyhedronPart a, MeshPart b, ref Triangle tri, ref Vector3 delta)
{
CollisionInfo cur = new CollisionInfo(), final = new CollisionInfo() { Depth = float.MaxValue };
Vector3 v;
float d, dx, curTime, finalTime = 0f, tlast = float.MaxValue;
// axis: face normal of B
cur.Normal = tri.Normal;
Vector3.Negate(ref cur.Normal, out cur.NormalNeg);
cur.FeatureA = a.ExtremeVertex(ref cur.NormalNeg);
cur.FeatureA.X = -cur.FeatureA.X;
cur.FeatureB.Type = TriangleFeatureType.Face;
Vector3.Dot(ref cur.Normal, ref tri.V1, out cur.FeatureB.X);
cur.Depth = cur.FeatureB.X - cur.FeatureA.X;
Vector3.Dot(ref cur.Normal, ref delta, out dx);
curTime = cur.Depth / dx;
final = cur;
if (cur.Depth >= 0f)
{
if (dx > 0f)
tlast = curTime;
}
else
{
if (dx >= 0f || curTime > 1f)
return false;
finalTime = curTime;
}
// axes: face normals of A
for (int i = 0; i < a.FaceCount; i++)
{
a.FaceNormal(i, out cur.NormalNeg);
Vector3.Negate(ref cur.NormalNeg, out cur.Normal);
a.World(a.Face(i)[0], out v);
cur.FeatureA = new PolyhedronFeature(PolyhedronFeatureType.Face, i, 0f);
Vector3.Dot(ref cur.Normal, ref v, out cur.FeatureA.X);
cur.FeatureB = tri.ExtremeVertex(ref cur.Normal);
cur.Depth = cur.FeatureB.X - cur.FeatureA.X;
Vector3.Dot(ref cur.Normal, ref delta, out dx);
curTime = cur.Depth / dx;
if (cur.Depth >= 0f)
{
if (finalTime <= 0f && cur.Depth < final.Depth)
{
final = cur;
finalTime = 0f;
}
if (dx > 0f && curTime < tlast)
tlast = curTime;
}
else
{
if (dx >= 0f || curTime > 1f)
return false;
if (curTime > finalTime)
{
final = cur;
finalTime = curTime;
}
}
}
// crossed edges from A and B
Vector3 centerA, centerB;
a.Center(out centerA);
tri.Center(out centerB);
for (int i = 0; i < a.EdgeVectorCount; i++)
{
for (int j = 1; j <= 3; j++)
{
// calculate normal from the two edge vectors
Vector3 eva, evb;
a.EdgeVector(i, out eva);
tri.EdgeVector(j, out evb);
Vector3.Cross(ref eva, ref evb, out cur.Normal);
if (cur.Normal.LengthSquared() < Constants.Epsilon)
continue;
cur.Normal.Normalize();
float ca, cb;
Vector3.Dot(ref cur.Normal, ref centerA, out ca);
Vector3.Dot(ref cur.Normal, ref centerB, out cb);
if (ca < cb)
{
cur.NormalNeg = cur.Normal;
Vector3.Negate(ref cur.NormalNeg, out cur.Normal);
}
else
Vector3.Negate(ref cur.Normal, out cur.NormalNeg);
// skip this normal if it's close to one we already have
Vector3.Dot(ref cur.Normal, ref final.Normal, out d);
if (Math.Abs(1f - d) < Constants.Epsilon)
continue;
cur.FeatureA = a.ExtremeVertex(ref cur.NormalNeg);
cur.FeatureA.X = -cur.FeatureA.X;
cur.FeatureB = tri.ExtremeVertex(ref cur.Normal);
cur.Depth = cur.FeatureB.X - cur.FeatureA.X;
Vector3.Dot(ref cur.Normal, ref delta, out dx);
curTime = cur.Depth / dx;
if (cur.Depth >= 0f)
{
if (finalTime <= 0f && cur.Depth < final.Depth)
{
final = cur;
finalTime = 0f;
}
if (dx > 0f && curTime < tlast)
tlast = curTime;
}
else
{
if (dx >= 0f || curTime > 1f)
return false;
if (curTime > finalTime)
{
final = cur;
finalTime = curTime;
}
}
}
}
if (finalTime >= tlast)
return false;
Vector3.Dot(ref final.Normal, ref delta, out dx);
if (finalTime <= 0f)
dx = 0f;
if (final.FeatureA.Type == PolyhedronFeatureType.Vertex)
{
final.FeatureA = a.ExtremeFeature(ref final.NormalNeg, dx - final.FeatureB.X);
final.FeatureA.X = -final.FeatureA.X;
}
if (final.FeatureB.Type == TriangleFeatureType.Vertex)
{
final.FeatureB = tri.ExtremeFeature(ref final.Normal, dx + final.FeatureA.X);
}
// make sure the normal points outward
Vector3.Dot(ref tri.Normal, ref final.Normal, out d);
if (d < 0f)
{
Vector3.Multiply(ref tri.Normal, -2f * d, out v);
Vector3.Add(ref final.Normal, ref v, out final.Normal);
}
CalculateContactPoints(cf, a, b, ref tri, ref final);
return true;
}
private static void CalculateVertexVertexPoint(CollisionFunctor cf, PolyhedronPart a, PolyhedronPart b, ref CollisionInfo ci)
{
Vector3 pa, pb;
a.World(ci.FeatureA.Index, out pa);
b.World(ci.FeatureB.Index, out pb);
cf.WritePoint(ref pa, ref pb, ref ci.Normal);
}
private static bool SweptTest(CollisionFunctor cf, PolyhedronPart a, MeshPart b, ref Triangle tri, ref Vector3 delta)
{
CollisionInfo cur = new CollisionInfo(), final = new CollisionInfo()
{
Depth = float.MaxValue
};
Vector3 v;
float d, dx, curTime, finalTime = 0f, tlast = float.MaxValue;
// axis: face normal of B
cur.Normal = tri.Normal;
Vector3.Negate(ref cur.Normal, out cur.NormalNeg);
cur.FeatureA = a.ExtremeVertex(ref cur.NormalNeg);
cur.FeatureA.X = -cur.FeatureA.X;
cur.FeatureB.Type = TriangleFeatureType.Face;
Vector3.Dot(ref cur.Normal, ref tri.V1, out cur.FeatureB.X);
cur.Depth = cur.FeatureB.X - cur.FeatureA.X;
Vector3.Dot(ref cur.Normal, ref delta, out dx);
curTime = cur.Depth / dx;
final = cur;
if (cur.Depth >= 0f)
{
if (dx > 0f)
{
tlast = curTime;
}
}
else
{
if (dx >= 0f || curTime > 1f)
{
return(false);
}
finalTime = curTime;
}
// axes: face normals of A
for (int i = 0; i < a.FaceCount; i++)
{
a.FaceNormal(i, out cur.NormalNeg);
Vector3.Negate(ref cur.NormalNeg, out cur.Normal);
a.World(a.Face(i)[0], out v);
cur.FeatureA = new PolyhedronFeature(PolyhedronFeatureType.Face, i, 0f);
Vector3.Dot(ref cur.Normal, ref v, out cur.FeatureA.X);
cur.FeatureB = tri.ExtremeVertex(ref cur.Normal);
cur.Depth = cur.FeatureB.X - cur.FeatureA.X;
Vector3.Dot(ref cur.Normal, ref delta, out dx);
curTime = cur.Depth / dx;
if (cur.Depth >= 0f)
{
if (finalTime <= 0f && cur.Depth < final.Depth)
{
final = cur;
finalTime = 0f;
}
if (dx > 0f && curTime < tlast)
{
tlast = curTime;
}
}
else
{
if (dx >= 0f || curTime > 1f)
{
return(false);
}
if (curTime > finalTime)
{
final = cur;
finalTime = curTime;
}
}
}
// crossed edges from A and B
Vector3 centerA, centerB;
a.Center(out centerA);
tri.Center(out centerB);
for (int i = 0; i < a.EdgeVectorCount; i++)
{
for (int j = 1; j <= 3; j++)
{
// calculate normal from the two edge vectors
Vector3 eva, evb;
a.EdgeVector(i, out eva);
tri.EdgeVector(j, out evb);
Vector3.Cross(ref eva, ref evb, out cur.Normal);
if (cur.Normal.LengthSquared() < Constants.Epsilon)
{
continue;
}
cur.Normal.Normalize();
float ca, cb;
Vector3.Dot(ref cur.Normal, ref centerA, out ca);
Vector3.Dot(ref cur.Normal, ref centerB, out cb);
if (ca < cb)
{
cur.NormalNeg = cur.Normal;
Vector3.Negate(ref cur.NormalNeg, out cur.Normal);
}
else
{
Vector3.Negate(ref cur.Normal, out cur.NormalNeg);
}
// skip this normal if it's close to one we already have
Vector3.Dot(ref cur.Normal, ref final.Normal, out d);
if (Math.Abs(1f - d) < Constants.Epsilon)
{
continue;
}
cur.FeatureA = a.ExtremeVertex(ref cur.NormalNeg);
cur.FeatureA.X = -cur.FeatureA.X;
cur.FeatureB = tri.ExtremeVertex(ref cur.Normal);
cur.Depth = cur.FeatureB.X - cur.FeatureA.X;
Vector3.Dot(ref cur.Normal, ref delta, out dx);
curTime = cur.Depth / dx;
if (cur.Depth >= 0f)
{
if (finalTime <= 0f && cur.Depth < final.Depth)
{
final = cur;
finalTime = 0f;
}
if (dx > 0f && curTime < tlast)
{
tlast = curTime;
}
}
else
{
if (dx >= 0f || curTime > 1f)
{
return(false);
}
if (curTime > finalTime)
{
final = cur;
finalTime = curTime;
}
}
}
}
if (finalTime >= tlast)
{
return(false);
}
Vector3.Dot(ref final.Normal, ref delta, out dx);
if (finalTime <= 0f)
{
dx = 0f;
}
if (final.FeatureA.Type == PolyhedronFeatureType.Vertex)
{
final.FeatureA = a.ExtremeFeature(ref final.NormalNeg, dx - final.FeatureB.X);
final.FeatureA.X = -final.FeatureA.X;
}
if (final.FeatureB.Type == TriangleFeatureType.Vertex)
{
final.FeatureB = tri.ExtremeFeature(ref final.Normal, dx + final.FeatureA.X);
}
// make sure the normal points outward
Vector3.Dot(ref tri.Normal, ref final.Normal, out d);
if (d < 0f)
{
Vector3.Multiply(ref tri.Normal, -2f * d, out v);
Vector3.Add(ref final.Normal, ref v, out final.Normal);
}
CalculateContactPoints(cf, a, b, ref tri, ref final);
return(true);
}
