/// <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);
}
public static bool SegmentCapsuleIntersection(out float tS, Segment seg, Capsule capsule)
{
var bestFrac = float.MaxValue;
tS = 0;
var sideFrac = float.MaxValue;
if (!SegmentInfiniteCylinderIntersection(out sideFrac, seg, new Segment(capsule.Position, capsule.Orientation.Backward), capsule.Radius))
{
return(false);
}
var 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;
}
var originFrac = float.MaxValue;
SegmentSphereIntersection(out originFrac, seg, new Sphere(capsule.Position, capsule.Radius));
var endFrac = float.MaxValue;
SegmentSphereIntersection(out endFrac, seg, new Sphere(capsule.GetEnd(), capsule.Radius));
bestFrac = MathHelper.Min(sideFrac, originFrac);
bestFrac = MathHelper.Min(bestFrac, endFrac);
if (bestFrac <= 1.0f)
{
tS = bestFrac;
return(true);
}
return(false);
}
/// <summary>
/// CollDetectCapsuleStaticMeshOverlap
/// </summary>
/// <param name="oldCapsule"></param>
/// <param name="newCapsule"></param>
/// <param name="mesh"></param>
/// <param name="info"></param>
/// <param name="collTolerance"></param>
/// <param name="collisionFunctor"></param>
private void CollDetectCapsuleStaticMeshOverlap(Capsule oldCapsule, Capsule newCapsule,
TriangleMesh mesh, CollDetectInfo info, float collTolerance, CollisionFunctor collisionFunctor)
{
Vector3 body0Pos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero;
Vector3 body1Pos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero;
float capsuleTolR = collTolerance + newCapsule.Radius;
float capsuleTolR2 = capsuleTolR * capsuleTolR;
Vector3 collNormal = Vector3.Zero;
BoundingBox bb = BoundingBoxHelper.InitialBox;
BoundingBoxHelper.AddCapsule(newCapsule, ref bb);
unsafe
{
#if USE_STACKALLOC
SmallCollPointInfo* collPts = stackalloc SmallCollPointInfo[MaxLocalStackSCPI];
int* potentialTriangles = stackalloc int[MaxLocalStackTris];
{
{
#else
SmallCollPointInfo[] collPtArray = SCPIStackAlloc();
fixed (SmallCollPointInfo* collPts = collPtArray)
{
int[] potTriArray = IntStackAlloc();
fixed (int* potentialTriangles = potTriArray)
{
#endif
int numCollPts = 0;
int numTriangles = mesh.GetTrianglesIntersectingtAABox(potentialTriangles, MaxLocalStackTris, ref bb);
Vector3 capsuleStart = newCapsule.Position;
Vector3 capsuleEnd = newCapsule.GetEnd();
Matrix4 meshInvTransform = mesh.InverseTransformMatrix;
Vector3 meshSpaceCapsuleStart = Vector3.Transform(capsuleStart, meshInvTransform);
Vector3 meshSpaceCapsuleEnd = Vector3.Transform(capsuleEnd, meshInvTransform);
for (int iTriangle = 0; iTriangle < numTriangles; ++iTriangle)
{
IndexedTriangle meshTriangle = mesh.GetTriangle(potentialTriangles[iTriangle]);
// we do the plane test using the capsule in mesh space
float distToStart = meshTriangle.Plane.DotCoordinate(meshSpaceCapsuleStart);
float distToEnd = meshTriangle.Plane.DotCoordinate(meshSpaceCapsuleEnd);
// BEN-BUG-FIX: Fixed by replacing 0.0F with -capsuleTolR.
if ((distToStart > capsuleTolR && distToEnd > capsuleTolR)
|| (distToStart < -capsuleTolR && distToEnd < -capsuleTolR))
continue;
// we now transform the triangle into world space (we could keep leave the mesh alone
// but at this point 3 vector transforms is probably not a major slow down)
int i0, i1, i2;
meshTriangle.GetVertexIndices(out i0, out i1, out i2);
Vector3 triVec0;
Vector3 triVec1;
Vector3 triVec2;
mesh.GetVertex(i0, out triVec0);
mesh.GetVertex(i1, out triVec1);
mesh.GetVertex(i2, out triVec2);
// Deano move tri into world space
Matrix4 transformMatrix = mesh.TransformMatrix;
Vector3.Transform(ref triVec0, ref transformMatrix, out triVec0);
Vector3.Transform(ref triVec1, ref transformMatrix, out triVec1);
Vector3.Transform(ref triVec2, ref transformMatrix, out triVec2);
Triangle triangle = new Triangle(ref triVec0, ref triVec1, ref triVec2);
Segment seg = new Segment(capsuleStart, capsuleEnd - capsuleStart);
float tS, tT0, tT1;
float d2 = Distance.SegmentTriangleDistanceSq(out tS, out tT0, out tT1, seg, triangle);
if (d2 < capsuleTolR2)
{
Vector3 oldCapsuleStart = oldCapsule.Position;
Vector3 oldCapsuleEnd = oldCapsule.GetEnd();
Segment oldSeg = new Segment(oldCapsuleStart, oldCapsuleEnd - oldCapsuleStart);
d2 = Distance.SegmentTriangleDistanceSq(out tS, out tT0, out tT1, oldSeg, triangle);
// report result from old position
float dist = (float)System.Math.Sqrt(d2);
float depth = oldCapsule.Radius - dist;
Vector3 pt = triangle.GetPoint(tT0, tT1);
Vector3 collisionN = (d2 > JiggleMath.Epsilon) ? JiggleMath.NormalizeSafe(oldSeg.GetPoint(tS) - pt) :
meshTriangle.Plane.Normal;
if (numCollPts < MaxLocalStackSCPI)
{
// BEN-OPTIMISATION: Reused existing collPts.
collPts[numCollPts].R0 = pt - body0Pos;
collPts[numCollPts].R1 = pt - body1Pos;
collPts[numCollPts++].InitialPenetration = depth;
}
collNormal += collisionN;
}
}
if (numCollPts > 0)
{
JiggleMath.NormalizeSafe(ref collNormal);
collisionFunctor.CollisionNotify(ref info, ref collNormal, collPts, numCollPts);
}
#if USE_STACKALLOC
}
}
#else
}
FreeStackAlloc(potTriArray);
}
FreeStackAlloc(collPtArray);
#endif
}
}
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.Position, capsule.Radius));
bestFrac = MathHelper.Min(sideFrac, originFrac);
bestFrac = MathHelper.Min(bestFrac, endFrac);
if (bestFrac <= 1.0f)
{
tS = bestFrac;
return true;
}
return false;
}
