コード例 #1
0
        /// <summary>
        ///     Evaluates the index a
        /// </summary>
        /// <param name="indexA">The index</param>
        /// <param name="indexB">The index</param>
        /// <param name="t">The </param>
        /// <param name="proxyA">The proxy</param>
        /// <param name="sweepA">The sweep</param>
        /// <param name="proxyB">The proxy</param>
        /// <param name="sweepB">The sweep</param>
        /// <param name="axis">The axis</param>
        /// <param name="localPoint">The local point</param>
        /// <param name="type">The type</param>
        /// <returns>The float</returns>
        public static float Evaluate(int indexA, int indexB, float t, DistanceProxy proxyA, ref Sweep sweepA,
                                     DistanceProxy proxyB, ref Sweep sweepB, ref Vector2 axis, ref Vector2 localPoint,
                                     SeparationFunctionType type)
        {
            sweepA.GetTransform(out Transform xfA, t);
            sweepB.GetTransform(out Transform xfB, t);

            switch (type)
            {
            case SeparationFunctionType.Points:
            {
                Vector2 localPointA = proxyA.Vertices[indexA];
                Vector2 localPointB = proxyB.Vertices[indexB];

                Vector2 pointA     = MathUtils.Mul(ref xfA, localPointA);
                Vector2 pointB     = MathUtils.Mul(ref xfB, localPointB);
                float   separation = Vector2.Dot(pointB - pointA, axis);

                return(separation);
            }

            case SeparationFunctionType.FaceA:
            {
                Vector2 normal = MathUtils.Mul(ref xfA.Q, axis);
                Vector2 pointA = MathUtils.Mul(ref xfA, localPoint);

                Vector2 localPointB = proxyB.Vertices[indexB];
                Vector2 pointB      = MathUtils.Mul(ref xfB, localPointB);

                float separation = Vector2.Dot(pointB - pointA, normal);
                return(separation);
            }

            case SeparationFunctionType.FaceB:
            {
                Vector2 normal = MathUtils.Mul(ref xfB.Q, axis);
                Vector2 pointB = MathUtils.Mul(ref xfB, localPoint);

                Vector2 localPointA = proxyA.Vertices[indexA];
                Vector2 pointA      = MathUtils.Mul(ref xfA, localPointA);

                float separation = Vector2.Dot(pointA - pointB, normal);
                return(separation);
            }

            default:
                Debug.Assert(false);
                return(0.0f);
            }
        }
