public override void OverlapTest(CollisionFunctor cf, Part partA, Part partB)
{
var a = (CapsulePart)partA;
var b = (CapsulePart)partB;
DoOverlapTest(cf, a, b, Vector3.Zero);
}
public override void OverlapTest(CollisionFunctor cf, Part partA, Part partB)
{
var a = (CapsulePart)partA;
var b = (PlanePart)partB;
float ax, bx;
Vector3 pa, pb, radius;
Vector3.Multiply(ref b.Plane.Normal, -a.World.Radius, out radius);
Vector3.Dot(ref b.Plane.Normal, ref b.Plane.P, out bx);
Vector3.Dot(ref b.Plane.Normal, ref a.World.P1, out ax);
if (ax - bx - a.World.Radius < Constants.Epsilon)
{
Vector3.Add(ref a.World.P1, ref radius, out pa);
b.Plane.ClosestPointTo(ref pa, out pb);
cf.WritePoint(ref pa, ref pb, ref b.Plane.Normal);
}
Vector3.Dot(ref b.Plane.Normal, ref a.World.P2, out ax);
if (ax - bx - a.World.Radius < Constants.Epsilon)
{
Vector3.Add(ref a.World.P2, ref radius, out pa);
b.Plane.ClosestPointTo(ref pa, out pb);
cf.WritePoint(ref pa, ref pb, ref b.Plane.Normal);
}
}
public override void SweptTest(CollisionFunctor cf, Part partA, Part partB, Vector3 delta)
{
var a = (SpherePart)partA;
var b = (PlanePart)partB;
Vector3 pa = a.World.Center, pb;
float ax, bx;
Vector3.Dot(ref b.Plane.Normal, ref b.Plane.P, out bx);
Vector3.Dot(ref b.Plane.Normal, ref pa, out ax);
if (ax - bx - a.World.Radius >= Constants.Epsilon)
{
Vector3.Add(ref a.World.Center, ref delta, out pa);
Vector3.Dot(ref b.Plane.Normal, ref pa, out ax);
if (ax - bx - a.World.Radius >= Constants.Epsilon)
{
return;
}
}
b.Plane.ClosestPointTo(ref pa, out pb);
Vector3.Multiply(ref b.Plane.Normal, -a.World.Radius, out pa);
Vector3.Add(ref a.World.Center, ref pa, out pa);
cf.WritePoint(ref pa, ref pb, ref b.Plane.Normal);
}
public override void OverlapTest(CollisionFunctor cf, Part partA, Part partB)
{
var a = (SpherePart)partA;
var b = (PolyhedronPart)partB;
DoOverlapTest(cf, a, b, Vector3.Zero);
}
public override void SweptTest(CollisionFunctor cf, Part partA, Part partB, Vector3 delta)
{
var a = (SpherePart)partA;
var b = (CapsulePart)partB;
Segment path;
path.P1 = a.World.Center;
Vector3.Add(ref path.P1, ref delta, out path.P2);
Capsule cap = b.World;
cap.Radius += a.World.Radius;
Segment capSegment = new Segment(b.World.P1, b.World.P2);
float k;
Vector3 pa, pb, normal;
cap.Intersect(ref path, out k, out pa);
if (k <= 1f)
{
capSegment.ClosestPointTo(ref pa, out k, out pb);
Vector3.Subtract(ref pa, ref pb, out normal);
normal.Normalize();
Vector3.Multiply(ref normal, b.World.Radius, out pa);
Vector3.Add(ref pb, ref pa, out pb);
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);
}
}
public override void Execute(CollisionFunctor cf)
{
while (_axisX.Count > _rows)
{
SetCapacity(_rows * _growFactor);
}
_tasks.AddTask(SortAndSweepX);
_tasks.AddTask(SortAndSweepY);
_tasks.AddTask(SortAndSweepZ);
_tasks.Execute();
_newItems = 0;
for (int i = 0; i < _rows; i++)
{
for (int j = 0; j < _columns; j++)
{
uint block = _matrixX[i, j] & _matrixY[i, j] & _matrixZ[i, j];
if (block > 0)
{
for (int k = 0; k < sizeof(uint); k++)
{
if (((1 << k) & block) > 0)
{
_tasks.AddTask(cf.WritePair, _list[i], _list[j * sizeof(uint) + k]);
}
}
}
}
}
_tasks.Execute();
}
public override void OverlapTest(CollisionFunctor cf, Part partA, Part partB)
{
var a = (CapsulePart)partA;
var b = (PolyhedronPart)partB;
DoOverlapTest(cf, a, b, Vector3.Zero);
}
public override void OverlapTest(CollisionFunctor cf, Part partA, Part partB)
{
var a = (SpherePart)partA;
var b = (CapsulePart)partB;
Segment cap = new Segment(b.World.P1, b.World.P2);
Vector3 pa, pb, normal, v;
float r2 = a.World.Radius + b.World.Radius;
r2 *= r2;
float sb;
cap.ClosestPointTo(ref a.World.Center, out sb, out pb);
Vector3.Subtract(ref a.World.Center, ref pb, out normal);
if (normal.LengthSquared() - r2 >= Constants.Epsilon)
{
return;
}
normal.Normalize();
Vector3.Multiply(ref normal, -a.World.Radius, out v);
Vector3.Add(ref a.World.Center, ref v, out pa);
Vector3.Multiply(ref normal, b.World.Radius, out v);
Vector3.Add(ref pb, ref v, out pb);
cf.WritePoint(ref pa, ref pb, ref normal);
}
public override void OverlapTest(CollisionFunctor cf, Part partA, Part partB)
{
var a = (PolyhedronPart)partA;
var b = (PlanePart)partB;
PolyhedronPlane.DoOverlapTest(cf, a, b, 0f);
}
public override void SweptTest(CollisionFunctor cf, Part partA, Part partB, Vector3 delta)
{
var a = (SpherePart)partA;
var b = (PlanePart)partB;
Vector3 pa = a.World.Center, pb;
float ax, bx;
Vector3.Dot(ref b.Plane.Normal, ref b.Plane.P, out bx);
Vector3.Dot(ref b.Plane.Normal, ref pa, out ax);
if (ax - bx - a.World.Radius >= Constants.Epsilon)
{
Vector3.Add(ref a.World.Center, ref delta, out pa);
Vector3.Dot(ref b.Plane.Normal, ref pa, out ax);
if (ax - bx - a.World.Radius >= Constants.Epsilon)
{
return;
}
}
b.Plane.ClosestPointTo(ref pa, out pb);
Vector3.Multiply(ref b.Plane.Normal, -a.World.Radius, out pa);
Vector3.Add(ref a.World.Center, ref pa, out pa);
cf.WritePoint(ref pa, ref pb, ref b.Plane.Normal);
}
public override void SweptTest(CollisionFunctor cf, Part partA, Part partB, Vector3 delta)
{
var a = (SpherePart)partA;
var b = (CapsulePart)partB;
Segment path;
path.P1 = a.World.Center;
Vector3.Add(ref path.P1, ref delta, out path.P2);
Capsule cap = b.World;
cap.Radius += a.World.Radius;
Segment capSegment = new Segment(b.World.P1, b.World.P2);
float k;
Vector3 pa, pb, normal;
cap.Intersect(ref path, out k, out pa);
if (k <= 1f)
{
capSegment.ClosestPointTo(ref pa, out k, out pb);
Vector3.Subtract(ref pa, ref pb, out normal);
normal.Normalize();
Vector3.Multiply(ref normal, b.World.Radius, out pa);
Vector3.Add(ref pb, ref pa, out pb);
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);
}
}
public override void OverlapTest(CollisionFunctor cf, Part partA, Part partB)
{
var a = (PolyhedronPart)partA;
var b = (PlanePart)partB;
PolyhedronPlane.DoOverlapTest(cf, a, b, 0f);
}
public override void SweptTest(CollisionFunctor cf, Part partA, Part partB, Vector3 delta)
{
var a = (CapsulePart)partA;
var b = (PlanePart)partB;
float ax, bx, dx;
Vector3 pa, pb, radius;
Vector3.Multiply(ref b.Plane.Normal, -a.World.Radius, out radius);
Vector3.Dot(ref b.Plane.Normal, ref b.Plane.P, out bx);
Vector3.Dot(ref b.Plane.Normal, ref delta, out dx);
dx = MathHelper.Min(dx, 0f);
Vector3.Dot(ref b.Plane.Normal, ref a.World.P1, out ax);
if (ax - bx - a.World.Radius + dx < Constants.Epsilon)
{
Vector3.Add(ref a.World.P1, ref radius, out pa);
b.Plane.ClosestPointTo(ref pa, out pb);
cf.WritePoint(ref pa, ref pb, ref b.Plane.Normal);
}
Vector3.Dot(ref b.Plane.Normal, ref a.World.P2, out ax);
if (ax - bx - a.World.Radius + dx < Constants.Epsilon)
{
Vector3.Add(ref a.World.P2, ref radius, out pa);
b.Plane.ClosestPointTo(ref pa, out pb);
cf.WritePoint(ref pa, ref pb, ref b.Plane.Normal);
}
}
public override void SweptTest(CollisionFunctor cf, Part partA, Part partB, Vector3 delta)
{
var a = (CapsulePart)partA;
var b = (PlanePart)partB;
float ax, bx, dx;
Vector3 pa, pb, radius;
Vector3.Multiply(ref b.Plane.Normal, -a.World.Radius, out radius);
Vector3.Dot(ref b.Plane.Normal, ref b.Plane.P, out bx);
Vector3.Dot(ref b.Plane.Normal, ref delta, out dx);
dx = MathHelper.Min(dx, 0f);
Vector3.Dot(ref b.Plane.Normal, ref a.World.P1, out ax);
if (ax - bx - a.World.Radius + dx < Constants.Epsilon)
{
Vector3.Add(ref a.World.P1, ref radius, out pa);
b.Plane.ClosestPointTo(ref pa, out pb);
cf.WritePoint(ref pa, ref pb, ref b.Plane.Normal);
}
Vector3.Dot(ref b.Plane.Normal, ref a.World.P2, out ax);
if (ax - bx - a.World.Radius + dx < Constants.Epsilon)
{
Vector3.Add(ref a.World.P2, ref radius, out pa);
b.Plane.ClosestPointTo(ref pa, out pb);
cf.WritePoint(ref pa, ref pb, ref b.Plane.Normal);
}
}
public override void OverlapTest(CollisionFunctor cf, Part partA, Part partB)
{
var a = (CapsulePart)partA;
var b = (PlanePart)partB;
float ax, bx;
Vector3 pa, pb, radius;
Vector3.Multiply(ref b.Plane.Normal, -a.World.Radius, out radius);
Vector3.Dot(ref b.Plane.Normal, ref b.Plane.P, out bx);
Vector3.Dot(ref b.Plane.Normal, ref a.World.P1, out ax);
if (ax - bx - a.World.Radius < Constants.Epsilon)
{
Vector3.Add(ref a.World.P1, ref radius, out pa);
b.Plane.ClosestPointTo(ref pa, out pb);
cf.WritePoint(ref pa, ref pb, ref b.Plane.Normal);
}
Vector3.Dot(ref b.Plane.Normal, ref a.World.P2, out ax);
if (ax - bx - a.World.Radius < Constants.Epsilon)
{
Vector3.Add(ref a.World.P2, ref radius, out pa);
b.Plane.ClosestPointTo(ref pa, out pb);
cf.WritePoint(ref pa, ref pb, ref b.Plane.Normal);
}
}
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 override void OverlapTest(CollisionFunctor cf, Part partA, Part partB)
{
var a = (CapsulePart)partA;
var b = (MeshPart)partB;
var tf = OverlapFunctor;
tf.Initialize(cf, a, b, Vector3.Zero);
b.ProcessTriangles(tf);
}
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 override void SweptTest(CollisionFunctor cf, Part partA, Part partB, Vector3 delta)
{
var a = (PolyhedronPart)partA;
var b = (MeshPart)partB;
var tf = Functor;
tf.Initialize(cf, a, b, delta);
b.ProcessTriangles(tf);
}
public override void SweptTest(CollisionFunctor cf, Part partA, Part partB, Vector3 delta)
{
var a = (SpherePart)partA;
var b = (MeshPart)partB;
var tf = SweptFunctor;
tf.Initialize(cf, a, b, delta);
b.ProcessTriangles(tf);
}
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 override void OverlapTest(CollisionFunctor cf, Part partA, Part partB)
{
var a = (CapsulePart)partA;
var b = (MeshPart)partB;
var tf = OverlapFunctor;
tf.Initialize(cf, a, b, Vector3.Zero);
b.ProcessTriangles(tf);
}
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 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 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 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 bool DoOverlapTest(CollisionFunctor cf, CapsulePart a, CapsulePart b, Vector3 offset)
{
Segment capa, capb = new Segment(b.World.P1, b.World.P2);
Vector3.Add(ref a.World.P1, ref offset, out capa.P1);
Vector3.Add(ref a.World.P2, ref offset, out capa.P2);
Vector3 pa, pb, normal, v;
float sa, sb, r2 = a.World.Radius + b.World.Radius;
r2 *= r2;
// find the closest point between the two capsules
Segment.ClosestPoints(ref capa, ref capb, out sa, out pa, out sb, out pb);
Vector3.Subtract(ref pa, ref pb, out normal);
if (normal.LengthSquared() - r2 >= Constants.Epsilon)
return false;
if (normal.LengthSquared() < Constants.Epsilon)
normal = Vector3.UnitZ;
normal.Normalize();
Vector3.Multiply(ref normal, -a.World.Radius, out v);
Vector3.Add(ref pa, ref v, out pa);
Vector3.Multiply(ref normal, b.World.Radius, out v);
Vector3.Add(ref pb, ref v, out pb);
Vector3.Subtract(ref pa, ref offset, out pa);
cf.WritePoint(ref pa, ref pb, ref normal);
// if the two capsules are nearly parallel, an additional support point provides stability
if (sa == 0f || sa == 1f)
{
pa = sa == 0f ? capa.P2 : capa.P1;
capb.ClosestPointTo(ref pa, out sa, out pb);
}
else if (sb == 0f || sb == 1f)
{
pb = sb == 0f ? capb.P2 : capb.P1;
capa.ClosestPointTo(ref pb, out sb, out pa);
}
else
return true;
float dist;
Vector3.DistanceSquared(ref pa, ref pb, out dist);
if (dist - r2 < Constants.Epsilon)
{
Vector3.Multiply(ref normal, -a.World.Radius, out v);
Vector3.Add(ref pa, ref v, out pa);
Vector3.Multiply(ref normal, b.World.Radius, out v);
Vector3.Add(ref pb, ref v, out pb);
Vector3.Subtract(ref pa, ref offset, out pa);
cf.WritePoint(ref pa, ref pb, ref normal);
}
return true;
}
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 override void SweptTest(CollisionFunctor cf, Part partA, Part partB, Vector3 delta)
{
var a = (CapsulePart)partA;
var b = (CapsulePart)partB;
Vector3 step, offset = Vector3.Zero;
int steps = (int)(delta.Length() / a.World.Radius * 0.9f);
Vector3.Divide(ref delta, steps, out step);
while (steps-- >= 0 && !DoOverlapTest(cf, a, b, offset))
{
Vector3.Add(ref offset, ref step, out offset);
}
}
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);
}
public override void SweptTest(CollisionFunctor cf, Part partA, Part partB, Vector3 delta)
{
var a = (PolyhedronPart)partA;
var b = (PlanePart)partB;
var plane = b.Plane;
float dx;
Vector3.Dot(ref plane.Normal, ref delta, out dx);
if (dx > 0f)
{
dx = 0f;
}
PolyhedronPlane.DoOverlapTest(cf, a, b, -dx);
}
public override void SweptTest(CollisionFunctor cf, Part partA, Part partB, Vector3 delta)
{
var a = (CapsulePart)partA;
var b = (PolyhedronPart)partB;
Vector3 step, offset = Vector3.Zero;
int steps = (int)(delta.Length() / a.World.Radius * 0.9f);
Vector3.Divide(ref delta, steps, out step);
while (steps-- >= 0 && !DoOverlapTest(cf, a, b, offset))
{
Vector3.Add(ref offset, ref step, out offset);
}
}
public void Initialize(CollisionFunctor cf, SpherePart a, MeshPart b)
{
_cf = cf;
_a = a;
_b = b;
_radius = a.World.Radius * b.TransformInverse.Scale;
_radiusSquared = _radius * _radius;
Depth = float.MaxValue;
Vector3.Transform(ref a.World.Center, ref b.TransformInverse.Combined, out _center);
BoundingBox.Minimum = BoundingBox.Maximum = _center;
var radius = new Vector3(_radius);
Vector3.Subtract(ref BoundingBox.Minimum, ref radius, out BoundingBox.Minimum);
Vector3.Add(ref BoundingBox.Maximum, ref radius, out BoundingBox.Maximum);
}
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 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);
}
}
}
public void Initialize(CollisionFunctor cf, SpherePart a, MeshPart b)
{
_cf = cf;
_a = a;
_b = b;
_radius = a.World.Radius * b.TransformInverse.Scale;
_radiusSquared = _radius * _radius;
Depth = float.MaxValue;
Vector3.Transform(ref a.World.Center, ref b.TransformInverse.Combined, out _center);
BoundingBox.Minimum = BoundingBox.Maximum = _center;
var radius = new Vector3(_radius);
Vector3.Subtract(ref BoundingBox.Minimum, ref radius, out BoundingBox.Minimum);
Vector3.Add(ref BoundingBox.Maximum, ref radius, out BoundingBox.Maximum);
}
public override void SweptTest(CollisionFunctor cf, Part partA, Part partB, Vector3 delta)
{
var a = (PolyhedronPart)partA;
var b = (PlanePart)partB;
var plane = b.Plane;
float dx;
Vector3.Dot(ref plane.Normal, ref delta, out dx);
if (dx > 0f)
{
dx = 0f;
}
PolyhedronPlane.DoOverlapTest(cf, a, b, -dx);
}
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);
}
}
}
}
public override void SweptTest(CollisionFunctor cf, Part partA, Part partB, Vector3 delta)
{
var a = (CapsulePart)partA;
var b = (MeshPart)partB;
Vector3 step, offset = Vector3.Zero;
int steps = (int)(delta.Length() / a.World.Radius * 0.9f);
Vector3.Divide(ref delta, steps, out step);
var tf = OverlapFunctor;
while (steps-- >= 0)
{
tf.Initialize(cf, a, b, offset);
b.ProcessTriangles(tf);
if (tf.HasCollision)
break;
Vector3.Add(ref offset, ref step, out offset);
}
}
public override void OverlapTest(CollisionFunctor cf, Part partA, Part partB)
{
var a = (SpherePart)partA;
var b = (MeshPart)partB;
var tf = OverlapFunctor;
tf.Initialize(cf, a, b);
b.ProcessTriangles(tf);
// if the sphere is inside the mesh, push it out via the normal of least depth
if (tf.Depth > 0f && tf.Depth < float.MaxValue)
{
Triangle tri;
Vector3 pb;
Triangle.Transform(ref tf.NearestTriangle, ref b.Transform, out tri);
tri.Center(out pb);
cf.WritePoint(ref a.World.Center, ref pb, ref tri.Normal);
}
}
public void Initialize(CollisionFunctor cf, CapsulePart a, MeshPart b, Vector3 offset)
{
_cf = cf;
_b = b;
_radius = a.World.Radius * b.TransformInverse.Scale;
_radiusSquared = _radius * _radius;
_offset = offset;
_hasCollision = false;
Vector3.Add(ref a.World.P1, ref _offset, out _cap.P1);
Vector3.Add(ref a.World.P2, ref _offset, out _cap.P2);
// calculate points and bounding box in body space
var radius = new Vector3(_radius);
Vector3.Transform(ref _cap.P1, ref b.TransformInverse.Combined, out _cap.P1);
Vector3.Transform(ref _cap.P2, ref b.TransformInverse.Combined, out _cap.P2);
AlignedBox.Fit(ref _cap.P1, ref _cap.P2, out BoundingBox);
Vector3.Subtract(ref BoundingBox.Minimum, ref radius, out BoundingBox.Minimum);
Vector3.Add(ref BoundingBox.Maximum, ref radius, out BoundingBox.Maximum);
}
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);
}
}
}
public void Initialize(CollisionFunctor cf, CapsulePart a, MeshPart b, Vector3 offset)
{
_cf = cf;
_b = b;
_radius = a.World.Radius * b.TransformInverse.Scale;
_radiusSquared = _radius * _radius;
_offset = offset;
_hasCollision = false;
Vector3.Add(ref a.World.P1, ref _offset, out _cap.P1);
Vector3.Add(ref a.World.P2, ref _offset, out _cap.P2);
// calculate points and bounding box in body space
var radius = new Vector3(_radius);
Vector3.Transform(ref _cap.P1, ref b.TransformInverse.Combined, out _cap.P1);
Vector3.Transform(ref _cap.P2, ref b.TransformInverse.Combined, out _cap.P2);
AlignedBox.Fit(ref _cap.P1, ref _cap.P2, out BoundingBox);
Vector3.Subtract(ref BoundingBox.Minimum, ref radius, out BoundingBox.Minimum);
Vector3.Add(ref BoundingBox.Maximum, ref radius, out BoundingBox.Maximum);
}
public void Initialize(CollisionFunctor cf, SpherePart a, MeshPart b, Vector3 delta)
{
_cf = cf;
_a = a;
_b = b;
_radius = a.World.Radius * b.TransformInverse.Scale;
_radiusSquared = _radius * _radius;
Vector3.Transform(ref a.World.Center, ref b.TransformInverse.Combined, out _path.P1);
Vector3.Transform(ref delta, ref b.TransformInverse.Orientation, out delta);
Vector3.Multiply(ref delta, b.TransformInverse.Scale, out delta);
Vector3.Add(ref _path.P1, ref delta, out _path.P2);
AlignedBox.Fit(ref _path.P1, ref _path.P2, out BoundingBox);
var radius = new Vector3(_radius);
Vector3.Subtract(ref BoundingBox.Minimum, ref radius, out BoundingBox.Minimum);
Vector3.Add(ref BoundingBox.Maximum, ref radius, out BoundingBox.Maximum);
}
public override void OverlapTest(CollisionFunctor cf, Part partA, Part partB)
{
var a = (SpherePart)partA;
var b = (SpherePart)partB;
float r2 = a.World.Radius + b.World.Radius;
r2 *= r2;
Vector3 pa, pb, normal;
Vector3.Subtract(ref a.World.Center, ref b.World.Center, out normal);
if (normal.LengthSquared() - r2 >= Constants.Epsilon)
return;
normal.Normalize();
Vector3.Multiply(ref normal, -a.World.Radius, out pa);
Vector3.Add(ref a.World.Center, ref pa, out pa);
Vector3.Multiply(ref normal, b.World.Radius, out pb);
Vector3.Add(ref b.World.Center, ref pb, out pb);
cf.WritePoint(ref pa, ref pb, ref normal);
}
public override void OverlapTest(CollisionFunctor cf, Part partA, Part partB)
{
var a = (SpherePart)partA;
var b = (MeshPart)partB;
var tf = OverlapFunctor;
tf.Initialize(cf, a, b);
b.ProcessTriangles(tf);
// if the sphere is inside the mesh, push it out via the normal of least depth
if (tf.Depth > 0f && tf.Depth < float.MaxValue)
{
Triangle tri;
Vector3 pb;
Triangle.Transform(ref tf.NearestTriangle, ref b.Transform, out tri);
tri.Center(out pb);
cf.WritePoint(ref a.World.Center, ref pb, ref tri.Normal);
}
}
/// <summary> /// When overridden in a derived class, executes the broad phase collision detection and provides /// results to the functor. /// </summary> /// <param name="cf">The functor that will receive the results of the collision detection.</param> public abstract void Execute(CollisionFunctor cf);
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;
}
/// <summary> /// When overridden in a derived class, performs a swept (or simulated-swept) test between two parts. /// </summary> /// <param name="cf">The functor to which all collisions are reported.</param> /// <param name="partA">The first part to test.</param> /// <param name="partB">The second part to test.</param> /// <param name="delta">The direction and magnitude of movement of partA, relative to partB.</param> public abstract void SweptTest(CollisionFunctor cf, Part partA, Part partB, Vector3 delta);
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 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 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 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);
}
}
}
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);
}
}
public override void OverlapTest(CollisionFunctor cf, Part partA, Part partB)
{
var a = (PolyhedronPart)partA;
var b = (PolyhedronPart)partB;
CollisionInfo cur = new CollisionInfo(),
final = new CollisionInfo { Depth = float.PositiveInfinity };
Vector3 v;
// 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 = b.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;
}
// axes: face normals of B
for (int i = 0; i < b.FaceCount; i++)
{
b.FaceNormal(i, out cur.Normal);
Vector3.Negate(ref cur.Normal, out cur.NormalNeg);
b.World(b.Face(i)[0], out v);
cur.FeatureB = new PolyhedronFeature(PolyhedronFeatureType.Face, i, 0f);
Vector3.Dot(ref cur.Normal, ref v, out cur.FeatureB.X);
cur.FeatureA = a.ExtremeVertex(ref cur.NormalNeg);
cur.FeatureA.X = -cur.FeatureA.X;
cur.Depth = cur.FeatureB.X - cur.FeatureA.X;
if (cur.Depth <= -Constants.Epsilon)
return;
else if (cur.Depth < final.Depth)
final = cur;
}
// axes: crossed edges from A and B
Vector3 centerA, centerB;
a.Center(out centerA);
b.Center(out centerB);
for (int i = 0; i < a.EdgeVectorCount; i++)
{
for (int j = 0; j < b.EdgeVectorCount; j++)
{
Vector3 eva, evb;
a.EdgeVector(i, out eva);
b.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);
// ignore this axis if it's close to the one we already have
float d;
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 = b.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 == PolyhedronFeatureType.Vertex)
{
final.FeatureB = b.ExtremeFeature(ref final.Normal, final.FeatureA.X);
}
CalculateContactPoints(cf, a, b, ref final);
}
public override void SweptTest(CollisionFunctor cf, Part partA, Part partB, Vector3 delta)
{
var a = (SpherePart)partA;
var b = (SpherePart)partB;
float d2 = a.World.Radius + b.World.Radius;
d2 *= d2;
Vector3 v, p = b.World.Center, q = a.World.Center;
Vector3.Add(ref q, ref delta, out v);
Vector3.Subtract(ref v, ref q, out v);
Vector3 pq;
Vector3.Subtract(ref q, ref p, out pq);
float ax, bx, cx;
Vector3.Dot(ref v, ref v, out ax);
Vector3.Dot(ref v, ref pq, out bx);
Vector3.Dot(ref pq, ref pq, out cx);
cx -= d2;
float n = (-bx - (float)Math.Sqrt(bx * bx - ax * cx)) / ax;
if (n <= 1f)
{
Vector3.Multiply(ref v, n, out q);
Vector3.Add(ref a.World.Center, ref q, out q);
Vector3 normal;
Vector3.Subtract(ref q, ref p, out normal);
normal.Normalize();
Vector3 pa, pb;
Vector3.Multiply(ref normal, -a.World.Radius, out pa);
Vector3.Add(ref a.World.Center, ref pa, out pa);
Vector3.Multiply(ref normal, b.World.Radius, out pb);
Vector3.Add(ref b.World.Center, ref pb, out pb);
cf.WritePoint(ref pa, ref pb, ref normal);
}
//Segment path;
//path.P1 = a.World.Center;
//Vector3.Add(ref path.P1, ref delta, out path.P2);
//Vector3.Subtract(ref path.P2, ref path.P1, out v);
//v.Normalize();
//q = path.P1;
//float scalar;
//Vector3 normal, pa, pb;
//path.ClosestPointTo(ref b.World.Center, out scalar, out pa);
//Vector3.Subtract(ref pa, ref b.World.Center, 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);
// Vector3.Multiply(ref normal, b.World.Radius, out pb);
// Vector3.Add(ref b.World.Center, ref pb, out pb);
// cf.WritePoint(ref pa, ref pb, ref normal);
//}
}
