public static bool SweptSphereTriangleIntersection(out Vector3 pt, out Vector3 N, out float depth,
BoundingSphere oldSphere, BoundingSphere newSphere, Triangle triangle,
float oldCentreDistToPlane, float newCentreDistToPlane,
EdgesToTest edgesToTest, CornersToTest cornersToTest)
{
int i;
Microsoft.Xna.Framework.Plane trianglePlane = triangle.Plane;
N = Vector3.Zero;
// Check against plane
if (!SweptSpherePlaneIntersection(out pt, out depth, oldSphere, newSphere, trianglePlane.Normal, oldCentreDistToPlane, newCentreDistToPlane))
{
return(false);
}
Vector3 v0 = triangle.GetPoint(0);
Vector3 v1 = triangle.GetPoint(1);
Vector3 v2 = triangle.GetPoint(2);
Vector3 e0 = v1 - v0;
Vector3 e1 = v2 - v1;
Vector3 e2 = v0 - v2;
// If the point is inside the triangle, this is a hit
bool allInside = true;
Vector3 outDir0 = Vector3.Cross(e0, trianglePlane.Normal);
if (Vector3.Dot(pt - v0, outDir0) > 0.0f)
{
allInside = false;
}
Vector3 outDir1 = Vector3.Cross(e1, trianglePlane.Normal);
if (Vector3.Dot(pt - v1, outDir1) > 0.0f)
{
allInside = false;
}
Vector3 outDir2 = Vector3.Cross(e2, trianglePlane.Normal);
if (Vector3.Dot(pt - v2, outDir2) > 0.0f)
{
allInside = false;
}
// Quick result?
if (allInside)
{
N = trianglePlane.Normal;
return(true);
}
// Now check against the edges
float bestT = float.MaxValue;
Vector3 Ks = newSphere.Center - oldSphere.Center;
float kss = Vector3.Dot(Ks, Ks);
float radius = newSphere.Radius;
float radiusSq = radius * radius;
for (i = 0; i < 3; ++i)
{
int mask = 1 << i;
if (!((mask != 0) & ((int)edgesToTest != 0))) // TODO: CHECK THIS
{
continue;
}
Vector3 Ke;
Vector3 vp;
switch (i)
{
case 0:
Ke = e0;
vp = v0;
break;
case 1:
Ke = e1;
vp = v1;
break;
case 2:
default:
Ke = e2;
vp = v2;
break;
}
Vector3 Kg = vp - oldSphere.Center;
float kee = Vector3.Dot(Ke, Ke);
if (System.Math.Abs(kee) < JiggleMath.Epsilon)
{
continue;
}
float kes = Vector3.Dot(Ke, Ks);
float kgs = Vector3.Dot(Kg, Ks);
float keg = Vector3.Dot(Ke, Kg);
float kgg = Vector3.Dot(Kg, Kg);
// a * t^2 + b * t + c = 0
float a = kee * kss - (kes * kes);
if (System.Math.Abs(a) < JiggleMath.Epsilon)
{
continue;
}
float b = 2.0f * (keg * kes - kee * kgs);
float c = kee * (kgg - radiusSq) - keg * keg;
float blah = b * b - 4.0f * a * c;
if (blah < 0.0f)
{
continue;
}
// solve for t - take minimum
float t = (-b - (float)System.Math.Sqrt(blah)) / (2.0f * a);
if (t < 0.0f || t > 1.0f)
{
continue;
}
if (t > bestT)
{
continue;
}
// now check where it hit on the edge
Vector3 Ct = oldSphere.Center + t * Ks;
float d = Vector3.Dot((Ct - vp), Ke) / kee;
if (d < 0.0f || d > 1.0f)
{
continue;
}
// wahay - got hit. Already checked that t < bestT
bestT = t;
pt = vp + d * Ke;
N = (Ct - pt);// .GetNormalisedSafe();
JiggleMath.NormalizeSafe(ref N);
// depth is already calculated
}
if (bestT <= 1.0f)
{
return(true);
}
// check the corners
bestT = float.MaxValue;
for (i = 0; i < 3; ++i)
{
int mask = 1 << i;
if (!((mask != 0) & (cornersToTest != 0))) // CHECK THIS
{
continue;
}
Vector3 vp;
switch (i)
{
case 0:
vp = v0;
break;
case 1:
vp = v1;
break;
case 2:
default:
vp = v2;
break;
}
Vector3 Kg = vp - oldSphere.Center;
float kgs = Vector3.Dot(Kg, Ks);
float kgg = Vector3.Dot(Kg, Kg);
float a = kss;
if (System.Math.Abs(a) < JiggleMath.Epsilon)
{
continue;
}
float b = -2.0f * kgs;
float c = kgg - radiusSq;
float blah = (b * b) - 4.0f * a * c;
if (blah < 0.0f)
{
continue;
}
// solve for t - take minimum
float t = (-b - (float)System.Math.Sqrt(blah)) / (2.0f * a);
if (t < 0.0f || t > 1.0f)
{
continue;
}
if (t > bestT)
{
continue;
}
bestT = t;
Vector3 Ct = oldSphere.Center + t * Ks;
N = (Ct - vp);//.GetNormalisedSafe();
JiggleMath.NormalizeSafe(ref N);
}
if (bestT <= 1.0f)
{
return(true);
}
return(false);
}
public static bool SweptSphereTriangleIntersection(out Vector3 pt, out Vector3 N, out float depth,
BoundingSphere oldSphere, BoundingSphere newSphere, Triangle triangle,
float oldCentreDistToPlane, float newCentreDistToPlane,
EdgesToTest edgesToTest, CornersToTest cornersToTest)
{
int i;
SlimDX.Plane trianglePlane = triangle.Plane;
N = Vector3.Zero;
// Check against plane
if (!SweptSpherePlaneIntersection(out pt,out depth, oldSphere, newSphere, trianglePlane.Normal, oldCentreDistToPlane, newCentreDistToPlane))
return false;
Vector3 v0 = triangle.GetPoint(0);
Vector3 v1 = triangle.GetPoint(1);
Vector3 v2 = triangle.GetPoint(2);
Vector3 e0 = v1 - v0;
Vector3 e1 = v2 - v1;
Vector3 e2 = v0 - v2;
// If the point is inside the triangle, this is a hit
bool allInside = true;
Vector3 outDir0 = Vector3.Cross(e0, trianglePlane.Normal);
if (Vector3.Dot(pt - v0, outDir0) > 0.0f)
{
allInside = false;
}
Vector3 outDir1 = Vector3.Cross(e1, trianglePlane.Normal);
if (Vector3.Dot(pt - v1, outDir1) > 0.0f)
{
allInside = false;
}
Vector3 outDir2 = Vector3.Cross(e2, trianglePlane.Normal);
if (Vector3.Dot(pt - v2, outDir2) > 0.0f)
{
allInside = false;
}
// Quick result?
if (allInside)
{
N = trianglePlane.Normal;
return true;
}
// Now check against the edges
float bestT = float.MaxValue;
Vector3 Ks = newSphere.Center - oldSphere.Center;
float kss = Vector3.Dot(Ks, Ks);
float radius = newSphere.Radius;
float radiusSq = radius * radius;
for (i = 0; i < 3; ++i)
{
int mask = 1 << i;
if (!((mask !=0) & ((int)edgesToTest != 0))) // TODO: CHECK THIS
continue;
Vector3 Ke;
Vector3 vp;
switch (i)
{
case 0:
Ke = e0;
vp = v0;
break;
case 1:
Ke = e1;
vp = v1;
break;
case 2:
default:
Ke = e2;
vp = v2;
break;
}
Vector3 Kg = vp - oldSphere.Center;
float kee = Vector3.Dot(Ke, Ke);
if (System.Math.Abs(kee) < JiggleMath.Epsilon)
continue;
float kes = Vector3.Dot(Ke, Ks);
float kgs = Vector3.Dot(Kg, Ks);
float keg = Vector3.Dot(Ke, Kg);
float kgg = Vector3.Dot(Kg, Kg);
// a * t^2 + b * t + c = 0
float a = kee * kss - (kes * kes);
if (System.Math.Abs(a) < JiggleMath.Epsilon)
continue;
float b = 2.0f * (keg * kes - kee * kgs);
float c = kee * (kgg - radiusSq) - keg * keg;
float blah = b*b - 4.0f * a * c;
if (blah < 0.0f)
continue;
// solve for t - take minimum
float t = (-b - (float)System.Math.Sqrt(blah)) / (2.0f * a);
if (t < 0.0f || t > 1.0f)
continue;
if (t > bestT)
continue;
// now check where it hit on the edge
Vector3 Ct = oldSphere.Center + t * Ks;
float d = Vector3.Dot((Ct - vp), Ke) / kee;
if (d < 0.0f || d > 1.0f)
continue;
// wahay - got hit. Already checked that t < bestT
bestT = t;
pt = vp + d * Ke;
N = (Ct - pt);// .GetNormalisedSafe();
JiggleMath.NormalizeSafe(ref N);
// depth is already calculated
}
if (bestT <= 1.0f)
return true;
// check the corners
bestT = float.MaxValue;
for (i = 0; i < 3; ++i)
{
int mask = 1 << i;
if (!((mask != 0) & (cornersToTest != 0))) // CHECK THIS
continue;
Vector3 vp;
switch (i)
{
case 0:
vp = v0;
break;
case 1:
vp = v1;
break;
case 2:
default:
vp = v2;
break;
}
Vector3 Kg = vp - oldSphere.Center;
float kgs = Vector3.Dot(Kg, Ks);
float kgg = Vector3.Dot(Kg, Kg);
float a = kss;
if (System.Math.Abs(a) < JiggleMath.Epsilon)
continue;
float b = -2.0f * kgs;
float c = kgg - radiusSq;
float blah = (b * b) - 4.0f * a * c;
if (blah < 0.0f)
continue;
// solve for t - take minimum
float t = (-b - (float) System.Math.Sqrt(blah)) / (2.0f * a);
if (t < 0.0f || t > 1.0f)
continue;
if (t > bestT)
continue;
bestT = t;
Vector3 Ct = oldSphere.Center + t * Ks;
N = (Ct - vp);//.GetNormalisedSafe();
JiggleMath.NormalizeSafe(ref N);
}
if (bestT <= 1.0f)
return true;
return false;
}
public static bool SweptSphereTriangleIntersection(out Vector3 pt, out Vector3 N, out float depth, BoundingSphere oldSphere, BoundingSphere newSphere, Triangle triangle, float oldCentreDistToPlane, float newCentreDistToPlane, EdgesToTest edgesToTest, CornersToTest cornersToTest)
{
int i;
var trianglePlane = triangle.Plane;
N = Vector3.Zero;
if (!SweptSpherePlaneIntersection(out pt, out depth, oldSphere, newSphere, trianglePlane.Normal, oldCentreDistToPlane, newCentreDistToPlane))
{
return(false);
}
var v0 = triangle.GetPoint(0);
var v1 = triangle.GetPoint(1);
var v2 = triangle.GetPoint(2);
var e0 = v1 - v0;
var e1 = v2 - v1;
var e2 = v0 - v2;
var allInside = true;
var outDir0 = Vector3.Cross(e0, trianglePlane.Normal);
if (Vector3.Dot(pt - v0, outDir0) > 0.0f)
{
allInside = false;
}
var outDir1 = Vector3.Cross(e1, trianglePlane.Normal);
if (Vector3.Dot(pt - v1, outDir1) > 0.0f)
{
allInside = false;
}
var outDir2 = Vector3.Cross(e2, trianglePlane.Normal);
if (Vector3.Dot(pt - v2, outDir2) > 0.0f)
{
allInside = false;
}
if (allInside)
{
N = trianglePlane.Normal;
return(true);
}
var bestT = float.MaxValue;
var Ks = newSphere.Center - oldSphere.Center;
var kss = Vector3.Dot(Ks, Ks);
var radius = newSphere.Radius;
var radiusSq = radius * radius;
for (i = 0; i < 3; ++i)
{
var mask = 1 << i;
if (!((mask != 0) & ((int)edgesToTest != 0)))
{
continue;
}
Vector3 Ke;
Vector3 vp;
switch (i)
{
case 0:
Ke = e0;
vp = v0;
break;
case 1:
Ke = e1;
vp = v1;
break;
case 2:
default:
Ke = e2;
vp = v2;
break;
}
var Kg = vp - oldSphere.Center;
var kee = Vector3.Dot(Ke, Ke);
if (System.Math.Abs(kee) < JiggleMath.Epsilon)
{
continue;
}
var kes = Vector3.Dot(Ke, Ks);
var kgs = Vector3.Dot(Kg, Ks);
var keg = Vector3.Dot(Ke, Kg);
var kgg = Vector3.Dot(Kg, Kg);
var a = kee * kss - kes * kes;
if (System.Math.Abs(a) < JiggleMath.Epsilon)
{
continue;
}
var b = 2.0f * (keg * kes - kee * kgs);
var c = kee * (kgg - radiusSq) - keg * keg;
var blah = b * b - 4.0f * a * c;
if (blah < 0.0f)
{
continue;
}
var t = (-b - (float)System.Math.Sqrt(blah)) / (2.0f * a);
if (t < 0.0f || t > 1.0f)
{
continue;
}
if (t > bestT)
{
continue;
}
var Ct = oldSphere.Center + t * Ks;
var d = Vector3.Dot(Ct - vp, Ke) / kee;
if (d < 0.0f || d > 1.0f)
{
continue;
}
bestT = t;
pt = vp + d * Ke;
N = Ct - pt;
JiggleMath.NormalizeSafe(ref N);
}
if (bestT <= 1.0f)
{
return(true);
}
bestT = float.MaxValue;
for (i = 0; i < 3; ++i)
{
var mask = 1 << i;
if (!((mask != 0) & (cornersToTest != 0)))
{
continue;
}
Vector3 vp;
switch (i)
{
case 0:
vp = v0;
break;
case 1:
vp = v1;
break;
case 2:
default:
vp = v2;
break;
}
var Kg = vp - oldSphere.Center;
var kgs = Vector3.Dot(Kg, Ks);
var kgg = Vector3.Dot(Kg, Kg);
var a = kss;
if (System.Math.Abs(a) < JiggleMath.Epsilon)
{
continue;
}
var b = -2.0f * kgs;
var c = kgg - radiusSq;
var blah = b * b - 4.0f * a * c;
if (blah < 0.0f)
{
continue;
}
var t = (-b - (float)System.Math.Sqrt(blah)) / (2.0f * a);
if (t < 0.0f || t > 1.0f)
{
continue;
}
if (t > bestT)
{
continue;
}
bestT = t;
var Ct = oldSphere.Center + t * Ks;
N = Ct - vp;
JiggleMath.NormalizeSafe(ref N);
}
if (bestT <= 1.0f)
{
return(true);
}
return(false);
}
