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