///<summary> /// Sweeps two shapes against another. ///</summary> ///<param name="shapeA">First shape being swept.</param> ///<param name="shapeB">Second shape being swept.</param> ///<param name="sweepA">Sweep vector for the first shape.</param> ///<param name="sweepB">Sweep vector for the second shape.</param> ///<param name="transformA">Transform to apply to the first shape.</param> ///<param name="transformB">Transform to apply to the second shape.</param> ///<param name="hit">Hit data of the sweep test, if any.</param> ///<returns>Whether or not the swept shapes hit each other..</returns> public static bool ConvexCast(ConvexShape shapeA, ConvexShape shapeB, ref Vector3 sweepA, ref Vector3 sweepB, ref RigidTransform transformA, ref RigidTransform transformB, out RayHit hit) { //Put the velocity into shapeA's local space. Vector3 velocityWorld; Vector3.Subtract(ref sweepB, ref sweepA, out velocityWorld); Quaternion conjugateOrientationA; Quaternion.Conjugate(ref transformA.Orientation, out conjugateOrientationA); Vector3 rayDirection; Quaternion.Transform(ref velocityWorld, ref conjugateOrientationA, out rayDirection); //Transform b into a's local space. RigidTransform localTransformB; Quaternion.Concatenate(ref transformB.Orientation, ref conjugateOrientationA, out localTransformB.Orientation); Vector3.Subtract(ref transformB.Position, ref transformA.Position, out localTransformB.Position); Quaternion.Transform(ref localTransformB.Position, ref conjugateOrientationA, out localTransformB.Position); Vector3 w, p; hit.T = 0; hit.Location = Vector3.Zero; //The ray starts at the origin. hit.Normal = Toolbox.ZeroVector; Vector3 v = hit.Location; RaySimplex simplex = new RaySimplex(); float vw, vdir; int count = 0; do { if (++count > MaximumGJKIterations) { //It's taken too long to find a hit. Numerical problems are probable; quit. hit = new RayHit(); return(false); } MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref v, ref localTransformB, out p); Vector3.Subtract(ref hit.Location, ref p, out w); Vector3.Dot(ref v, ref w, out vw); if (vw > 0) { Vector3.Dot(ref v, ref rayDirection, out vdir); if (vdir >= 0) { hit = new RayHit(); return(false); } hit.T = hit.T - vw / vdir; if (hit.T > 1) { //If we've gone beyond where the ray can reach, there's obviously no hit. hit = new RayHit(); return(false); } //Shift the ray up. Vector3.Multiply(ref rayDirection, hit.T, out hit.Location); //The ray origin is the origin! Don't need to add any ray position. hit.Normal = v; } RaySimplex shiftedSimplex; simplex.AddNewSimplexPoint(ref p, ref hit.Location, out shiftedSimplex); shiftedSimplex.GetPointClosestToOrigin(ref simplex, out v); //Could measure the progress of the ray. If it's too little, could early out. //Not used by default since it's biased towards precision over performance. } while (v.LengthSquared() >= Toolbox.Epsilon * simplex.GetErrorTolerance(ref Toolbox.ZeroVector)); //This epsilon has a significant impact on performance and accuracy. Changing it to use BigEpsilon instead increases speed by around 30-40% usually, but jigging is more evident. //Transform the hit data into world space. Quaternion.Transform(ref hit.Normal, ref transformA.Orientation, out hit.Normal); Vector3.Multiply(ref velocityWorld, hit.T, out hit.Location); Vector3.Add(ref hit.Location, ref transformA.Position, out hit.Location); return(true); }
///<summary> /// Casts a fat (sphere expanded) ray against the shape. ///</summary> ///<param name="ray">Ray to test against the shape.</param> ///<param name="radius">Radius of the ray.</param> ///<param name="shape">Shape to test against.</param> ///<param name="shapeTransform">Transform to apply to the shape for the test.</param> ///<param name="maximumLength">Maximum length of the ray in units of the ray direction's length.</param> ///<param name="hit">Hit data of the sphere cast, if any.</param> ///<returns>Whether or not the sphere cast hit the shape.</returns> public static bool SphereCast(Ray ray, float radius, ConvexShape shape, ref RigidTransform shapeTransform, float maximumLength, out RayHit hit) { //Transform the ray into the object's local space. Vector3.Subtract(ref ray.Position, ref shapeTransform.Position, out ray.Position); Quaternion conjugate; Quaternion.Conjugate(ref shapeTransform.Orientation, out conjugate); Quaternion.Transform(ref ray.Position, ref conjugate, out ray.Position); Quaternion.Transform(ref ray.Direction, ref conjugate, out ray.Direction); Vector3 w, p; hit.T = 0; hit.Location = ray.Position; hit.Normal = Toolbox.ZeroVector; Vector3 v = hit.Location; RaySimplex simplex = new RaySimplex(); float vw, vdir; int count = 0; //This epsilon has a significant impact on performance and accuracy. Changing it to use BigEpsilon instead increases speed by around 30-40% usually, but jigging is more evident. while (v.LengthSquared() >= Toolbox.Epsilon * simplex.GetErrorTolerance(ref ray.Position)) { if (++count > MaximumGJKIterations) { //It's taken too long to find a hit. Numerical problems are probable; quit. hit = new RayHit(); return(false); } shape.GetLocalExtremePointWithoutMargin(ref v, out p); Vector3 contribution; MinkowskiToolbox.ExpandMinkowskiSum(shape.collisionMargin, radius, ref v, out contribution); Vector3.Add(ref p, ref contribution, out p); Vector3.Subtract(ref hit.Location, ref p, out w); Vector3.Dot(ref v, ref w, out vw); if (vw > 0) { Vector3.Dot(ref v, ref ray.Direction, out vdir); hit.T = hit.T - vw / vdir; if (vdir >= 0) { //We would have to back up! return(false); } if (hit.T > maximumLength) { //If we've gone beyond where the ray can reach, there's obviously no hit. return(false); } //Shift the ray up. Vector3.Multiply(ref ray.Direction, hit.T, out hit.Location); Vector3.Add(ref hit.Location, ref ray.Position, out hit.Location); hit.Normal = v; } RaySimplex shiftedSimplex; simplex.AddNewSimplexPoint(ref p, ref hit.Location, out shiftedSimplex); shiftedSimplex.GetPointClosestToOrigin(ref simplex, out v); } //Transform the hit data into world space. Quaternion.Transform(ref hit.Normal, ref shapeTransform.Orientation, out hit.Normal); Quaternion.Transform(ref hit.Location, ref shapeTransform.Orientation, out hit.Location); Vector3.Add(ref hit.Location, ref shapeTransform.Position, out hit.Location); return(true); }
//TODO: Consider changing the termination epsilons on these casts. Epsilon * Modifier is okay, but there might be better options. ///<summary> /// Tests a ray against a convex shape. ///</summary> ///<param name="ray">Ray to test against the shape.</param> ///<param name="shape">Shape to test.</param> ///<param name="shapeTransform">Transform to apply to the shape for the test.</param> ///<param name="maximumLength">Maximum length of the ray in units of the ray direction's length.</param> ///<param name="hit">Hit data of the ray cast, if any.</param> ///<returns>Whether or not the ray hit the shape.</returns> public static bool RayCast(Ray ray, ConvexShape shape, ref RigidTransform shapeTransform, float maximumLength, out RayHit hit) { //Transform the ray into the object's local space. Vector3.Subtract(ref ray.Position, ref shapeTransform.Position, out ray.Position); Quaternion conjugate; Quaternion.Conjugate(ref shapeTransform.Orientation, out conjugate); Quaternion.Transform(ref ray.Position, ref conjugate, out ray.Position); Quaternion.Transform(ref ray.Direction, ref conjugate, out ray.Direction); Vector3 extremePointToRayOrigin, extremePoint; hit.T = 0; hit.Location = ray.Position; hit.Normal = Toolbox.ZeroVector; Vector3 closestOffset = hit.Location; RaySimplex simplex = new RaySimplex(); float vw, closestPointDotDirection; int count = 0; //This epsilon has a significant impact on performance and accuracy. Changing it to use BigEpsilon instead increases speed by around 30-40% usually, but jigging is more evident. while (closestOffset.LengthSquared() >= Toolbox.Epsilon * simplex.GetErrorTolerance(ref ray.Position)) { if (++count > MaximumGJKIterations) { //It's taken too long to find a hit. Numerical problems are probable; quit. hit = new RayHit(); return(false); } shape.GetLocalExtremePoint(closestOffset, out extremePoint); Vector3.Subtract(ref hit.Location, ref extremePoint, out extremePointToRayOrigin); Vector3.Dot(ref closestOffset, ref extremePointToRayOrigin, out vw); //If the closest offset and the extreme point->ray origin direction point the same way, //then we might be able to conservatively advance the point towards the surface. if (vw > 0) { Vector3.Dot(ref closestOffset, ref ray.Direction, out closestPointDotDirection); if (closestPointDotDirection >= 0) { hit = new RayHit(); return(false); } hit.T = hit.T - vw / closestPointDotDirection; if (hit.T > maximumLength) { //If we've gone beyond where the ray can reach, there's obviously no hit. hit = new RayHit(); return(false); } //Shift the ray up. Vector3.Multiply(ref ray.Direction, hit.T, out hit.Location); Vector3.Add(ref hit.Location, ref ray.Position, out hit.Location); hit.Normal = closestOffset; } RaySimplex shiftedSimplex; simplex.AddNewSimplexPoint(ref extremePoint, ref hit.Location, out shiftedSimplex); //Compute the offset from the simplex surface to the origin. shiftedSimplex.GetPointClosestToOrigin(ref simplex, out closestOffset); } //Transform the hit data into world space. Quaternion.Transform(ref hit.Normal, ref shapeTransform.Orientation, out hit.Normal); Quaternion.Transform(ref hit.Location, ref shapeTransform.Orientation, out hit.Location); Vector3.Add(ref hit.Location, ref shapeTransform.Position, out hit.Location); return(true); }