public abstract GetLocalExtremePointWithoutMargin ( Vector3 &direction, Vector3 &extremePoint ) : void | ||
direction | Vector3 | Direction to find the extreme point in. |
extremePoint | Vector3 | Extreme point on the shape. |
return | void |
/// <summary> /// Computes a convex shape description for a TransformableShape and applies it. /// </summary> public void UpdateConvexShapeInfo() { //Compute the volume distribution. var samples = CommonResources.GetVectorList(); if (samples.Capacity < InertiaHelper.SampleDirections.Length) { samples.Capacity = InertiaHelper.SampleDirections.Length; } samples.Count = InertiaHelper.SampleDirections.Length; for (int i = 0; i < InertiaHelper.SampleDirections.Length; ++i) { shape.GetLocalExtremePointWithoutMargin(ref InertiaHelper.SampleDirections[i], out samples.Elements[i]); } var triangles = CommonResources.GetIntList(); ConvexHullHelper.GetConvexHull(samples, triangles); Fix64 volume; InertiaHelper.ComputeShapeDistribution(samples, triangles, out volume, out volumeDistribution); Volume = volume; //Estimate the minimum radius based on the surface mesh. MinimumRadius = InertiaHelper.ComputeMinimumRadius(samples, triangles, ref Toolbox.ZeroVector) + collisionMargin; MaximumRadius = ComputeMaximumRadius(); CommonResources.GiveBack(samples); CommonResources.GiveBack(triangles); }
///<summary> /// Gets the extreme point of the minkowski difference of shapeA and shapeB in the local space of shapeA. ///</summary> ///<param name="shapeA">First shape.</param> ///<param name="shapeB">Second shape.</param> ///<param name="direction">Extreme point direction in local space.</param> ///<param name="localTransformB">Transform of shapeB in the local space of A.</param> ///<param name="extremePoint">The extreme point in the local space of A.</param> public static void GetLocalMinkowskiExtremePoint(ConvexShape shapeA, ConvexShape shapeB, ref Vector3 direction, ref RigidTransform localTransformB, out Vector3 extremePoint) { //Extreme point of A-B along D = (extreme point of A along D) - (extreme point of B along -D) shapeA.GetLocalExtremePointWithoutMargin(ref direction, out extremePoint); Vector3 v; Vector3 negativeN; Vector3.Negate(ref direction, out negativeN); shapeB.GetExtremePointWithoutMargin(negativeN, ref localTransformB, out v); Vector3.Subtract(ref extremePoint, ref v, out extremePoint); ExpandMinkowskiSum(shapeA.collisionMargin, shapeB.collisionMargin, ref direction, out v); Vector3.Add(ref extremePoint, ref v, out extremePoint); }
///<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> /// Adds a new point to the simplex. ///</summary> ///<param name="shapeA">First shape in the pair.</param> ///<param name="shapeB">Second shape in the pair.</param> ///<param name="iterationCount">Current iteration count.</param> ///<param name="closestPoint">Current point on simplex closest to origin.</param> ///<returns>Whether or not GJK should exit due to a lack of progression.</returns> public bool GetNewSimplexPoint(ConvexShape shapeA, ConvexShape shapeB, int iterationCount, ref Vector3 closestPoint) { Vector3 negativeDirection; Vector3.Negate(ref closestPoint, out negativeDirection); Vector3 sa, sb; shapeA.GetLocalExtremePointWithoutMargin(ref negativeDirection, out sa); shapeB.GetExtremePointWithoutMargin(closestPoint, ref LocalTransformB, out sb); Vector3 S; Vector3.Subtract(ref sa, ref sb, out S); //If S is not further towards the origin along negativeDirection than closestPoint, then we're done. float dotS; Vector3.Dot(ref S, ref negativeDirection, out dotS); //-P * S float distanceToClosest = closestPoint.LengthSquared(); float progression = dotS + distanceToClosest; //It's likely that the system is oscillating between two or more states, usually because of a degenerate simplex. //Rather than detect specific problem cases, this approach just lets it run and catches whatever falls through. //During oscillation, one of the states is usually just BARELY outside of the numerical tolerance. //After a bunch of iterations, the system lets it pick the 'better' one. if (iterationCount > GJKToolbox.HighGJKIterations && distanceToClosest - previousDistanceToClosest < DistanceConvergenceEpsilon * errorTolerance) return true; if (distanceToClosest < previousDistanceToClosest) previousDistanceToClosest = distanceToClosest; //If "A" is the new point always, then the switch statement can be removed //in favor of just pushing three points up. switch (State) { case SimplexState.Point: if (progression <= (errorTolerance = MathHelper.Max(A.LengthSquared(), S.LengthSquared())) * ProgressionEpsilon) return true; State = SimplexState.Segment; B = S; SimplexA.B = sa; SimplexB.B = sb; return false; case SimplexState.Segment: if (progression <= (errorTolerance = MathHelper.Max(MathHelper.Max(A.LengthSquared(), B.LengthSquared()), S.LengthSquared())) * ProgressionEpsilon) return true; State = SimplexState.Triangle; C = S; SimplexA.C = sa; SimplexB.C = sb; return false; case SimplexState.Triangle: if (progression <= (errorTolerance = MathHelper.Max(MathHelper.Max(A.LengthSquared(), B.LengthSquared()), MathHelper.Max(C.LengthSquared(), S.LengthSquared()))) * ProgressionEpsilon) return true; State = SimplexState.Tetrahedron; D = S; SimplexA.D = sa; SimplexB.D = sb; return false; } return false; }
///<summary> /// Gets the extreme point of the minkowski difference of shapeA and shapeB in the local space of shapeA. ///</summary> ///<param name="shapeA">First shape.</param> ///<param name="shapeB">Second shape.</param> ///<param name="direction">Extreme point direction in local space.</param> ///<param name="localTransformB">Transform of shapeB in the local space of A.</param> /// <param name="extremePointA">The extreme point on shapeA.</param> ///<param name="extremePoint">The extreme point in the local space of A.</param> public static void GetLocalMinkowskiExtremePoint(ConvexShape shapeA, ConvexShape shapeB, ref System.Numerics.Vector3 direction, ref RigidTransform localTransformB, out System.Numerics.Vector3 extremePointA, out System.Numerics.Vector3 extremePoint) { //Extreme point of A-B along D = (extreme point of A along D) - (extreme point of B along -D) shapeA.GetLocalExtremePointWithoutMargin(ref direction, out extremePointA); System.Numerics.Vector3 v; Vector3Ex.Negate(ref direction, out v); System.Numerics.Vector3 extremePointB; shapeB.GetExtremePointWithoutMargin(v, ref localTransformB, out extremePointB); ExpandMinkowskiSum(shapeA.collisionMargin, shapeB.collisionMargin, direction, ref extremePointA, ref extremePointB); Vector3Ex.Subtract(ref extremePointA, ref extremePointB, out extremePoint); }
///<summary> /// Gets the extreme point of the minkowski difference of shapeA and shapeB in the local space of shapeA, without a margin. ///</summary> ///<param name="shapeA">First shape.</param> ///<param name="shapeB">Second shape.</param> ///<param name="direction">Extreme point direction in local space.</param> ///<param name="localTransformB">Transform of shapeB in the local space of A.</param> ///<param name="extremePoint">The extreme point in the local space of A.</param> public static void GetLocalMinkowskiExtremePointWithoutMargin(ConvexShape shapeA, ConvexShape shapeB, ref System.Numerics.Vector3 direction, ref RigidTransform localTransformB, out System.Numerics.Vector3 extremePoint) { //Extreme point of A-B along D = (extreme point of A along D) - (extreme point of B along -D) shapeA.GetLocalExtremePointWithoutMargin(ref direction, out extremePoint); System.Numerics.Vector3 extremePointB; System.Numerics.Vector3 negativeN; Vector3Ex.Negate(ref direction, out negativeN); shapeB.GetExtremePointWithoutMargin(negativeN, ref localTransformB, out extremePointB); Vector3Ex.Subtract(ref extremePoint, ref extremePointB, out extremePoint); }