/// <summary>
/// SweptSpherePlaneIntersection
/// </summary>
/// <param name="pt"></param>
/// <param name="finalPenetration"></param>
/// <param name="oldSphere"></param>
/// <param name="newSphere"></param>
/// <param name="planeNormal"></param>
/// <param name="pOldDistToPlane"></param>
/// <param name="pNewDistToPlane"></param>
/// <returns>bool</returns>
public static bool SweptSpherePlaneIntersection(out Vector3 pt, out float finalPenetration, BoundingSphere oldSphere, BoundingSphere newSphere,
Vector3 planeNormal, float pOldDistToPlane, float pNewDistToPlane)
{
float oldDistToPlane = pOldDistToPlane;
float newDistToPlane = pNewDistToPlane;
float radius = oldSphere.Radius;
pt = Vector3.Zero;
finalPenetration = 0.0f;
if (newDistToPlane >= oldDistToPlane)
{
return(false);
}
if (newDistToPlane > radius)
{
return(false);
}
// intersect with plane
float t = (newDistToPlane - radius) / (newDistToPlane - oldDistToPlane);
if (t < 0.0f || t > 1.0f)
{
return(false);
}
pt = oldSphere.Center + t * (newSphere.Center - oldSphere.Center) - OpenTKHelper.Min(radius, oldDistToPlane) * planeNormal;
finalPenetration = radius - newDistToPlane;
return(true);
}
/// <summary>
/// Disjoint Returns true if disjoint. Returns false if intersecting,
/// and sets the overlap depth, d scaled by the axis length.
/// </summary>
/// <param name="d"></param>
/// <param name="axis"></param>
/// <param name="box0"></param>
/// <param name="box1"></param>
/// <param name="collTolerance"></param>
/// <returns>bool</returns>
private static bool Disjoint(out float d, ref Vector3 axis, Box box0, Box box1, float collTolerance)
{
float min0, max0, min1, max1;
// BEN-OPTIMISATION: Used new GetSpan() with referenced axis.
box0.GetSpan(out min0, out max0, ref axis);
box1.GetSpan(out min1, out max1, ref axis);
if (min0 > (max1 + collTolerance + JiggleMath.Epsilon) ||
min1 > (max0 + collTolerance + JiggleMath.Epsilon))
{
d = 0.0f;
return(true);
}
if ((max0 > max1) && (min1 > min0))
{
// box1 is inside - choose the min dist to move it out
d = OpenTKHelper.Min(max0 - min1, max1 - min0);
}
else if ((max1 > max0) && (min0 > min1))
{
// box0 is inside - choose the min dist to move it out
d = OpenTKHelper.Min(max1 - min0, max0 - min1);
}
else
{
// boxes overlap
d = (max0 < max1) ? max0 : max1;
d -= (min0 > min1) ? min0 : min1;
}
return(false);
}
/// <summary>
/// SegmentCapsuleIntersection
/// </summary>
/// <param name="tS"></param>
/// <param name="seg"></param>
/// <param name="capsule"></param>
/// <returns>bool</returns>
public static bool SegmentCapsuleIntersection(out float tS, Segment seg, Capsule capsule)
{
float bestFrac = float.MaxValue;
tS = 0;
// do the main sides
float sideFrac = float.MaxValue;
if (!SegmentInfiniteCylinderIntersection(out sideFrac, seg,
new Segment(capsule.Position, capsule.Orientation.Backward()),
capsule.Radius))
{
return(false); // check this
}
// only keep this if the side intersection point is within the capsule segment ends
Vector3 sidePos = seg.GetPoint(sideFrac);
if (Vector3.Dot(sidePos - capsule.Position, capsule.Orientation.Backward()) < 0.0f)
{
sideFrac = float.MaxValue;
}
else if (Vector3.Dot(sidePos - capsule.GetEnd(), capsule.Orientation.Backward()) > 0.0f)
{
sideFrac = float.MaxValue;
}
// do the two ends
float originFrac = float.MaxValue;
SegmentSphereIntersection(out originFrac, seg, new Sphere(capsule.Position, capsule.Radius));
float endFrac = float.MaxValue; // Check this!
SegmentSphereIntersection(out endFrac, seg, new Sphere(capsule.GetEnd(), capsule.Radius));
bestFrac = OpenTKHelper.Min(sideFrac, originFrac);
bestFrac = OpenTKHelper.Min(bestFrac, endFrac);
if (bestFrac <= 1.0f)
{
tS = bestFrac;
return(true);
}
return(false);
}
/// <summary>
/// CollDetect
/// </summary>
/// <param name="infoOrig"></param>
/// <param name="collTolerance"></param>
/// <param name="collisionFunctor"></param>
public override void CollDetect(CollDetectInfo infoOrig, float collTolerance, CollisionFunctor collisionFunctor)
{
// get the skins in the order that we're expectiing
CollDetectInfo info = infoOrig;
if (info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0).Type == this.Type1)
{
CollisionSkin skinSwap = info.Skin0;
info.Skin0 = info.Skin1;
info.Skin1 = skinSwap;
int primSwap = info.IndexPrim0;
info.IndexPrim0 = info.IndexPrim1;
info.IndexPrim1 = primSwap;
}
Vector3 body0Pos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero;
Vector3 body1Pos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero;
// todo - proper swept test
Capsule oldCapsule = info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0) as Capsule;
Capsule newCapsule = info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0) as Capsule;
Segment oldSeg = new Segment(oldCapsule.Position, oldCapsule.Length * oldCapsule.Orientation.Backward());
Segment newSeg = new Segment(newCapsule.Position, newCapsule.Length * newCapsule.Orientation.Backward());
float radius = oldCapsule.Radius;
Box oldBox = info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1) as Box;
Box newBox = info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1) as Box;
float oldSegT;
float oldBoxT0, oldBoxT1, oldBoxT2;
float oldDistSq = Distance.SegmentBoxDistanceSq(out oldSegT, out oldBoxT0, out oldBoxT1, out oldBoxT2, oldSeg, oldBox);
float newSegT;
float newBoxT0, newBoxT1, newBoxT2;
float newDistSq = Distance.SegmentBoxDistanceSq(out newSegT, out newBoxT0, out newBoxT1, out newBoxT2, newSeg, newBox);
if (OpenTKHelper.Min(oldDistSq, newDistSq) < ((radius + collTolerance) * (radius + collTolerance)))
{
Vector3 segPos = oldSeg.GetPoint(oldSegT);
Vector3 boxPos = oldBox.GetCentre() + oldBoxT0 * oldBox.Orientation.Right() +
oldBoxT1 * oldBox.Orientation.Up() + oldBoxT2 * oldBox.Orientation.Backward();
float dist = (float)System.Math.Sqrt((float)oldDistSq);
float depth = radius - dist;
Vector3 dir;
if (dist > JiggleMath.Epsilon)
{
dir = segPos - boxPos;
JiggleMath.NormalizeSafe(ref dir);
}
else if ((segPos - oldBox.GetCentre()).LengthSquared > JiggleMath.Epsilon)
{
dir = segPos - oldBox.GetCentre();
JiggleMath.NormalizeSafe(ref dir);
}
else
{
// todo - make this not random
dir = Vector3.Transform(Vector3Extensions.Backward, Matrix4.CreateFromAxisAngle(Vector3Extensions.Up, OpenTKHelper.ToRadians(random.Next(360))));
}
unsafe
{
SmallCollPointInfo collInfo = new SmallCollPointInfo(boxPos - body0Pos, boxPos - body1Pos, depth);
collisionFunctor.CollisionNotify(ref info, ref dir, &collInfo, 1);
}
}
}
/// <summary>
/// CollDetect
/// </summary>
/// <param name="info"></param>
/// <param name="collTolerance"></param>
/// <param name="collisionFunctor"></param>
public override void CollDetect(CollDetectInfo info, float collTolerance, CollisionFunctor collisionFunctor)
{
if (info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0).Type == this.Type1)
{
CollisionSkin skinSwap = info.Skin0;
info.Skin0 = info.Skin1;
info.Skin1 = skinSwap;
int primSwap = info.IndexPrim0;
info.IndexPrim0 = info.IndexPrim1;
info.IndexPrim1 = primSwap;
}
Vector3 body0Pos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero;
Vector3 body1Pos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero;
// todo - proper swept test
Capsule oldCapsule = (Capsule)info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0);
Capsule newCapsule = (Capsule)info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0);
JigLibX.Geometry.Plane oldPlane = (JigLibX.Geometry.Plane)info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1);
JigLibX.Geometry.Plane newPlane = (JigLibX.Geometry.Plane)info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1);
Matrix4 newPlaneInvTransform = newPlane.InverseTransformMatrix;
Matrix4 oldPlaneInvTransform = oldPlane.InverseTransformMatrix;
unsafe
{
#if USE_STACKALLOC
SmallCollPointInfo *collPts = stackalloc SmallCollPointInfo[MaxLocalStackSCPI];
#else
SmallCollPointInfo[] collPtArray = SCPIStackAlloc();
fixed(SmallCollPointInfo *collPts = collPtArray)
#endif
{
int numCollPts = 0;
// the start
{
Vector3 oldCapsuleStartPos = Vector3.Transform(oldCapsule.Position, oldPlaneInvTransform);
Vector3 newCapsuleStartPos = Vector3.Transform(newCapsule.Position, newPlaneInvTransform);
float oldDist = Distance.PointPlaneDistance(oldCapsuleStartPos, oldPlane);
float newDist = Distance.PointPlaneDistance(newCapsuleStartPos, newPlane);
if (OpenTKHelper.Min(newDist, oldDist) < collTolerance + newCapsule.Radius)
{
float oldDepth = oldCapsule.Radius - oldDist;
// calc the world position based on the old position8(s)
Vector3 worldPos = oldCapsule.Position - oldCapsule.Radius * oldPlane.Normal;
// BEN-OPTIMISATION: Now reuses existing collPts instead of reallocating.
collPts[numCollPts].R0 = worldPos - body0Pos;
collPts[numCollPts].R1 = worldPos - body1Pos;
collPts[numCollPts++].InitialPenetration = oldDepth;
}
}
// the end
{
Vector3 oldCapsuleEndPos = Vector3.Transform(oldCapsule.GetEnd(), oldPlaneInvTransform);
Vector3 newCapsuleEndPos = Vector3.Transform(newCapsule.GetEnd(), newPlaneInvTransform);
float oldDist = Distance.PointPlaneDistance(oldCapsuleEndPos, oldPlane);
float newDist = Distance.PointPlaneDistance(newCapsuleEndPos, newPlane);
if (System.Math.Min(newDist, oldDist) < collTolerance + newCapsule.Radius)
{
float oldDepth = oldCapsule.Radius - oldDist;
// calc the world position based on the old position(s)
Vector3 worldPos = oldCapsule.GetEnd() - oldCapsule.Radius * oldPlane.Normal;
// BEN-OPTIMISATION: Now reuses existing collPts instead of reallocating.
collPts[numCollPts].R0 = worldPos - body0Pos;
collPts[numCollPts].R1 = worldPos - body1Pos;
collPts[numCollPts++].InitialPenetration = oldDepth;
}
if (numCollPts > 0)
{
collisionFunctor.CollisionNotify(ref info, ref oldPlane.normal, collPts, numCollPts);
}
}
}
#if !USE_STACKALLOC
FreeStackAlloc(collPtArray);
#endif
}
}
/// <summary>
/// CollDetect
/// </summary>
/// <param name="infoOrig"></param>
/// <param name="collTolerance"></param>
/// <param name="collisionFunctor"></param>
public override void CollDetect(CollDetectInfo infoOrig, float collTolerance, CollisionFunctor collisionFunctor)
{
CollDetectInfo info = infoOrig;
if (info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0).Type == this.Type1)
{
CollisionSkin skinSwap = info.Skin0;
info.Skin0 = info.Skin1;
info.Skin1 = skinSwap;
int primSwap = info.IndexPrim0;
info.IndexPrim0 = info.IndexPrim1;
info.IndexPrim1 = primSwap;
}
Vector3 body0Pos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero;
Vector3 body1Pos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero;
// todo - proper swept test
Capsule oldCapsule = info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0) as Capsule;
Capsule newCapsule = info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0) as Capsule;
Heightmap oldHeightmap = info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1) as Heightmap;
Heightmap newHeightmap = info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1) as Heightmap;
unsafe
{
#if USE_STACKALLOC
SmallCollPointInfo *collPts = stackalloc SmallCollPointInfo[MaxLocalStackSCPI];
#else
SmallCollPointInfo[] collPtArray = SCPIStackAlloc();
fixed(SmallCollPointInfo *collPts = collPtArray)
#endif
{
int numCollPts = 0;
Vector3 averageNormal = Vector3.Zero;
// the start
{
float oldDist, newDist;
Vector3 normal;
oldHeightmap.GetHeightAndNormal(out oldDist, out normal, oldCapsule.Position);
newHeightmap.GetHeightAndNormal(out newDist, out normal, newCapsule.Position);
if (OpenTKHelper.Min(newDist, oldDist) < collTolerance + newCapsule.Radius)
{
float oldDepth = oldCapsule.Radius - oldDist;
// calc the world position based on the old position(s)
Vector3 worldPos = oldCapsule.Position - oldCapsule.Radius * normal;
if (numCollPts < MaxLocalStackSCPI)
{
// BEN-OPTIMISATION: Now reuses existing collPts instead of reallocating.
collPts[numCollPts].R0 = worldPos - body0Pos;
collPts[numCollPts].R1 = worldPos - body1Pos;
collPts[numCollPts++].InitialPenetration = oldDepth;
}
averageNormal += normal;
}
}
// the end
{
Vector3 oldEnd = oldCapsule.GetEnd();
Vector3 newEnd = newCapsule.GetEnd();
float oldDist, newDist;
Vector3 normal;
oldHeightmap.GetHeightAndNormal(out oldDist, out normal, oldEnd);
newHeightmap.GetHeightAndNormal(out newDist, out normal, newEnd);
if (OpenTKHelper.Min(newDist, oldDist) < collTolerance + newCapsule.Radius)
{
float oldDepth = oldCapsule.Radius - oldDist;
// calc the world position based on the old position(s)
Vector3 worldPos = oldEnd - oldCapsule.Radius * normal;
if (numCollPts < MaxLocalStackSCPI)
{
// BEN-OPTIMISATION: Now reuses existing collPts instead of reallocating.
collPts[numCollPts].R0 = worldPos - body0Pos;
collPts[numCollPts].R1 = worldPos - body1Pos;
collPts[numCollPts++].InitialPenetration = oldDepth;
}
averageNormal += normal;
}
}
if (numCollPts > 0)
{
JiggleMath.NormalizeSafe(ref averageNormal);
collisionFunctor.CollisionNotify(ref info, ref averageNormal, collPts, numCollPts);
}
}
#if !USE_STACKALLOC
FreeStackAlloc(collPtArray);
#endif
}
}
/// <summary>
/// Detect BoxBox Collisions.
/// </summary>
/// <param name="info"></param>
/// <param name="collTolerance"></param>
/// <param name="collisionFunctor"></param>
public override void CollDetect(CollDetectInfo info, float collTolerance, CollisionFunctor collisionFunctor)
{
Box box0 = info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0) as Box;
Box box1 = info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1) as Box;
Box oldBox0 = info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0) as Box;
Box oldBox1 = info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1) as Box;
Matrix4 dirs0 = box0.Orientation;
Matrix4 dirs1 = box1.Orientation;
Vector3 box0_Right = dirs0.Right();
Vector3 box0_Up = dirs0.Up();
Vector3 box0_Backward = dirs0.Backward();
Vector3 box1_Right = dirs1.Right();
Vector3 box1_Up = dirs1.Up();
Vector3 box1_Backward = dirs1.Backward();
float testDepth;
if (Disjoint(out testDepth, ref box0_Right, box0, box1, collTolerance))
{
return;
}
float depth = testDepth;
Vector3 N = box0_Right;
int minAxis = 0;
if (Disjoint(out testDepth, ref box0_Up, box0, box1, collTolerance))
{
return;
}
if (testDepth < depth)
{
depth = testDepth;
N = box0_Up;
minAxis = 1;
}
if (Disjoint(out testDepth, ref box0_Backward, box0, box1, collTolerance))
{
return;
}
if (testDepth < depth)
{
depth = testDepth;
N = box0_Backward;
minAxis = 2;
}
if (Disjoint(out testDepth, ref box1_Right, box0, box1, collTolerance))
{
return;
}
if (testDepth < depth)
{
depth = testDepth;
N = box1_Right;
minAxis = 3;
}
if (Disjoint(out testDepth, ref box1_Up, box0, box1, collTolerance))
{
return;
}
if (testDepth < depth)
{
depth = testDepth;
N = box1_Up;
minAxis = 4;
}
if (Disjoint(out testDepth, ref box1_Backward, box0, box1, collTolerance))
{
return;
}
if (testDepth < depth)
{
depth = testDepth;
N = box1_Backward;
minAxis = 5;
}
Vector3 axis;
Vector3.Cross(ref box0_Right, ref box1_Right, out axis);
if (Disjoint(out testDepth, ref axis, box0, box1, collTolerance))
{
return;
}
testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z);
if (testDepth < depth)
{
depth = testDepth;
N = axis;
minAxis = 6;
}
Vector3.Cross(ref box0_Right, ref box1_Up, out axis);
if (Disjoint(out testDepth, ref axis, box0, box1, collTolerance))
{
return;
}
testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z);
if (testDepth < depth)
{
depth = testDepth;
N = axis;
minAxis = 7;
}
Vector3.Cross(ref box0_Right, ref box1_Backward, out axis);
if (Disjoint(out testDepth, ref axis, box0, box1, collTolerance))
{
return;
}
testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z);
if (testDepth < depth)
{
depth = testDepth;
N = axis;
minAxis = 8;
}
Vector3.Cross(ref box0_Up, ref box1_Right, out axis);
if (Disjoint(out testDepth, ref axis, box0, box1, collTolerance))
{
return;
}
testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z);
if (testDepth < depth)
{
depth = testDepth;
N = axis;
minAxis = 9;
}
Vector3.Cross(ref box0_Up, ref box1_Up, out axis);
if (Disjoint(out testDepth, ref axis, box0, box1, collTolerance))
{
return;
}
testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z);
if (testDepth < depth)
{
depth = testDepth;
N = axis;
minAxis = 10;
}
Vector3.Cross(ref box0_Up, ref box1_Backward, out axis);
if (Disjoint(out testDepth, ref axis, box0, box1, collTolerance))
{
return;
}
testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z);
if (testDepth < depth)
{
depth = testDepth;
N = axis;
minAxis = 11;
}
Vector3.Cross(ref box0_Backward, ref box1_Right, out axis);
if (Disjoint(out testDepth, ref axis, box0, box1, collTolerance))
{
return;
}
testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z);
if (testDepth < depth)
{
depth = testDepth;
N = axis;
minAxis = 12;
}
Vector3.Cross(ref box0_Backward, ref box1_Up, out axis);
if (Disjoint(out testDepth, ref axis, box0, box1, collTolerance))
{
return;
}
testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z);
if (testDepth < depth)
{
depth = testDepth;
N = axis;
minAxis = 13;
}
Vector3.Cross(ref box0_Backward, ref box1_Backward, out axis);
if (Disjoint(out testDepth, ref axis, box0, box1, collTolerance))
{
return;
}
testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z);
if (testDepth < depth)
{
depth = testDepth;
N = axis;
minAxis = 14;
}
Vector3 D = box1.GetCentre() - box0.GetCentre();
N.Normalize();
int i;
/*seperatingAxes[0] = dirs0.Right;
* seperatingAxes[1] = dirs0.Up;
* seperatingAxes[2] = dirs0.Backward;
* seperatingAxes[3] = dirs1.Right;
* seperatingAxes[4] = dirs1.Up;
* seperatingAxes[5] = dirs1.Backward;
* Vector3.Cross(ref seperatingAxes[0], ref seperatingAxes[3], out seperatingAxes[6]);
* Vector3.Cross(ref seperatingAxes[0], ref seperatingAxes[4], out seperatingAxes[7]);
* Vector3.Cross(ref seperatingAxes[0], ref seperatingAxes[5], out seperatingAxes[8]);
* Vector3.Cross(ref seperatingAxes[1], ref seperatingAxes[3], out seperatingAxes[9]);
* Vector3.Cross(ref seperatingAxes[1], ref seperatingAxes[4], out seperatingAxes[10]);
* Vector3.Cross(ref seperatingAxes[1], ref seperatingAxes[5], out seperatingAxes[11]);
* Vector3.Cross(ref seperatingAxes[2], ref seperatingAxes[3], out seperatingAxes[12]);
* Vector3.Cross(ref seperatingAxes[2], ref seperatingAxes[4], out seperatingAxes[13]);
* Vector3.Cross(ref seperatingAxes[2], ref seperatingAxes[5], out seperatingAxes[14]);
*
*
* // see if the boxes are separate along any axis, and if not keep a
* // record of the depths along each axis
* int i;
* for (i = 0; i < 15; ++i)
* {
* // If we can't normalise the axis, skip it
* float l2 = seperatingAxes[i].LengthSquared;
*
* if (l2 < JiggleMath.Epsilon) continue;
*
* overlapDepth[i] = float.MaxValue;
*
* if (Disjoint(out overlapDepth[i], ref seperatingAxes[i], box0, box1, collTolerance))
* return;
* }
*
* // The box overlap, find the seperation depth closest to 0.
* float minDepth = float.MaxValue;
* int minAxis = -1;
*
* for (i = 0; i < 15; ++i)
* {
* // If we can't normalise the axis, skip it
* float l2 = seperatingAxes[i].LengthSquared;
* if (l2 < JiggleMath.Epsilon) continue;
*
* // Normalise the separation axis and depth
* float invl = 1.0f / (float)System.Math.Sqrt(l2);
* seperatingAxes[i] *= invl;
* overlapDepth[i] *= invl;
*
* // If this axis is the minmum, select it
* if (overlapDepth[i] < minDepth)
* {
* minDepth = overlapDepth[i];
* minAxis = i;
* }
* }
*
* if (minAxis == -1)
* return;
*
* // Make sure the axis is facing towards the 0th box.
* // if not, invert it
* Vector3 D = box1.GetCentre() - box0.GetCentre();
* Vector3 N = seperatingAxes[minAxis];
* float depth = overlapDepth[minAxis];*/
if (Vector3.Dot(D, N) > 0.0f)
{
N *= -1.0f;
}
float minA = OpenTKHelper.Min(box0.SideLengths.X, OpenTKHelper.Min(box0.SideLengths.Y, box0.SideLengths.Z));
float minB = OpenTKHelper.Min(box1.SideLengths.X, OpenTKHelper.Min(box1.SideLengths.Y, box1.SideLengths.Z));
float combinationDist = 0.05f * OpenTKHelper.Min(minA, minB);
// the contact points
bool contactPointsFromOld = true;
contactPts.Clear();
if (depth > -JiggleMath.Epsilon)
{
GetBoxBoxIntersectionPoints(contactPts, oldBox0, oldBox1, combinationDist, collTolerance);
}
int numPts = contactPts.Count;
if (numPts == 0)
{
contactPointsFromOld = false;
GetBoxBoxIntersectionPoints(contactPts, box0, box1, combinationDist, collTolerance);
}
numPts = contactPts.Count;
Vector3 body0OldPos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero;
Vector3 body1OldPos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero;
Vector3 body0NewPos = (info.Skin0.Owner != null) ? info.Skin0.Owner.Position : Vector3.Zero;
Vector3 body1NewPos = (info.Skin1.Owner != null) ? info.Skin1.Owner.Position : Vector3.Zero;
#region REFERENCE: Vector3 bodyDelta = body0NewPos - body0OldPos - body1NewPos + body1OldPos;
Vector3 bodyDelta;
Vector3.Subtract(ref body0NewPos, ref body0OldPos, out bodyDelta);
Vector3.Subtract(ref bodyDelta, ref body1NewPos, out bodyDelta);
Vector3.Add(ref bodyDelta, ref body1OldPos, out bodyDelta);
#endregion
#region REFERENCE: float bodyDeltaLen = Vector3.Dot(bodyDelta,N);
float bodyDeltaLen;
Vector3.Dot(ref bodyDelta, ref N, out bodyDeltaLen);
#endregion
float oldDepth = depth + bodyDeltaLen;
unsafe
{
#if USE_STACKALLOC
SmallCollPointInfo *collPts = stackalloc SmallCollPointInfo[MaxLocalStackSCPI];
#else
SmallCollPointInfo[] collPtArray = SCPIStackAlloc();
fixed(SmallCollPointInfo *collPts = collPtArray)
#endif
{
int numCollPts = 0;
Vector3 SATPoint;
switch (minAxis)
{
// Box0 face, Box1 corner collision
case 0:
case 1:
case 2:
{
// Get the lowest point on the box1 along box1 normal
GetSupportPoint(out SATPoint, box1, -N);
break;
}
// We have a Box2 corner/Box1 face collision
case 3:
case 4:
case 5:
{
// Find with vertex on the triangle collided
GetSupportPoint(out SATPoint, box0, N);
break;
}
// We have an edge/edge collision
default:
/*case 6:
* case 7:
* case 8:
* case 9:
* case 10:
* case 11:
* case 12:
* case 13:
* case 14:*/
{
{
// Retrieve which edges collided.
i = minAxis - 6;
int ia = i / 3;
int ib = i - ia * 3;
// find two P0, P1 point on both edges.
Vector3 P0, P1;
GetSupportPoint(out P0, box0, N);
GetSupportPoint(out P1, box1, -N);
// Find the edge intersection.
// plane along N and F, and passing through PB
Vector3 box0Orient, box1Orient;
JiggleUnsafe.Get(ref box0.transform.Orientation, ia, out box0Orient);
JiggleUnsafe.Get(ref box1.transform.Orientation, ib, out box1Orient);
#region REFERENCE: Vector3 planeNormal = Vector3.Cross(N, box1Orient[ib]);
Vector3 planeNormal;
Vector3.Cross(ref N, ref box1Orient, out planeNormal);
#endregion
#region REFERENCE: float planeD = Vector3.Dot(planeNormal, P1);
float planeD;
Vector3.Dot(ref planeNormal, ref P1, out planeD);
#endregion
// find the intersection t, where Pintersection = P0 + t*box edge dir
#region REFERENCE: float div = Vector3.Dot(box0Orient, planeNormal);
float div;
Vector3.Dot(ref box0Orient, ref planeNormal, out div);
#endregion
// plane and ray colinear, skip the intersection.
if (System.Math.Abs(div) < JiggleMath.Epsilon)
{
return;
}
float t = (planeD - Vector3.Dot(P0, planeNormal)) / div;
// point on edge of box0
#region REFERENCE: P0 += box0Orient * t;
P0 = Vector3.Add(Vector3.Multiply(box0Orient, t), P0);
#endregion
#region REFERENCE: SATPoint = (P0 + (0.5f * depth) * N);
Vector3.Multiply(ref N, 0.5f * depth, out SATPoint);
Vector3.Add(ref SATPoint, ref P0, out SATPoint);
#endregion
}
break;
}
/*default:
* throw new Exception("Impossible switch");*/
}
// distribute the depth according to the distance to the SAT point
if (numPts > 0)
{
float minDist = float.MaxValue;
float maxDist = float.MinValue;
for (i = 0; i < numPts; ++i)
{
float dist = Distance.PointPointDistance(contactPts[i].Pos, SATPoint);
if (dist < minDist)
{
minDist = dist;
}
if (dist > maxDist)
{
maxDist = dist;
}
}
if (maxDist < minDist + JiggleMath.Epsilon)
{
maxDist = minDist + JiggleMath.Epsilon;
}
// got some intersection points
for (i = 0; i < numPts; ++i)
{
float minDepthScale = 0.0f;
float dist = Distance.PointPointDistance(contactPts[i].Pos, SATPoint);
float depthDiv = System.Math.Max(JiggleMath.Epsilon, maxDist - minDist);
float depthScale = (dist - minDist) / depthDiv;
depth = (1.0f - depthScale) * oldDepth + minDepthScale * depthScale * oldDepth;
if (contactPointsFromOld)
{
if (numCollPts < MaxLocalStackSCPI)
{
// BEN-OPTIMISATION: Instead of allocating a new SmallCollPointInfo we reuse the existing one.
collPts[numCollPts].R0 = contactPts[i].Pos - body0OldPos;
collPts[numCollPts].R1 = contactPts[i].Pos - body1OldPos;
collPts[numCollPts++].InitialPenetration = depth;
}
}
else
{
if (numCollPts < MaxLocalStackSCPI)
{
// BEN-OPTIMISATION: Instead of allocating a new SmallCollPointInfo we reuse the existing one.
collPts[numCollPts].R0 = contactPts[i].Pos - body0NewPos;
collPts[numCollPts].R1 = contactPts[i].Pos - body1NewPos;
collPts[numCollPts++].InitialPenetration = depth;
}
}
}
}
else
{
#region REFERENCE: collPts.Add(new CollPointInfo(SATPoint - body0NewPos, SATPoint - body1NewPos, oldDepth));
//collPts.Add(new CollPointInfo(SATPoint - body0NewPos, SATPoint - body1NewPos, oldDepth));
Vector3 cp0;
Vector3.Subtract(ref SATPoint, ref body0NewPos, out cp0);
Vector3 cp1;
Vector3.Subtract(ref SATPoint, ref body1NewPos, out cp1);
if (numCollPts < MaxLocalStackSCPI)
{
// BEN-OPTIMISATION: Instead of allocating a new SmallCollPointInfo we reuse the existing one.
collPts[numCollPts].R0 = cp0;
collPts[numCollPts].R1 = cp1;
collPts[numCollPts++].InitialPenetration = oldDepth;
}
#endregion
}
// report Collisions
collisionFunctor.CollisionNotify(ref info, ref N, collPts, numCollPts);
}
#if !USE_STACKALLOC
FreeStackAlloc(collPtArray);
#endif
}
}
/// <summary>
/// CollDetect
/// </summary>
/// <param name="infoOrig"></param>
/// <param name="collTolerance"></param>
/// <param name="collisionFunctor"></param>
public override void CollDetect(CollDetectInfo infoOrig, float collTolerance, CollisionFunctor collisionFunctor)
{
CollDetectInfo info = infoOrig;
// get the skins in the order that we're expecting
if (info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0).Type == this.Type1)
{
CollisionSkin skinSwap = info.Skin0;
info.Skin0 = info.Skin1;
info.Skin1 = skinSwap;
int primSwap = info.IndexPrim0;
info.IndexPrim0 = info.IndexPrim1;
info.IndexPrim1 = primSwap;
}
Vector3 body0Pos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero;
Vector3 body1Pos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero;
// todo - proper swept test
Sphere oldSphere = info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0) as Sphere;
Sphere newSphere = info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0) as Sphere;
Capsule oldCapsule = info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1) as Capsule;
Capsule newCapsule = info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1) as Capsule;
Segment oldSeg = new Segment(oldCapsule.Position, oldCapsule.Length * oldCapsule.Orientation.Backward());
Segment newSeg = new Segment(oldCapsule.Position, newCapsule.Length * newCapsule.Orientation.Backward());
float radSum = newCapsule.Radius + newSphere.Radius;
float oldt, newt;
float oldDistSq = Distance.PointSegmentDistanceSq(out oldt, oldSphere.Position, oldSeg);
float newDistSq = Distance.PointSegmentDistanceSq(out newt, newSphere.Position, newSeg);
if (OpenTKHelper.Min(oldDistSq, newDistSq) < (radSum + collTolerance) * (radSum + collTolerance))
{
Vector3 segPos = oldSeg.GetPoint(oldt);
Vector3 delta = oldSphere.Position - segPos;
float dist = (float)System.Math.Sqrt((float)oldDistSq);
float depth = radSum - dist;
if (dist > JiggleMath.Epsilon)
{
delta /= dist;
}
else
{
// todo - make this not random
delta = Vector3Extensions.TransformNormal(Vector3Extensions.Backward, Matrix4.CreateFromAxisAngle(Vector3Extensions.Up, OpenTKHelper.ToRadians(random.Next(360))));
}
Vector3 worldPos = segPos +
((oldCapsule.Radius - 0.5f * depth) * delta);
unsafe
{
SmallCollPointInfo collInfo = new SmallCollPointInfo(worldPos - body0Pos,
worldPos - body1Pos, depth);
collisionFunctor.CollisionNotify(ref info, ref delta, &collInfo, 1);
}
}
}