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