A distance proxy is used by the GJK algorithm. It encapsulates any shape. TODO: see if we can just do assignments with m_vertices, instead of copying stuff over
Beispiel #1
0
        // TODO_ERIN might not need to return the separation
        public float Initialize(Distance.SimplexCache cache, Distance.DistanceProxy proxyA, Sweep sweepA, Distance.DistanceProxy proxyB, Sweep sweepB, float t1)
        {
            ProxyA = proxyA;
            ProxyB = proxyB;
            int count = cache.Count;
            Debug.Assert(0 < count && count < 3);

            SweepA = sweepA;
            SweepB = sweepB;

            SweepA.GetTransform(xfa, t1);
            SweepB.GetTransform(xfb, t1);

            // log.debug("initializing separation.\n" +
            // "cache: "+cache.count+"-"+cache.metric+"-"+cache.indexA+"-"+cache.indexB+"\n"
            // "distance: "+proxyA.

            if (count == 1)
            {
                Type = Type.Points;
                /*
                * Vec2 localPointA = m_proxyA.GetVertex(cache.indexA[0]); Vec2 localPointB =
                * m_proxyB.GetVertex(cache.indexB[0]); Vec2 pointA = Mul(transformA, localPointA); Vec2
                * pointB = Mul(transformB, localPointB); m_axis = pointB - pointA; m_axis.Normalize();
                */
                localPointA.Set(ProxyA.GetVertex(cache.IndexA[0]));
                localPointB.Set(ProxyB.GetVertex(cache.IndexB[0]));
                Transform.MulToOutUnsafe(xfa, localPointA, pointA);
                Transform.MulToOutUnsafe(xfb, localPointB, pointB);
                Axis.Set(pointB).SubLocal(pointA);
                float s = Axis.Normalize();
                return s;
            }
            else if (cache.IndexA[0] == cache.IndexA[1])
            {
                // Two points on B and one on A.
                Type = Type.FaceB;

                localPointB1.Set(ProxyB.GetVertex(cache.IndexB[0]));
                localPointB2.Set(ProxyB.GetVertex(cache.IndexB[1]));

                temp.Set(localPointB2).SubLocal(localPointB1);
                Vec2.CrossToOutUnsafe(temp, 1f, Axis);
                Axis.Normalize();

                Rot.MulToOutUnsafe(xfb.Q, Axis, normal);

                LocalPoint.Set(localPointB1).AddLocal(localPointB2).MulLocal(.5f);
                Transform.MulToOutUnsafe(xfb, LocalPoint, pointB);

                localPointA.Set(proxyA.GetVertex(cache.IndexA[0]));
                Transform.MulToOutUnsafe(xfa, localPointA, pointA);

                temp.Set(pointA).SubLocal(pointB);
                float s = Vec2.Dot(temp, normal);
                if (s < 0.0f)
                {
                    Axis.NegateLocal();
                    s = -s;
                }
                return s;
            }
            else
            {
                // Two points on A and one or two points on B.
                Type = Type.FaceA;

                localPointA1.Set(ProxyA.GetVertex(cache.IndexA[0]));
                localPointA2.Set(ProxyA.GetVertex(cache.IndexA[1]));

                temp.Set(localPointA2).SubLocal(localPointA1);
                Vec2.CrossToOutUnsafe(temp, 1.0f, Axis);
                Axis.Normalize();

                Rot.MulToOutUnsafe(xfa.Q, Axis, normal);

                LocalPoint.Set(localPointA1).AddLocal(localPointA2).MulLocal(.5f);
                Transform.MulToOutUnsafe(xfa, LocalPoint, pointA);

                localPointB.Set(ProxyB.GetVertex(cache.IndexB[0]));
                Transform.MulToOutUnsafe(xfb, localPointB, pointB);

                temp.Set(pointB).SubLocal(pointA);
                float s = Vec2.Dot(temp, normal);
                if (s < 0.0f)
                {
                    Axis.NegateLocal();
                    s = -s;
                }
                return s;
            }
        }
        // TODO_ERIN might not need to return the separation

        public float Initialize(Distance.SimplexCache cache, Distance.DistanceProxy proxyA, Sweep sweepA, Distance.DistanceProxy proxyB, Sweep sweepB, float t1)
        {
            ProxyA = proxyA;
            ProxyB = proxyB;
            int count = cache.Count;

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

            SweepA = sweepA;
            SweepB = sweepB;

            SweepA.GetTransform(xfa, t1);
            SweepB.GetTransform(xfb, t1);

            // log.debug("initializing separation.\n" +
            // "cache: "+cache.count+"-"+cache.metric+"-"+cache.indexA+"-"+cache.indexB+"\n"
            // "distance: "+proxyA.

            if (count == 1)
            {
                Type = Type.Points;

                /*
                 * Vec2 localPointA = m_proxyA.GetVertex(cache.indexA[0]); Vec2 localPointB =
                 * m_proxyB.GetVertex(cache.indexB[0]); Vec2 pointA = Mul(transformA, localPointA); Vec2
                 * pointB = Mul(transformB, localPointB); m_axis = pointB - pointA; m_axis.Normalize();
                 */
                localPointA.Set(ProxyA.GetVertex(cache.IndexA[0]));
                localPointB.Set(ProxyB.GetVertex(cache.IndexB[0]));
                Transform.MulToOutUnsafe(xfa, localPointA, pointA);
                Transform.MulToOutUnsafe(xfb, localPointB, pointB);
                Axis.Set(pointB).SubLocal(pointA);
                float s = Axis.Normalize();
                return(s);
            }
            else if (cache.IndexA[0] == cache.IndexA[1])
            {
                // Two points on B and one on A.
                Type = Type.FaceB;

                localPointB1.Set(ProxyB.GetVertex(cache.IndexB[0]));
                localPointB2.Set(ProxyB.GetVertex(cache.IndexB[1]));

                temp.Set(localPointB2).SubLocal(localPointB1);
                Vec2.CrossToOutUnsafe(temp, 1f, Axis);
                Axis.Normalize();

                Rot.MulToOutUnsafe(xfb.Q, Axis, normal);

                LocalPoint.Set(localPointB1).AddLocal(localPointB2).MulLocal(.5f);
                Transform.MulToOutUnsafe(xfb, LocalPoint, pointB);

                localPointA.Set(proxyA.GetVertex(cache.IndexA[0]));
                Transform.MulToOutUnsafe(xfa, localPointA, pointA);

                temp.Set(pointA).SubLocal(pointB);
                float s = Vec2.Dot(temp, normal);
                if (s < 0.0f)
                {
                    Axis.NegateLocal();
                    s = -s;
                }
                return(s);
            }
            else
            {
                // Two points on A and one or two points on B.
                Type = Type.FaceA;

                localPointA1.Set(ProxyA.GetVertex(cache.IndexA[0]));
                localPointA2.Set(ProxyA.GetVertex(cache.IndexA[1]));

                temp.Set(localPointA2).SubLocal(localPointA1);
                Vec2.CrossToOutUnsafe(temp, 1.0f, Axis);
                Axis.Normalize();

                Rot.MulToOutUnsafe(xfa.Q, Axis, normal);

                LocalPoint.Set(localPointA1).AddLocal(localPointA2).MulLocal(.5f);
                Transform.MulToOutUnsafe(xfa, LocalPoint, pointA);

                localPointB.Set(ProxyB.GetVertex(cache.IndexB[0]));
                Transform.MulToOutUnsafe(xfb, localPointB, pointB);

                temp.Set(pointB).SubLocal(pointA);
                float s = Vec2.Dot(temp, normal);
                if (s < 0.0f)
                {
                    Axis.NegateLocal();
                    s = -s;
                }
                return(s);
            }
        }
