Beispiel #1
0
        public static void CalculateTimeOfImpact(out TOIOutput output, TOIInput input)
        {
            output       = default(TOIOutput);
            output.State = TOIOutputState.Unknown;
            output.T     = input.TMax;
            Sweep sweepA = input.SweepA;
            Sweep sweepB = input.SweepB;

            sweepA.Normalize();
            sweepB.Normalize();
            FP tMax = input.TMax;
            FP x    = input.ProxyA.Radius + input.ProxyB.Radius;
            FP fP   = TSMath.Max(Settings.LinearSlop, x - 3f * Settings.LinearSlop);
            FP y    = 0.25f * Settings.LinearSlop;

            Debug.Assert(fP > y);
            FP  fP2 = 0f;
            int num = 0;

            TimeOfImpact._distanceInput          = (TimeOfImpact._distanceInput ?? new DistanceInput());
            TimeOfImpact._distanceInput.ProxyA   = input.ProxyA;
            TimeOfImpact._distanceInput.ProxyB   = input.ProxyB;
            TimeOfImpact._distanceInput.UseRadii = false;
            while (true)
            {
                Transform transformA;
                sweepA.GetTransform(out transformA, fP2);
                Transform transformB;
                sweepB.GetTransform(out transformB, fP2);
                TimeOfImpact._distanceInput.TransformA = transformA;
                TimeOfImpact._distanceInput.TransformB = transformB;
                DistanceOutput distanceOutput;
                SimplexCache   simplexCache;
                Distance.ComputeDistance(out distanceOutput, out simplexCache, TimeOfImpact._distanceInput);
                bool flag = distanceOutput.Distance <= 0f;
                if (flag)
                {
                    break;
                }
                bool flag2 = distanceOutput.Distance < fP + y;
                if (flag2)
                {
                    goto Block_3;
                }
                SeparationFunction.Set(ref simplexCache, input.ProxyA, ref sweepA, input.ProxyB, ref sweepB, fP2);
                bool flag3 = false;
                FP   fP3   = tMax;
                int  num2  = 0;
                while (true)
                {
                    int  indexA;
                    int  indexB;
                    FP   x2    = SeparationFunction.FindMinSeparation(out indexA, out indexB, fP3);
                    bool flag4 = x2 > fP + y;
                    if (flag4)
                    {
                        goto Block_4;
                    }
                    bool flag5 = x2 > fP - y;
                    if (flag5)
                    {
                        goto Block_5;
                    }
                    FP   fP4   = SeparationFunction.Evaluate(indexA, indexB, fP2);
                    bool flag6 = fP4 < fP - y;
                    if (flag6)
                    {
                        goto Block_6;
                    }
                    bool flag7 = fP4 <= fP + y;
                    if (flag7)
                    {
                        goto Block_7;
                    }
                    int  num3 = 0;
                    FP   fP5  = fP2;
                    FP   fP6  = fP3;
                    FP   fP7;
                    bool flag11;
                    do
                    {
                        bool flag8 = (num3 & 1) != 0;
                        if (flag8)
                        {
                            fP7 = fP5 + (fP - fP4) * (fP6 - fP5) / (x2 - fP4);
                        }
                        else
                        {
                            fP7 = 0.5f * (fP5 + fP6);
                        }
                        num3++;
                        FP   fP8   = SeparationFunction.Evaluate(indexA, indexB, fP7);
                        bool flag9 = FP.Abs(fP8 - fP) < y;
                        if (flag9)
                        {
                            goto Block_9;
                        }
                        bool flag10 = fP8 > fP;
                        if (flag10)
                        {
                            fP5 = fP7;
                            fP4 = fP8;
                        }
                        else
                        {
                            fP6 = fP7;
                            x2  = fP8;
                        }
                        flag11 = (num3 == 50);
                    }while (!flag11);
IL_373:
                    num2++;
                    bool flag12 = num2 == Settings.MaxPolygonVertices;
                    if (flag12)
                    {
                        break;
                    }
                    continue;
Block_9:
                    fP3 = fP7;
                    goto IL_373;
                }
IL_396:
                num++;
                bool flag13 = flag3;
                if (flag13)
                {
                    goto Block_13;
                }
                bool flag14 = num == 20;
                if (flag14)
                {
                    goto Block_14;
                }
                continue;
Block_4:
                output.State = TOIOutputState.Seperated;
                output.T     = tMax;
                flag3        = true;
                goto IL_396;
Block_5:
                fP2 = fP3;
                goto IL_396;
Block_6:
                output.State = TOIOutputState.Failed;
                output.T     = fP2;
                flag3        = true;
                goto IL_396;
Block_7:
                output.State = TOIOutputState.Touching;
                output.T     = fP2;
                flag3        = true;
                goto IL_396;
            }
            output.State = TOIOutputState.Overlapped;
            output.T     = 0f;
            return;

Block_3:
            output.State = TOIOutputState.Touching;
            output.T     = fP2;
Block_13:
            return;

Block_14:
            output.State = TOIOutputState.Failed;
            output.T     = fP2;
        }
        /// <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="output">The output.</param>
        /// <param name="input">The input.</param>
        public static void CalculateTimeOfImpact(out TOIOutput output, TOIInput input)
        {
            if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled
            {
                ++TOICalls;
            }

            output       = new TOIOutput();
            output.State = TOIOutputState.Unknown;
            output.T     = input.TMax;

            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();

            FP tMax = input.TMax;

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

            Debug.Assert(target > tolerance);

            FP        t1 = 0.0f;
            const int k_maxIterations = 20;
            int       iter            = 0;

            // Prepare input for distance query.
            _distanceInput          = _distanceInput ?? new DistanceInput();
            _distanceInput.ProxyA   = input.ProxyA;
            _distanceInput.ProxyB   = input.ProxyB;
            _distanceInput.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 (; ;)
            {
                Transform xfA, xfB;
                sweepA.GetTransform(out xfA, t1);
                sweepB.GetTransform(out xfB, t1);

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

                // 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.Set(ref cache, input.ProxyA, ref sweepA, input.ProxyB, ref sweepB, t1);

                // 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;
                FP   t2           = tMax;
                int  pushBackIter = 0;
                for (; ;)
                {
                    // Find the deepest point at t2. Store the witness point indices.
                    int indexA, indexB;
                    FP  s2 = SeparationFunction.FindMinSeparation(out indexA, out indexB, t2);

                    // 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.
                    FP s1 = SeparationFunction.Evaluate(indexA, indexB, t1);

                    // 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;
                    FP  a1 = t1, a2 = t2;
                    for (; ;)
                    {
                        // Use a mix of the secant rule and bisection.
                        FP 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;

                        if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled
                        {
                            ++TOIRootIters;
                        }

                        FP s = SeparationFunction.Evaluate(indexA, indexB, t);

                        if (FP.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;
                        }
                    }

                    if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled
                    {
                        TOIMaxRootIters = Math.Max(TOIMaxRootIters, rootIterCount);
                    }

                    ++pushBackIter;

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

                ++iter;

                if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled
                {
                    ++TOIIters;
                }

                if (done)
                {
                    break;
                }

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

            if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled
            {
                TOIMaxIters = Math.Max(TOIMaxIters, iter);
            }
        }