コード例 #2
0
        /// <summary>
        ///     Initializes the cache
        /// </summary>
        /// <param name="cache">The cache</param>
        /// <param name="proxyA">The proxy</param>
        /// <param name="sweepA">The sweep</param>
        /// <param name="proxyB">The proxy</param>
        /// <param name="sweepB">The sweep</param>
        /// <param name="t1">The </param>
        /// <param name="axis">The axis</param>
        /// <param name="localPoint">The local point</param>
        /// <param name="type">The type</param>
        public static void Initialize(ref SimplexCache cache, DistanceProxy proxyA, ref Sweep sweepA,
                                      DistanceProxy proxyB, ref Sweep sweepB, float t1, out Vector2 axis, out Vector2 localPoint,
                                      out SeparationFunctionType type)
        {
            int count = cache.Count;

            Debug.Assert(0 < count && count < 3);

            sweepA.GetTransform(out Transform xfA, t1);
            sweepB.GetTransform(out Transform xfB, t1);

            if (count == 1)
            {
                localPoint = Vector2.Zero;
                type       = SeparationFunctionType.Points;
                Vector2 localPointA = proxyA.Vertices[cache.IndexA[0]];
                Vector2 localPointB = proxyB.Vertices[cache.IndexB[0]];
                Vector2 pointA      = MathUtils.Mul(ref xfA, localPointA);
                Vector2 pointB      = MathUtils.Mul(ref xfB, localPointB);
                axis = pointB - pointA;
                axis = Vector2.Normalize(axis);
            }
            else if (cache.IndexA[0] == cache.IndexA[1])
            {
                // Two points on B and one on A.
                type = SeparationFunctionType.FaceB;
                Vector2 localPointB1 = proxyB.Vertices[cache.IndexB[0]];
                Vector2 localPointB2 = proxyB.Vertices[cache.IndexB[1]];

                Vector2 a = localPointB2 - localPointB1;
                axis = new Vector2(a.Y, -a.X);
                axis = Vector2.Normalize(axis);
                Vector2 normal = MathUtils.Mul(ref xfB.Q, axis);

                localPoint = 0.5f * (localPointB1 + localPointB2);
                Vector2 pointB = MathUtils.Mul(ref xfB, localPoint);

                Vector2 localPointA = proxyA.Vertices[cache.IndexA[0]];
                Vector2 pointA      = MathUtils.Mul(ref xfA, localPointA);

                float s = Vector2.Dot(pointA - pointB, normal);
                if (s < 0.0f)
                {
                    axis = -axis;
                }
            }
            else
            {
                // Two points on A and one or two points on B.
                type = SeparationFunctionType.FaceA;
                Vector2 localPointA1 = proxyA.Vertices[cache.IndexA[0]];
                Vector2 localPointA2 = proxyA.Vertices[cache.IndexA[1]];

                Vector2 a = localPointA2 - localPointA1;
                axis = new Vector2(a.Y, -a.X);
                axis = Vector2.Normalize(axis);
                Vector2 normal = MathUtils.Mul(ref xfA.Q, axis);

                localPoint = 0.5f * (localPointA1 + localPointA2);
                Vector2 pointA = MathUtils.Mul(ref xfA, localPoint);

                Vector2 localPointB = proxyB.Vertices[cache.IndexB[0]];
                Vector2 pointB      = MathUtils.Mul(ref xfB, localPointB);

                float s = Vector2.Dot(pointB - pointA, normal);
                if (s < 0.0f)
                {
                    axis = -axis;
                }
            }

            //Velcro note: the returned value that used to be here has been removed, as it was not used.
        }
