///<summary> /// Gets the point on the simplex that is closest to the origin. ///</summary> ///<param name="simplex">Simplex to test.</param> ///<param name="point">Closest point on the simplex.</param> ///<returns>Whether or not the simplex contains the origin.</returns> public bool GetPointClosestToOrigin(ref RaySimplex simplex, out Vector3 point) { //This method finds the closest point on the simplex to the origin. //Barycentric coordinates are assigned to the MinimumNormCoordinates as necessary to perform the inclusion calculation. //If the simplex is a tetrahedron and found to be overlapping the origin, the function returns true to tell the caller to terminate. //Elements of the simplex that are not used to determine the point of minimum norm are removed from the simplex. switch (State) { case SimplexState.Point: point = A; break; case SimplexState.Segment: GetPointOnSegmentClosestToOrigin(ref simplex, out point); break; case SimplexState.Triangle: GetPointOnTriangleClosestToOrigin(ref simplex, out point); break; case SimplexState.Tetrahedron: return GetPointOnTetrahedronClosestToOrigin(ref simplex, out point); default: point = Toolbox.ZeroVector; break; } return false; }
///<summary> /// Finds the point on the segment to the origin. ///</summary> ///<param name="simplex">Simplex to test.</param> ///<param name="point">Closest point.</param> public void GetPointOnSegmentClosestToOrigin(ref RaySimplex simplex, out Vector3 point) { Vector3 segmentDisplacement; Vector3.Subtract(ref B, ref A, out segmentDisplacement); float dotA; Vector3.Dot(ref segmentDisplacement, ref A, out dotA); if (dotA > 0) { //'Behind' segment. This can't happen in a boolean version, //but with closest points warmstarting or raycasts, it will. simplex.State = SimplexState.Point; point = A; return; } float dotB; Vector3.Dot(ref segmentDisplacement, ref B, out dotB); if (dotB > 0) { //Inside segment. float V = -dotA / segmentDisplacement.LengthSquared(); Vector3.Multiply(ref segmentDisplacement, V, out point); Vector3.Add(ref point, ref A, out point); return; } //It should be possible in the warmstarted closest point calculation/raycasting to be outside B. //It is not possible in a 'boolean' GJK, where it early outs as soon as a separating axis is found. //Outside B. //Remove current A; we're becoming a point. simplex.A = simplex.B; simplex.State = SimplexState.Point; point = A; }
///<summary> /// Adds a new point to the simplex. ///</summary> ///<param name="point">Point to add.</param> ///<param name="hitLocation">Current ray hit location.</param> ///<param name="shiftedSimplex">Simplex shifted with the hit location.</param> public void AddNewSimplexPoint(ref Vector3 point, ref Vector3 hitLocation, out RaySimplex shiftedSimplex) { shiftedSimplex = new RaySimplex(); switch (State) { case SimplexState.Empty: State = SimplexState.Point; A = point; Vector3.Subtract(ref hitLocation, ref A, out shiftedSimplex.A); break; case SimplexState.Point: State = SimplexState.Segment; B = point; Vector3.Subtract(ref hitLocation, ref A, out shiftedSimplex.A); Vector3.Subtract(ref hitLocation, ref B, out shiftedSimplex.B); break; case SimplexState.Segment: State = SimplexState.Triangle; C = point; Vector3.Subtract(ref hitLocation, ref A, out shiftedSimplex.A); Vector3.Subtract(ref hitLocation, ref B, out shiftedSimplex.B); Vector3.Subtract(ref hitLocation, ref C, out shiftedSimplex.C); break; case SimplexState.Triangle: State = SimplexState.Tetrahedron; D = point; Vector3.Subtract(ref hitLocation, ref A, out shiftedSimplex.A); Vector3.Subtract(ref hitLocation, ref B, out shiftedSimplex.B); Vector3.Subtract(ref hitLocation, ref C, out shiftedSimplex.C); Vector3.Subtract(ref hitLocation, ref D, out shiftedSimplex.D); break; } shiftedSimplex.State = State; }
///<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; }
///<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; }
//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; }
private static bool TryTetrahedronTriangle(ref Vector3 A, ref Vector3 B, ref Vector3 C, ref Vector3 simplexA, ref Vector3 simplexB, ref Vector3 simplexC, ref Vector3 otherPoint, out RaySimplex simplex, out Vector3 point) { //Note that there may be some extra terms that can be removed from this process. //Some conditions could use less parameters, since it is known that the origin //is not 'behind' BC or AC. simplex = new RaySimplex(); point = new Vector3(); Vector3 ab, ac; Vector3.Subtract(ref B, ref A, out ab); Vector3.Subtract(ref C, ref A, out ac); Vector3 normal; Vector3.Cross(ref ab, ref ac, out normal); float AdotN, ADdotN; Vector3 AD; Vector3.Subtract(ref otherPoint, ref A, out AD); Vector3.Dot(ref A, ref normal, out AdotN); Vector3.Dot(ref AD, ref normal, out ADdotN); //If (-A * N) * (AD * N) < 0, D and O are on opposite sides of the triangle. if (AdotN * ADdotN >= 0) { //The point we are comparing against the triangle is 0,0,0, so instead of storing an "A->P" vector, //just use -A. //Same for B->, C->P... //Check to see if it's outside A. //TODO: Note that in a boolean-style GJK, it shouldn't be possible to be outside A. float AdotAB, AdotAC; Vector3.Dot(ref ab, ref A, out AdotAB); Vector3.Dot(ref ac, ref A, out AdotAC); AdotAB = -AdotAB; AdotAC = -AdotAC; if (AdotAC <= 0f && AdotAB <= 0) { //It is A! simplex.State = SimplexState.Point; simplex.A = simplexA; point = A; return true; } //Check to see if it's outside B. //TODO: Note that in a boolean-style GJK, it shouldn't be possible to be outside B. float BdotAB, BdotAC; Vector3.Dot(ref ab, ref B, out BdotAB); Vector3.Dot(ref ac, ref B, out BdotAC); BdotAB = -BdotAB; BdotAC = -BdotAC; if (BdotAB >= 0f && BdotAC <= BdotAB) { //It is B! simplex.State = SimplexState.Point; simplex.A = simplexB; point = B; return true; } //Check to see if it's outside AB. float vc = AdotAB * BdotAC - BdotAB * AdotAC; if (vc <= 0 && AdotAB > 0 && BdotAB < 0) //Note > and < instead of => <=; avoids possibly division by zero { simplex.State = SimplexState.Segment; simplex.A = simplexA; simplex.B = simplexB; float V = AdotAB / (AdotAB - BdotAB); Vector3.Multiply(ref ab, V, out point); Vector3.Add(ref point, ref A, out point); return true; } //Check to see if it's outside C. //TODO: Note that in a boolean-style GJK, it shouldn't be possible to be outside C. float CdotAB, CdotAC; Vector3.Dot(ref ab, ref C, out CdotAB); Vector3.Dot(ref ac, ref C, out CdotAC); CdotAB = -CdotAB; CdotAC = -CdotAC; if (CdotAC >= 0f && CdotAB <= CdotAC) { //It is C! simplex.State = SimplexState.Point; simplex.A = simplexC; point = C; return true; } //Check if it's outside AC. //float AdotAB, AdotAC; //Vector3.Dot(ref ab, ref A, out AdotAB); //Vector3.Dot(ref ac, ref A, out AdotAC); //AdotAB = -AdotAB; //AdotAC = -AdotAC; float vb = CdotAB * AdotAC - AdotAB * CdotAC; if (vb <= 0f && AdotAC > 0f && CdotAC < 0f) //Note > instead of >= and < instead of <=; prevents bad denominator { simplex.State = SimplexState.Segment; simplex.A = simplexA; simplex.B = simplexC; float V = AdotAC / (AdotAC - CdotAC); Vector3.Multiply(ref ac, V, out point); Vector3.Add(ref point, ref A, out point); return true; } //Check if it's outside BC. //float BdotAB, BdotAC; //Vector3.Dot(ref ab, ref B, out BdotAB); //Vector3.Dot(ref ac, ref B, out BdotAC); //BdotAB = -BdotAB; //BdotAC = -BdotAC; float va = BdotAB * CdotAC - CdotAB * BdotAC; float d3d4; float d6d5; if (va <= 0f && (d3d4 = BdotAC - BdotAB) > 0f && (d6d5 = CdotAB - CdotAC) > 0f)//Note > instead of >= and < instead of <=; prevents bad denominator { simplex.State = SimplexState.Segment; simplex.A = simplexB; simplex.B = simplexC; float V = d3d4 / (d3d4 + d6d5); Vector3 bc; Vector3.Subtract(ref C, ref B, out bc); Vector3.Multiply(ref bc, V, out point); Vector3.Add(ref point, ref B, out point); return true; } //On the face of the triangle. simplex.State = SimplexState.Triangle; simplex.A = simplexA; simplex.B = simplexB; simplex.C = simplexC; float denom = 1f / (va + vb + vc); float w = vc * denom; float v = vb * denom; Vector3.Multiply(ref ab, v, out point); Vector3 acw; Vector3.Multiply(ref ac, w, out acw); Vector3.Add(ref A, ref point, out point); Vector3.Add(ref point, ref acw, out point); return true; } return false; }
///<summary> /// Gets the point closest to the origin on the tetrahedron. ///</summary> ///<param name="simplex">Simplex to test.</param> ///<param name="point">Closest point.</param> ///<returns>Whether or not the tetrahedron encloses the origin.</returns> public bool GetPointOnTetrahedronClosestToOrigin(ref RaySimplex simplex, out Vector3 point) { //Thanks to the fact that D is new and that we know that the origin is within the extruded //triangular prism of ABC (and on the "D" side of ABC), //we can immediately ignore voronoi regions: //A, B, C, AC, AB, BC, ABC //and only consider: //D, DA, DB, DC, DAC, DCB, DBA //There is some overlap of calculations in this method, since DAC, DCB, and DBA are tested fully. //When this method is being called, we don't care about the state of 'this' simplex. It's just a temporary shifted simplex. //The one that needs to be updated is the simplex being passed in. var minimumSimplex = new RaySimplex(); point = new Vector3(); float minimumDistance = float.MaxValue; RaySimplex candidate; float candidateDistance; Vector3 candidatePoint; if (TryTetrahedronTriangle(ref A, ref C, ref D, ref simplex.A, ref simplex.C, ref simplex.D, ref B, out candidate, out candidatePoint)) { point = candidatePoint; minimumSimplex = candidate; minimumDistance = candidatePoint.LengthSquared(); } if (TryTetrahedronTriangle(ref C, ref B, ref D, ref simplex.C, ref simplex.B, ref simplex.D, ref A, out candidate, out candidatePoint) && (candidateDistance = candidatePoint.LengthSquared()) < minimumDistance) { point = candidatePoint; minimumSimplex = candidate; minimumDistance = candidateDistance; } if (TryTetrahedronTriangle(ref B, ref A, ref D, ref simplex.B, ref simplex.A, ref simplex.D, ref C, out candidate, out candidatePoint) && (candidateDistance = candidatePoint.LengthSquared()) < minimumDistance) { point = candidatePoint; minimumSimplex = candidate; minimumDistance = candidateDistance; } if (TryTetrahedronTriangle(ref A, ref B, ref C, ref simplex.A, ref simplex.B, ref simplex.C, ref D, out candidate, out candidatePoint) && (candidateDistance = candidatePoint.LengthSquared()) < minimumDistance) { point = candidatePoint; minimumSimplex = candidate; minimumDistance = candidateDistance; } if (minimumDistance < float.MaxValue) { simplex = minimumSimplex; return false; } return true; }
///<summary> /// Gets the point on the triangle that is closest to the origin. ///</summary> ///<param name="simplex">Simplex to test.</param> ///<param name="point">Closest point to origin.</param> public void GetPointOnTriangleClosestToOrigin(ref RaySimplex simplex, out Vector3 point) { Vector3 ab, ac; Vector3.Subtract(ref B, ref A, out ab); Vector3.Subtract(ref C, ref A, out ac); //The point we are comparing against the triangle is 0,0,0, so instead of storing an "A->P" vector, //just use -A. //Same for B->P, C->P... //Check to see if it's outside A. //TODO: Note that in a boolean-style GJK, it shouldn't be possible to be outside A. float AdotAB, AdotAC; Vector3.Dot(ref ab, ref A, out AdotAB); Vector3.Dot(ref ac, ref A, out AdotAC); AdotAB = -AdotAB; AdotAC = -AdotAC; if (AdotAC <= 0f && AdotAB <= 0) { //It is A! simplex.State = SimplexState.Point; point = A; return; } //Check to see if it's outside B. //TODO: Note that in a boolean-style GJK, it shouldn't be possible to be outside B. float BdotAB, BdotAC; Vector3.Dot(ref ab, ref B, out BdotAB); Vector3.Dot(ref ac, ref B, out BdotAC); BdotAB = -BdotAB; BdotAC = -BdotAC; if (BdotAB >= 0f && BdotAC <= BdotAB) { //It is B! simplex.State = SimplexState.Point; simplex.A = simplex.B; point = B; return; } //Check to see if it's outside AB. float vc = AdotAB * BdotAC - BdotAB * AdotAC; if (vc <= 0 && AdotAB > 0 && BdotAB < 0)//Note > and < instead of => <=; avoids possibly division by zero { simplex.State = SimplexState.Segment; float V = AdotAB / (AdotAB - BdotAB); Vector3.Multiply(ref ab, V, out point); Vector3.Add(ref point, ref A, out point); return; } //Check to see if it's outside C. //TODO: Note that in a boolean-style GJK, it shouldn't be possible to be outside C. float CdotAB, CdotAC; Vector3.Dot(ref ab, ref C, out CdotAB); Vector3.Dot(ref ac, ref C, out CdotAC); CdotAB = -CdotAB; CdotAC = -CdotAC; if (CdotAC >= 0f && CdotAB <= CdotAC) { //It is C! simplex.State = SimplexState.Point; simplex.A = simplex.C; point = A; return; } //Check if it's outside AC. //float AdotAB, AdotAC; //Vector3.Dot(ref ab, ref A, out AdotAB); //Vector3.Dot(ref ac, ref A, out AdotAC); //AdotAB = -AdotAB; //AdotAC = -AdotAC; float vb = CdotAB * AdotAC - AdotAB * CdotAC; if (vb <= 0f && AdotAC > 0f && CdotAC < 0f)//Note > instead of >= and < instead of <=; prevents bad denominator { //Get rid of B. Compress C into B. simplex.State = SimplexState.Segment; simplex.B = simplex.C; float V = AdotAC / (AdotAC - CdotAC); Vector3.Multiply(ref ac, V, out point); Vector3.Add(ref point, ref A, out point); return; } //Check if it's outside BC. //float BdotAB, BdotAC; //Vector3.Dot(ref ab, ref B, out BdotAB); //Vector3.Dot(ref ac, ref B, out BdotAC); //BdotAB = -BdotAB; //BdotAC = -BdotAC; float va = BdotAB * CdotAC - CdotAB * BdotAC; float d3d4; float d6d5; if (va <= 0f && (d3d4 = BdotAC - BdotAB) > 0f && (d6d5 = CdotAB - CdotAC) > 0f)//Note > instead of >= and < instead of <=; prevents bad denominator { //Throw away A. C->A. //TODO: Does B->A, C->B work better? simplex.State = SimplexState.Segment; simplex.A = simplex.C; float U = d3d4 / (d3d4 + d6d5); Vector3 bc; Vector3.Subtract(ref C, ref B, out bc); Vector3.Multiply(ref bc, U, out point); Vector3.Add(ref point, ref B, out point); return; } //On the face of the triangle. float denom = 1f / (va + vb + vc); float v = vb * denom; float w = vc * denom; Vector3.Multiply(ref ab, v, out point); Vector3 acw; Vector3.Multiply(ref ac, w, out acw); Vector3.Add(ref A, ref point, out point); Vector3.Add(ref point, ref acw, out point); }
private static bool TryTetrahedronTriangle(ref Vector3 A, ref Vector3 B, ref Vector3 C, ref Vector3 simplexA, ref Vector3 simplexB, ref Vector3 simplexC, ref Vector3 otherPoint, out RaySimplex simplex, out Vector3 point) { //Note that there may be some extra terms that can be removed from this process. //Some conditions could use less parameters, since it is known that the origin //is not 'behind' BC or AC. simplex = new RaySimplex(); point = new Vector3(); Vector3 ab, ac; Vector3.Subtract(ref B, ref A, out ab); Vector3.Subtract(ref C, ref A, out ac); Vector3 normal; Vector3.Cross(ref ab, ref ac, out normal); float AdotN, ADdotN; Vector3 AD; Vector3.Subtract(ref otherPoint, ref A, out AD); Vector3.Dot(ref A, ref normal, out AdotN); Vector3.Dot(ref AD, ref normal, out ADdotN); //If (-A * N) * (AD * N) < 0, D and O are on opposite sides of the triangle. if (AdotN * ADdotN >= 0) { //The point we are comparing against the triangle is 0,0,0, so instead of storing an "A->P" vector, //just use -A. //Same for B->, C->P... //Check to see if it's outside A. //TODO: Note that in a boolean-style GJK, it shouldn't be possible to be outside A. float AdotAB, AdotAC; Vector3.Dot(ref ab, ref A, out AdotAB); Vector3.Dot(ref ac, ref A, out AdotAC); AdotAB = -AdotAB; AdotAC = -AdotAC; if (AdotAC <= 0f && AdotAB <= 0) { //It is A! simplex.State = SimplexState.Point; simplex.A = simplexA; point = A; return(true); } //Check to see if it's outside B. //TODO: Note that in a boolean-style GJK, it shouldn't be possible to be outside B. float BdotAB, BdotAC; Vector3.Dot(ref ab, ref B, out BdotAB); Vector3.Dot(ref ac, ref B, out BdotAC); BdotAB = -BdotAB; BdotAC = -BdotAC; if (BdotAB >= 0f && BdotAC <= BdotAB) { //It is B! simplex.State = SimplexState.Point; simplex.A = simplexB; point = B; return(true); } //Check to see if it's outside AB. float vc = AdotAB * BdotAC - BdotAB * AdotAC; if (vc <= 0 && AdotAB > 0 && BdotAB < 0) //Note > and < instead of => <=; avoids possibly division by zero { simplex.State = SimplexState.Segment; simplex.A = simplexA; simplex.B = simplexB; float V = AdotAB / (AdotAB - BdotAB); Vector3.Multiply(ref ab, V, out point); Vector3.Add(ref point, ref A, out point); return(true); } //Check to see if it's outside C. //TODO: Note that in a boolean-style GJK, it shouldn't be possible to be outside C. float CdotAB, CdotAC; Vector3.Dot(ref ab, ref C, out CdotAB); Vector3.Dot(ref ac, ref C, out CdotAC); CdotAB = -CdotAB; CdotAC = -CdotAC; if (CdotAC >= 0f && CdotAB <= CdotAC) { //It is C! simplex.State = SimplexState.Point; simplex.A = simplexC; point = C; return(true); } //Check if it's outside AC. //float AdotAB, AdotAC; //Vector3.Dot(ref ab, ref A, out AdotAB); //Vector3.Dot(ref ac, ref A, out AdotAC); //AdotAB = -AdotAB; //AdotAC = -AdotAC; float vb = CdotAB * AdotAC - AdotAB * CdotAC; if (vb <= 0f && AdotAC > 0f && CdotAC < 0f) //Note > instead of >= and < instead of <=; prevents bad denominator { simplex.State = SimplexState.Segment; simplex.A = simplexA; simplex.B = simplexC; float V = AdotAC / (AdotAC - CdotAC); Vector3.Multiply(ref ac, V, out point); Vector3.Add(ref point, ref A, out point); return(true); } //Check if it's outside BC. //float BdotAB, BdotAC; //Vector3.Dot(ref ab, ref B, out BdotAB); //Vector3.Dot(ref ac, ref B, out BdotAC); //BdotAB = -BdotAB; //BdotAC = -BdotAC; float va = BdotAB * CdotAC - CdotAB * BdotAC; float d3d4; float d6d5; if (va <= 0f && (d3d4 = BdotAC - BdotAB) > 0f && (d6d5 = CdotAB - CdotAC) > 0f)//Note > instead of >= and < instead of <=; prevents bad denominator { simplex.State = SimplexState.Segment; simplex.A = simplexB; simplex.B = simplexC; float V = d3d4 / (d3d4 + d6d5); Vector3 bc; Vector3.Subtract(ref C, ref B, out bc); Vector3.Multiply(ref bc, V, out point); Vector3.Add(ref point, ref B, out point); return(true); } //On the face of the triangle. simplex.State = SimplexState.Triangle; simplex.A = simplexA; simplex.B = simplexB; simplex.C = simplexC; float denom = 1f / (va + vb + vc); float w = vc * denom; float v = vb * denom; Vector3.Multiply(ref ab, v, out point); Vector3 acw; Vector3.Multiply(ref ac, w, out acw); Vector3.Add(ref A, ref point, out point); Vector3.Add(ref point, ref acw, out point); return(true); } return(false); }
///<summary> /// Gets the point closest to the origin on the tetrahedron. ///</summary> ///<param name="simplex">Simplex to test.</param> ///<param name="point">Closest point.</param> ///<returns>Whether or not the tetrahedron encloses the origin.</returns> public bool GetPointOnTetrahedronClosestToOrigin(ref RaySimplex simplex, out Vector3 point) { //Thanks to the fact that D is new and that we know that the origin is within the extruded //triangular prism of ABC (and on the "D" side of ABC), //we can immediately ignore voronoi regions: //A, B, C, AC, AB, BC, ABC //and only consider: //D, DA, DB, DC, DAC, DCB, DBA //There is some overlap of calculations in this method, since DAC, DCB, and DBA are tested fully. //When this method is being called, we don't care about the state of 'this' simplex. It's just a temporary shifted simplex. //The one that needs to be updated is the simplex being passed in. var minimumSimplex = new RaySimplex(); point = new Vector3(); float minimumDistance = float.MaxValue; RaySimplex candidate; float candidateDistance; Vector3 candidatePoint; if (TryTetrahedronTriangle(ref A, ref C, ref D, ref simplex.A, ref simplex.C, ref simplex.D, ref B, out candidate, out candidatePoint)) { point = candidatePoint; minimumSimplex = candidate; minimumDistance = candidatePoint.LengthSquared(); } if (TryTetrahedronTriangle(ref C, ref B, ref D, ref simplex.C, ref simplex.B, ref simplex.D, ref A, out candidate, out candidatePoint) && (candidateDistance = candidatePoint.LengthSquared()) < minimumDistance) { point = candidatePoint; minimumSimplex = candidate; minimumDistance = candidateDistance; } if (TryTetrahedronTriangle(ref B, ref A, ref D, ref simplex.B, ref simplex.A, ref simplex.D, ref C, out candidate, out candidatePoint) && (candidateDistance = candidatePoint.LengthSquared()) < minimumDistance) { point = candidatePoint; minimumSimplex = candidate; minimumDistance = candidateDistance; } if (TryTetrahedronTriangle(ref A, ref B, ref C, ref simplex.A, ref simplex.B, ref simplex.C, ref D, out candidate, out candidatePoint) && (candidateDistance = candidatePoint.LengthSquared()) < minimumDistance) { point = candidatePoint; minimumSimplex = candidate; minimumDistance = candidateDistance; } if (minimumDistance < float.MaxValue) { simplex = minimumSimplex; return(false); } 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); Vector3.Transform(ref ray.Position, ref conjugate, out ray.Position); Vector3.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. Vector3.Transform(ref hit.Normal, ref shapeTransform.Orientation, out hit.Normal); Vector3.Transform(ref hit.Location, ref shapeTransform.Orientation, out hit.Location); Vector3.Add(ref hit.Location, ref shapeTransform.Position, out hit.Location); return(true); }
///<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; Vector3.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); Vector3.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. Vector3.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); }
//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); Vector3.Transform(ref ray.Position, ref conjugate, out ray.Position); Vector3.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. Vector3.Transform(ref hit.Normal, ref shapeTransform.Orientation, out hit.Normal); Vector3.Transform(ref hit.Location, ref shapeTransform.Orientation, out hit.Location); Vector3.Add(ref hit.Location, ref shapeTransform.Position, out hit.Location); return(true); }