// Missing: SweptSpherePlaneOverlap
#region SegmentAABoxOverlap
/// <summary>
/// Indicates if a segment overlaps an AABox
/// </summary>
/// <param name="seg"></param>
/// <param name="AABox"></param>
/// <returns>bool</returns>
public static bool SegmentAABoxOverlap(Segment seg, AABox AABox)
{
Vector3 p0 = seg.Origin;
Vector3 p1 = seg.GetEnd();
float[] faceOffsets = new float[2];
// The AABox faces are aligned with the world directions. Loop
// over the 3 directions and do the two tests.
for (int iDir = 0; iDir < 3; iDir++)
{
int jDir = (iDir + 1) % 3;
int kDir = (iDir + 2) % 3;
// one plane goes through the origin, one is offset
faceOffsets[0] = JiggleUnsafe.Get(AABox.MinPos, iDir);
faceOffsets[1] = JiggleUnsafe.Get(AABox.MaxPos, iDir);
for (int iFace = 0; iFace < 2; iFace++)
{
// distance of each point from to the face plane
float dist0 = JiggleUnsafe.Get(ref p0, iDir) - faceOffsets[iFace];
float dist1 = JiggleUnsafe.Get(ref p1, iDir) - faceOffsets[iFace];
float frac = -1.0f;
if (dist0 * dist1 < -JiggleMath.Epsilon)
{
frac = -dist0 / (dist1 - dist0);
}
else if (System.Math.Abs(dist0) < JiggleMath.Epsilon)
{
frac = 0.0f;
}
else if (System.Math.Abs(dist1) < JiggleMath.Epsilon)
{
frac = 1.0f;
}
if (frac >= 0.0f)
{
//Assert(frac <= 1.0f);
Vector3 pt = seg.GetPoint(frac);
// check the point is within the face rectangle
if ((JiggleUnsafe.Get(ref pt, jDir) > JiggleUnsafe.Get(AABox.MinPos, jDir) - JiggleMath.Epsilon) &&
(JiggleUnsafe.Get(ref pt, jDir) < JiggleUnsafe.Get(AABox.MaxPos, jDir) + JiggleMath.Epsilon) &&
(JiggleUnsafe.Get(ref pt, kDir) > JiggleUnsafe.Get(AABox.MinPos, kDir) - JiggleMath.Epsilon) &&
(JiggleUnsafe.Get(ref pt, kDir) < JiggleUnsafe.Get(AABox.MaxPos, kDir) + JiggleMath.Epsilon))
{
return(true);
}
}
}
}
return(false);
}
public static bool SegmentAABoxOverlap(Segment seg, AABox AABox)
{
var p0 = seg.Origin;
var p1 = seg.GetEnd();
var faceOffsets = new float[2];
for (var iDir = 0; iDir < 3; iDir++)
{
var jDir = (iDir + 1) % 3;
var kDir = (iDir + 2) % 3;
faceOffsets[0] = JiggleUnsafe.Get(AABox.MinPos, iDir);
faceOffsets[1] = JiggleUnsafe.Get(AABox.MaxPos, iDir);
for (var iFace = 0; iFace < 2; iFace++)
{
var dist0 = JiggleUnsafe.Get(ref p0, iDir) - faceOffsets[iFace];
var dist1 = JiggleUnsafe.Get(ref p1, iDir) - faceOffsets[iFace];
var frac = -1.0f;
if (dist0 * dist1 < -JiggleMath.Epsilon)
{
frac = -dist0 / (dist1 - dist0);
}
else if (System.Math.Abs(dist0) < JiggleMath.Epsilon)
{
frac = 0.0f;
}
else if (System.Math.Abs(dist1) < JiggleMath.Epsilon)
{
frac = 1.0f;
}
if (frac >= 0.0f)
{
var pt = seg.GetPoint(frac);
if (JiggleUnsafe.Get(ref pt, jDir) > JiggleUnsafe.Get(AABox.MinPos, jDir) - JiggleMath.Epsilon && JiggleUnsafe.Get(ref pt, jDir) < JiggleUnsafe.Get(AABox.MaxPos, jDir) + JiggleMath.Epsilon && JiggleUnsafe.Get(ref pt, kDir) > JiggleUnsafe.Get(AABox.MinPos, kDir) - JiggleMath.Epsilon && JiggleUnsafe.Get(ref pt, kDir) < JiggleUnsafe.Get(AABox.MaxPos, kDir) + JiggleMath.Epsilon)
{
return(true);
}
}
}
}
return(false);
}
/// <summary>
/// Indicates if a segment overlaps an AABox
/// </summary>
/// <param name="seg"></param>
/// <param name="AABox"></param>
/// <returns>bool</returns>
public static bool SegmentAABoxOverlap(Segment seg, AABox AABox)
{
Vector3 p0 = seg.Origin;
Vector3 p1 = seg.GetEnd();
float[] faceOffsets = new float[2];
// The AABox faces are aligned with the world directions. Loop
// over the 3 directions and do the two tests.
for (int iDir = 0; iDir < 3; iDir++)
{
int jDir = (iDir + 1) % 3;
int kDir = (iDir + 2) % 3;
// one plane goes through the origin, one is offset
faceOffsets[0] = JiggleUnsafe.Get(AABox.MinPos, iDir);
faceOffsets[1] = JiggleUnsafe.Get(AABox.MaxPos, iDir);
for (int iFace = 0; iFace < 2; iFace++)
{
// distance of each point from to the face plane
float dist0 = JiggleUnsafe.Get(ref p0, iDir) - faceOffsets[iFace];
float dist1 = JiggleUnsafe.Get(ref p1, iDir) - faceOffsets[iFace];
float frac = -1.0f;
if (dist0 * dist1 < -JiggleMath.Epsilon)
frac = -dist0 / (dist1 - dist0);
else if (System.Math.Abs(dist0) < JiggleMath.Epsilon)
frac = 0.0f;
else if (System.Math.Abs(dist1) < JiggleMath.Epsilon)
frac = 1.0f;
if (frac >= 0.0f)
{
//Assert(frac <= 1.0f);
Vector3 pt = seg.GetPoint(frac);
// check the point is within the face rectangle
if ((JiggleUnsafe.Get(ref pt, jDir) > JiggleUnsafe.Get(AABox.MinPos, jDir) - JiggleMath.Epsilon) &&
(JiggleUnsafe.Get(ref pt, jDir) < JiggleUnsafe.Get(AABox.MaxPos, jDir) + JiggleMath.Epsilon) &&
(JiggleUnsafe.Get(ref pt, kDir) > JiggleUnsafe.Get(AABox.MinPos, kDir) - JiggleMath.Epsilon) &&
(JiggleUnsafe.Get(ref pt, kDir) < JiggleUnsafe.Get(AABox.MaxPos, kDir) + JiggleMath.Epsilon))
{
return true;
}
}
}
}
return false;
}
public override bool SegmentIntersect(out float frac, out Vector3 pos, out Vector3 normal, Segment seg)
{
frac = 0;
pos = Vector3.Zero;
normal = Vector3.Up;
if (seg.Delta.Y > -JiggleMath.Epsilon)
{
return(false);
}
Vector3 normalStart;
float heightStart;
GetHeightAndNormal(out heightStart, out normalStart, seg.Origin);
if (heightStart < 0.0f)
{
return(false);
}
Vector3 normalEnd;
float heightEnd;
Vector3 end = seg.GetEnd();
GetHeightAndNormal(out heightEnd, out normalEnd, end);
if (heightEnd > 0.0f)
{
return(false);
}
// start is above, end is below...
float depthEnd = -heightEnd;
// normal is the weighted mean of these...
float weightStart = 1.0f / (JiggleMath.Epsilon + heightStart);
float weightEnd = 1.0f / (JiggleMath.Epsilon + depthEnd);
normal = (normalStart * weightStart + normalEnd * weightEnd) /
(weightStart + weightEnd);
frac = heightStart / (heightStart + depthEnd + JiggleMath.Epsilon);
pos = seg.GetPoint(frac);
return(true);
}
public override bool SegmentIntersect(out float fracOut, out CollisionSkin skinOut, out Microsoft.Xna.Framework.Vector3 posOut, out Microsoft.Xna.Framework.Vector3 normalOut, JigLibX.Geometry.Segment seg, CollisionSkinPredicate1 collisionPredicate)
{
fracOut = float.MaxValue;
skinOut = null;
posOut = normalOut = Vector3.Zero;
Vector3 min = seg.GetPoint(0);
Vector3 tmp = seg.GetEnd();
Vector3 max;
Vector3.Max(ref min, ref tmp, out max);
Vector3.Min(ref min, ref tmp, out min);
BoundingBox box = new BoundingBox(min, max);
float frac;
Vector3 pos;
Vector3 normal;
active_.Clear();
Extract(min, max, active_);
int nActive = active_.Count;
for (int i = 0; i != nActive; ++i)
{
CollisionSkin skin = active_[i];
if (collisionPredicate == null || collisionPredicate.ConsiderSkin(skin))
{
if (BoundingBoxHelper.OverlapTest(ref box, ref skin.WorldBoundingBox))
{
if (skin.SegmentIntersect(out frac, out pos, out normal, seg))
{
if (frac >= 0 && frac < fracOut)
{
fracOut = frac;
skinOut = skin;
posOut = pos;
normalOut = normal;
}
}
}
}
}
return(fracOut <= 1);
}
/// <summary>
/// SegmentIntersect
/// </summary>
/// <param name="frac"></param>
/// <param name="pos"></param>
/// <param name="normal"></param>
/// <param name="seg"></param>
/// <returns>bool</returns>
public override bool SegmentIntersect(out float frac, out Vector3 pos, out Vector3 normal,Segment seg)
{
frac = 0;
pos = Vector3.Zero;
normal = Vector3.Up;
//if (seg.Delta.Y > -JiggleMath.Epsilon )
// return false;
Vector3 normalStart;
float heightStart;
GetHeightAndNormal(out heightStart, out normalStart,seg.Origin);
if (heightStart < 0.0f)
return false;
Vector3 normalEnd;
float heightEnd;
Vector3 end = seg.GetEnd();
GetHeightAndNormal(out heightEnd, out normalEnd,end);
if (heightEnd > 0.0f)
return false;
// start is above, end is below...
float depthEnd = -heightEnd;
// normal is the weighted mean of these...
float weightStart = 1.0f / (JiggleMath.Epsilon + heightStart);
float weightEnd = 1.0f / (JiggleMath.Epsilon + depthEnd);
normal = (normalStart * weightStart + normalEnd * weightEnd) /
(weightStart + weightEnd);
frac = heightStart / (heightStart + depthEnd + JiggleMath.Epsilon);
pos = seg.GetPoint(frac);
return true;
}
/// <summary>
/// SegmentTriangleDistanceSq
/// </summary>
/// <param name="segT"></param>
/// <param name="triT0"></param>
/// <param name="triT1"></param>
/// <param name="seg"></param>
/// <param name="triangle"></param>
/// <returns>float</returns>
public static float SegmentTriangleDistanceSq(out float segT, out float triT0, out float triT1, Segment seg, Triangle triangle)
{
// compare segment to all three edges of the triangle
float distSq = float.MaxValue;
if (Intersection.SegmentTriangleIntersection(out segT, out triT0, out triT1, seg, triangle))
{
segT = triT0 = triT1 = 0.0f;
return 0.0f;
}
float s, t, u;
float distEdgeSq;
distEdgeSq = SegmentSegmentDistanceSq(out s, out t, seg, new Segment(triangle.Origin, triangle.Edge0));
if (distEdgeSq < distSq)
{
distSq = distEdgeSq;
segT = s;
triT0 = t;
triT1 = 0.0f;
}
distEdgeSq = SegmentSegmentDistanceSq(out s, out t, seg, new Segment(triangle.Origin, triangle.Edge1));
if (distEdgeSq < distSq)
{
distSq = distEdgeSq;
segT = s;
triT0 = 0.0f;
triT1 = t;
}
distEdgeSq = SegmentSegmentDistanceSq(out s, out t, seg, new Segment(triangle.Origin + triangle.Edge0, triangle.Edge2));
if (distEdgeSq < distSq)
{
distSq = distEdgeSq;
segT = s;
triT0 = 1.0f - t;
triT1 = t;
}
// compare segment end points to triangle interior
float startTriSq = PointTriangleDistanceSq(out t, out u, seg.Origin, triangle);
if (startTriSq < distSq)
{
distSq = startTriSq;
segT = 0.0f;
triT0 = t;
triT1 = u;
}
float endTriSq = PointTriangleDistanceSq(out t, out u, seg.GetEnd(), triangle);
if (endTriSq < distSq)
{
distSq = endTriSq;
segT = 1.0f;
triT0 = t;
triT1 = u;
}
return distSq;
}
/// <summary>
/// Every skin must support a ray/segment intersection test -
/// operates on the new value of the primitives
/// </summary>
/// <param name="frac"></param>
/// <param name="pos"></param>
/// <param name="normal"></param>
/// <param name="seg"></param>
/// <returns>bool</returns>
public bool SegmentIntersect(out float frac, out Vector3 pos, out Vector3 normal, Segment seg)
{
Vector3 segEnd = seg.GetEnd();
frac = float.MaxValue;
float thisSegLenRelToOrig = 1.0f;
Segment segCopy = seg;
pos = normal = Vector3.Zero;
for (int prim = primitivesNewWorld.Count; prim-- != 0; )
{
float thisFrac;
Vector3 newPosition = pos;
if (primitivesNewWorld[prim].SegmentIntersect(out thisFrac, out newPosition, out normal, segCopy))
{
pos = newPosition;
frac = thisFrac * thisSegLenRelToOrig;
segCopy.Delta *= thisFrac;
thisSegLenRelToOrig *= frac;
}
}
//System.Diagnostics.Debug.WriteLineIf(frac <= 1.0f, pos);
return (frac <= 1.0f);
}
static public void AddSegment(Segment seg, ref BoundingBox bb)
{
AddPoint(seg.Origin, ref bb);
AddPoint(seg.GetEnd(), ref bb);
}
/// <summary>
/// AddSegment
/// </summary>
/// <param name="seg"></param>
/// <param name="bb"></param>
public static void AddSegment(Segment seg, ref BoundingBox bb)
{
AddPoint(seg.Origin,ref bb);
AddPoint(seg.GetEnd(), ref bb);
}
/// <summary>
/// SegmentIntersect
/// </summary>
/// <param name="frac"></param>
/// <param name="pos"></param>
/// <param name="normal"></param>
/// <param name="seg"></param>
/// <returns>bool</returns>
public override bool SegmentIntersect(out float frac, out Vector3 pos, out Vector3 normal,Segment seg)
{
/* Bug with SegmentIntersect, if the segment >= 1 square, then the code fails.
* We need to start at seg.Origin, and move to seg.Origin + (dx,dz) until one is below and one is above 0)
*/
frac = 0;
pos = Vector3.Zero;
normal = Vector3.Up;
Vector3 normalStart = Vector3.Zero;
float heightStart = 0.0f;
Vector3 normalEnd;
float heightEnd;
Vector3 segPos = Vector3.Zero;
Vector3 segPosEnd = seg.Origin;
Vector3 segEnd = seg.GetEnd();
Vector3 segDirection = seg.Delta;
segDirection.Y = 0;
JiggleMath.NormalizeSafe(ref segDirection);
segDirection.Y = seg.Delta.Y * (segDirection.X / seg.Delta.X); // Scale the Y to be make segDirection a scaler of segDelta, but normalized in the XY plane
float increment = JiggleMath.Min(dx, dz, float.MaxValue); // Move by the smaller of the two dx/dz values so we dont "jump" over a point ... this could be done better
GetHeightAndNormal(out heightEnd, out normalEnd, seg.Origin);
bool done = false;
while (!done)
{
segPos = segPosEnd;
normalStart = normalEnd;
heightStart = heightEnd;
segPosEnd += segDirection * increment;
if ((((segPos.X - segEnd.X) > 0.0f) != ((segPosEnd.X - segEnd.X) > 0.0f)) // We have moved onto the other side of the seg end in X
|| (((segPos.Z - segEnd.Z) > 0.0f) != ((segPosEnd.Z - segEnd.Z) > 0.0f))) // We have moved onto the other side of the seg end in Z
{
segPosEnd = segEnd;
}
GetHeightAndNormal(out heightEnd, out normalEnd, segPosEnd);
if ((heightStart >= 0.0f && heightEnd <= 0.0f) // Passes down through
|| (heightStart <= 0.0f && heightEnd >= 0.0f)) // Passes up through
{
done = true;
//We intersect here
}
if (segEnd.X == segPosEnd.X && segEnd.Z == segPosEnd.Z && !done) // Checking for X,Z handles cases of segments only in Y direction
{
// No intersection found and we are at the end of the ray
return false;
}
}
/*Vector3 normalStart;
float heightStart;
GetHeightAndNormal(out heightStart, out normalStart,seg.Origin);
if (heightStart < 0.0f)
return false;
Vector3 normalEnd;
float heightEnd;
Vector3 end = seg.GetEnd();
GetHeightAndNormal(out heightEnd, out normalEnd,end);
if (heightEnd > 0.0f)
return false;*/
//Rest of calculations are % based, do not need negatives
heightStart = System.Math.Abs(heightStart);
heightEnd = System.Math.Abs(heightEnd);
// normal is the weighted mean of these...
float weightStart = 1.0f / (JiggleMath.Epsilon + heightStart);
float weightEnd = 1.0f / (JiggleMath.Epsilon + heightEnd);
normal = (normalStart * weightStart + normalEnd * weightEnd) /
(weightStart + weightEnd);
frac = heightStart / (heightStart + heightEnd + JiggleMath.Epsilon);
//pos = seg.GetPoint(frac);
pos = segPos + (segPosEnd - segPos) * frac;
return true;
}
/// <summary>
/// SegmentIntersect
/// </summary>
/// <param name="frac"></param>
/// <param name="pos"></param>
/// <param name="normal"></param>
/// <param name="seg"></param>
/// <returns>bool</returns>
public override bool SegmentIntersect(out float frac, out Vector3 pos, out Vector3 normal, Segment seg)
{
/* Bug with SegmentIntersect, if the segment >= 1 square, then the code fails.
* We need to start at seg.Origin, and move to seg.Origin + (dx,dz) until one is below and one is above 0)
*/
frac = 0;
pos = Vector3.Zero;
normal = Vector3.Up;
Vector3 normalStart = Vector3.Zero;
float heightStart = 0.0f;
Vector3 normalEnd;
float heightEnd;
Vector3 segPos = Vector3.Zero;
Vector3 segPosEnd = seg.Origin;
Vector3 segEnd = seg.GetEnd();
Vector3 segDirection = seg.Delta;
segDirection.Y = 0;
JiggleMath.NormalizeSafe(ref segDirection);
segDirection.Y = seg.Delta.Y * (segDirection.X / seg.Delta.X); // Scale the Y to be make segDirection a scaler of segDelta, but normalized in the XY plane
float increment = JiggleMath.Min(dx, dz, float.MaxValue); // Move by the smaller of the two dx/dz values so we dont "jump" over a point ... this could be done better
GetHeightAndNormal(out heightEnd, out normalEnd, seg.Origin);
bool done = false;
while (!done)
{
segPos = segPosEnd;
normalStart = normalEnd;
heightStart = heightEnd;
segPosEnd += segDirection * increment;
if ((((segPos.X - segEnd.X) > 0.0f) != ((segPosEnd.X - segEnd.X) > 0.0f)) || // We have moved onto the other side of the seg end in X
(((segPos.Z - segEnd.Z) > 0.0f) != ((segPosEnd.Z - segEnd.Z) > 0.0f))) // We have moved onto the other side of the seg end in Z
{
segPosEnd = segEnd;
}
GetHeightAndNormal(out heightEnd, out normalEnd, segPosEnd);
if ((heightStart >= 0.0f && heightEnd <= 0.0f) || // Passes down through
(heightStart <= 0.0f && heightEnd >= 0.0f)) // Passes up through
{
done = true;
//We intersect here
}
if (segEnd.X == segPosEnd.X && segEnd.Z == segPosEnd.Z && !done) // Checking for X,Z handles cases of segments only in Y direction
{
// No intersection found and we are at the end of the ray
return(false);
}
}
/*Vector3 normalStart;
* float heightStart;
*
* GetHeightAndNormal(out heightStart, out normalStart,seg.Origin);
*
* if (heightStart < 0.0f)
* return false;
*
* Vector3 normalEnd;
* float heightEnd;
* Vector3 end = seg.GetEnd();
* GetHeightAndNormal(out heightEnd, out normalEnd,end);
*
* if (heightEnd > 0.0f)
* return false;*/
//Rest of calculations are % based, do not need negatives
heightStart = System.Math.Abs(heightStart);
heightEnd = System.Math.Abs(heightEnd);
// normal is the weighted mean of these...
float weightStart = 1.0f / (JiggleMath.Epsilon + heightStart);
float weightEnd = 1.0f / (JiggleMath.Epsilon + heightEnd);
normal = (normalStart * weightStart + normalEnd * weightEnd) /
(weightStart + weightEnd);
frac = heightStart / (heightStart + heightEnd + JiggleMath.Epsilon);
//pos = seg.GetPoint(frac);
pos = segPos + (segPosEnd - segPos) * frac;
return(true);
}