コード例 #3
0
        /// <summary>
        ///     Compute the upper bound on time before two shapes penetrate. Time is represented as a fraction between
        ///     [0,tMax]. This uses a swept separating axis and may miss some intermediate, non-tunneling collision. If you change
        ///     the
        ///     time interval, you should call this function again. Note: use Distance() to compute the contact point and normal at
        ///     the
        ///     time of impact.
        /// </summary>
        /// <param name="input">The input.</param>
        /// <param name="output">The output.</param>
        public static void CalculateTimeOfImpact(ref ToiInput input, out ToiOutput output)
        {
            ++ToiCalls;

            output = new ToiOutput
            {
                State = ToiOutputState.Unknown,
                T     = input.Max
            };

            Sweep sweepA = input.SweepA;
            Sweep sweepB = input.SweepB;

            // Large rotations can make the root finder fail, so we normalize the
            // sweep angles.
            sweepA.Normalize();
            sweepB.Normalize();

            float tMax = input.Max;

            float totalRadius = input.ProxyA.Radius + input.ProxyB.Radius;
            float target      = Math.Max(Settings.LinearSlop, totalRadius - 3.0f * Settings.LinearSlop);
            float tolerance   = 0.25f * Settings.LinearSlop;

            Debug.Assert(target > tolerance);

            float     t1             = 0.0f;
            const int kMaxIterations = 20;
            int       iter           = 0;

            // Prepare input for distance query.
            DistanceInput distanceInput = new DistanceInput
            {
                ProxyA   = input.ProxyA,
                ProxyB   = input.ProxyB,
                UseRadii = false
            };

            // The outer loop progressively attempts to compute new separating axes.
            // This loop terminates when an axis is repeated (no progress is made).
            for (;;)
            {
                sweepA.GetTransform(out Transform xfA, t1);
                sweepB.GetTransform(out Transform xfB, t1);

                // Get the distance between shapes. We can also use the results
                // to get a separating axis.
                distanceInput.TransformA = xfA;
                distanceInput.TransformB = xfB;
                DistanceGjk.ComputeDistance(ref distanceInput, out DistanceOutput distanceOutput,
                                            out SimplexCache cache);

                // If the shapes are overlapped, we give up on continuous collision.
                if (distanceOutput.Distance <= 0.0f)
                {
                    // Failure!
                    output.State = ToiOutputState.Overlapped;
                    output.T     = 0.0f;
                    break;
                }

                if (distanceOutput.Distance < target + tolerance)
                {
                    // Victory!
                    output.State = ToiOutputState.Touching;
                    output.T     = t1;
                    break;
                }

                SeparationFunction.Initialize(ref cache, input.ProxyA, ref sweepA, input.ProxyB, ref sweepB, t1,
                                              out Vector2 axis, out Vector2 localPoint, out SeparationFunctionType type);

                // Compute the TOI on the separating axis. We do this by successively
                // resolving the deepest point. This loop is bounded by the number of vertices.
                bool  done         = false;
                float t2           = tMax;
                int   pushBackIter = 0;
                for (;;)
                {
                    // Find the deepest point at t2. Store the witness point indices.
                    float s2 = SeparationFunction.FindMinSeparation(out int indexA, out int indexB, t2, input.ProxyA,
                                                                    ref sweepA, input.ProxyB, ref sweepB, ref axis, ref localPoint, type);

                    // Is the final configuration separated?
                    if (s2 > target + tolerance)
                    {
                        // Victory!
                        output.State = ToiOutputState.Seperated;
                        output.T     = tMax;
                        done         = true;
                        break;
                    }

                    // Has the separation reached tolerance?
                    if (s2 > target - tolerance)
                    {
                        // Advance the sweeps
                        t1 = t2;
                        break;
                    }

                    // Compute the initial separation of the witness points.
                    float s1 = SeparationFunction.Evaluate(indexA, indexB, t1, input.ProxyA, ref sweepA, input.ProxyB,
                                                           ref sweepB, ref axis, ref localPoint, type);

                    // Check for initial overlap. This might happen if the root finder
                    // runs out of iterations.
                    if (s1 < target - tolerance)
                    {
                        output.State = ToiOutputState.Failed;
                        output.T     = t1;
                        done         = true;
                        break;
                    }

                    // Check for touching
                    if (s1 <= target + tolerance)
                    {
                        // Victory! t1 should hold the TOI (could be 0.0).
                        output.State = ToiOutputState.Touching;
                        output.T     = t1;
                        done         = true;
                        break;
                    }

                    // Compute 1D root of: f(x) - target = 0
                    int   rootIterCount = 0;
                    float a1 = t1, a2 = t2;
                    for (;;)
                    {
                        // Use a mix of the secant rule and bisection.
                        float t;
                        if ((rootIterCount & 1) != 0)
                        {
                            // Secant rule to improve convergence.
                            t = a1 + (target - s1) * (a2 - a1) / (s2 - s1);
                        }
                        else
                        {
                            // Bisection to guarantee progress.
                            t = 0.5f * (a1 + a2);
                        }

                        ++rootIterCount;
                        ++ToiRootIters;

                        float s = SeparationFunction.Evaluate(indexA, indexB, t, input.ProxyA, ref sweepA, input.ProxyB,
                                                              ref sweepB, ref axis, ref localPoint, type);

                        if (Math.Abs(s - target) < tolerance)
                        {
                            // t2 holds a tentative value for t1
                            t2 = t;
                            break;
                        }

                        // Ensure we continue to bracket the root.
                        if (s > target)
                        {
                            a1 = t;
                            s1 = s;
                        }
                        else
                        {
                            a2 = t;
                            s2 = s;
                        }

                        if (rootIterCount == 50)
                        {
                            break;
                        }
                    }

                    ToiMaxRootIters = Math.Max(ToiMaxRootIters, rootIterCount);

                    ++pushBackIter;

                    if (pushBackIter == Settings.MaxPolygonVertices)
                    {
                        break;
                    }
                }

                ++iter;
                ++ToiIters;

                if (done)
                {
                    break;
                }

                if (iter == kMaxIterations)
                {
                    // Root finder got stuck. Semi-victory.
                    output.State = ToiOutputState.Failed;
                    output.T     = t1;
                    break;
                }
            }

            ToiMaxIters = Math.Max(ToiMaxIters, iter);
        }