Beispiel #3
0
        // TODO_ERIN might not need to return the separation
        public virtual float initialize(Distance.SimplexCache cache, Distance.DistanceProxy proxyA, Sweep sweepA, Distance.DistanceProxy proxyB, Sweep sweepB, float t1)
        {
            m_proxyA = proxyA;
            m_proxyB = proxyB;
            int count = cache.count;
            Debug.Assert(0 < count && count < 3);

            m_sweepA = sweepA;
            m_sweepB = sweepB;

            m_sweepA.getTransform(xfa, t1);
            m_sweepB.getTransform(xfb, t1);

            // log.debug("initializing separation.\n" +
            // "cache: "+cache.count+"-"+cache.metric+"-"+cache.indexA+"-"+cache.indexB+"\n"
            // "distance: "+proxyA.

            if (count == 1)
            {
                m_type = Type.POINTS;
                /*
                * Vec2 localPointA = m_proxyA.GetVertex(cache.indexA[0]); Vec2 localPointB =
                * m_proxyB.GetVertex(cache.indexB[0]); Vec2 pointA = Mul(transformA, localPointA); Vec2
                * pointB = Mul(transformB, localPointB); m_axis = pointB - pointA; m_axis.Normalize();
                */
                localPointA.set_Renamed(m_proxyA.getVertex(cache.indexA[0]));
                localPointB.set_Renamed(m_proxyB.getVertex(cache.indexB[0]));
                Transform.mulToOutUnsafe(xfa, localPointA, pointA);
                Transform.mulToOutUnsafe(xfb, localPointB, pointB);
                m_axis.set_Renamed(pointB).subLocal(pointA);
                float s = m_axis.normalize();
                return s;
            }
            else if (cache.indexA[0] == cache.indexA[1])
            {
                // Two points on B and one on A.
                m_type = Type.FACE_B;

                localPointB1.set_Renamed(m_proxyB.getVertex(cache.indexB[0]));
                localPointB2.set_Renamed(m_proxyB.getVertex(cache.indexB[1]));

                temp.set_Renamed(localPointB2).subLocal(localPointB1);
                Vec2.crossToOutUnsafe(temp, 1f, m_axis);
                m_axis.normalize();

                Rot.mulToOutUnsafe(xfb.q, m_axis, normal);

                m_localPoint.set_Renamed(localPointB1).addLocal(localPointB2).mulLocal(.5f);
                Transform.mulToOutUnsafe(xfb, m_localPoint, pointB);

                localPointA.set_Renamed(proxyA.getVertex(cache.indexA[0]));
                Transform.mulToOutUnsafe(xfa, localPointA, pointA);

                temp.set_Renamed(pointA).subLocal(pointB);
                float s = Vec2.dot(temp, normal);
                if (s < 0.0f)
                {
                    m_axis.negateLocal();
                    s = -s;
                }
                return s;
            }
            else
            {
                // Two points on A and one or two points on B.
                m_type = Type.FACE_A;

                localPointA1.set_Renamed(m_proxyA.getVertex(cache.indexA[0]));
                localPointA2.set_Renamed(m_proxyA.getVertex(cache.indexA[1]));

                temp.set_Renamed(localPointA2).subLocal(localPointA1);
                Vec2.crossToOutUnsafe(temp, 1.0f, m_axis);
                m_axis.normalize();

                Rot.mulToOutUnsafe(xfa.q, m_axis, normal);

                m_localPoint.set_Renamed(localPointA1).addLocal(localPointA2).mulLocal(.5f);
                Transform.mulToOutUnsafe(xfa, m_localPoint, pointA);

                localPointB.set_Renamed(m_proxyB.getVertex(cache.indexB[0]));
                Transform.mulToOutUnsafe(xfb, localPointB, pointB);

                temp.set_Renamed(pointB).subLocal(pointA);
                float s = Vec2.dot(temp, normal);
                if (s < 0.0f)
                {
                    m_axis.negateLocal();
                    s = -s;
                }
                return s;
            }
        }
        /// <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"></param>
        /// <param name="input"></param>
        public void GetTimeOfImpact(TOIOutput output, TOIInput input)
        {
            // CCD via the local separating axis method. This seeks progression
            // by computing the largest time at which separation is maintained.

            ++ToiCalls;

            output.State = TOIOutputState.Unknown;
            output.T     = input.tMax;

            Distance.DistanceProxy proxyA = input.ProxyA;
            Distance.DistanceProxy proxyB = input.ProxyB;

            sweepA.Set(input.SweepA);
            sweepB.Set(input.SweepB);

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

            float tMax = input.tMax;

            float totalRadius = proxyA.Radius + proxyB.Radius;
            // djm: whats with all these constants?
            float       target    = MathUtils.Max(Settings.LINEAR_SLOP, totalRadius - 3.0f * Settings.LINEAR_SLOP);
            const float tolerance = 0.25f * Settings.LINEAR_SLOP;

            Debug.Assert(target > tolerance);

            float t1   = 0f;
            int   iter = 0;

            cache.Count            = 0;
            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 (; ;)
            {
                sweepA.GetTransform(xfA, t1);
                sweepB.GetTransform(xfB, t1);
                // System.out.printf("sweepA: %f, %f, sweepB: %f, %f\n",
                // sweepA.c.x, sweepA.c.y, sweepB.c.x, sweepB.c.y);
                // Get the distance between shapes. We can also use the results
                // to get a separating axis
                distanceInput.TransformA = xfA;
                distanceInput.TransformB = xfB;
                pool.GetDistance().GetDistance(distanceOutput, cache, distanceInput);

                // System.out.printf("Dist: %f at points %f, %f and %f, %f.  %d iterations\n",
                // distanceOutput.distance, distanceOutput.pointA.x, distanceOutput.pointA.y,
                // distanceOutput.pointB.x, distanceOutput.pointB.y,
                // distanceOutput.iterations);

                // If the shapes are overlapped, we give up on continuous collision.
                if (distanceOutput.Distance <= 0f)
                {
                    // System.out.println("failure, overlapped");
                    // Failure!
                    output.State = TOIOutputState.Overlapped;
                    output.T     = 0f;
                    break;
                }

                if (distanceOutput.Distance < target + tolerance)
                {
                    // System.out.println("touching, victory");
                    // Victory!
                    output.State = TOIOutputState.Touching;
                    output.T     = t1;
                    break;
                }

                // Initialize the separating axis.
                fcn.Initialize(cache, proxyA, sweepA, proxyB, 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;
                float t2           = tMax;
                int   pushBackIter = 0;
                for (; ;)
                {
                    // Find the deepest point at t2. Store the witness point indices.
                    float s2 = fcn.FindMinSeparation(indexes, t2);
                    // System.out.printf("s2: %f\n", s2);
                    // Is the final configuration separated?
                    if (s2 > target + tolerance)
                    {
                        // Victory!
                        // System.out.println("separated");
                        output.State = TOIOutputState.Separated;
                        output.T     = tMax;
                        done         = true;
                        break;
                    }

                    // Has the separation reached tolerance?
                    if (s2 > target - tolerance)
                    {
                        // System.out.println("advancing");
                        // Advance the sweeps
                        t1 = t2;
                        break;
                    }

                    // Compute the initial separation of the witness points.
                    float s1 = fcn.Evaluate(indexes[0], indexes[1], t1);
                    // Check for initial overlap. This might happen if the root finder
                    // runs out of iterations.
                    // System.out.printf("s1: %f, target: %f, tolerance: %f\n", s1, target,
                    // tolerance);
                    if (s1 < target - tolerance)
                    {
                        // System.out.println("failed?");
                        output.State = TOIOutputState.Failed;
                        output.T     = t1;
                        done         = true;
                        break;
                    }

                    // Check for touching
                    if (s1 <= target + tolerance)
                    {
                        // System.out.println("touching?");
                        // 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) == 1)
                        {
                            // Secant rule to improve convergence.
                            t = a1 + (target - s1) * (a2 - a1) / (s2 - s1);
                        }
                        else
                        {
                            // Bisection to guarantee progress.
                            t = 0.5f * (a1 + a2);
                        }

                        float s = fcn.Evaluate(indexes[0], indexes[1], t);

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

                        ++rootIterCount;
                        ++ToiRootIters;

                        // djm: whats with this? put in settings?
                        if (rootIterCount == 50)
                        {
                            break;
                        }
                    }

                    ToiMaxRootIters = MathUtils.Max(ToiMaxRootIters, rootIterCount);

                    ++pushBackIter;

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

                ++iter;
                ++ToiIters;

                if (done)
                {
                    // System.out.println("done");
                    break;
                }

                if (iter == MAX_ITERATIONS)
                {
                    // System.out.println("failed, root finder stuck");
                    // Root finder got stuck. Semi-victory.
                    output.State = TOIOutputState.Failed;
                    output.T     = t1;
                    break;
                }
            }

            // System.out.printf("final sweeps: %f, %f, %f; %f, %f, %f", input.s)
            ToiMaxIters = MathUtils.Max(ToiMaxIters, iter);
        }