axmol/Box2D/Collision/b2TimeOfImpact.cpp

/*
* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org
*
* This software is provided 'as-is', without any express or implied
* warranty.  In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/

#include <Box2D/Collision/b2Collision.h>
#include <Box2D/Collision/b2Distance.h>
#include <Box2D/Collision/b2TimeOfImpact.h>
#include <Box2D/Collision/Shapes/b2CircleShape.h>
#include <Box2D/Collision/Shapes/b2PolygonShape.h>

#ifdef SHP
#include <stdio.h>
#else
#include <cstdio>
#endif

using namespace std;

int32 b2_toiCalls, b2_toiIters, b2_toiMaxIters;
int32 b2_toiRootIters, b2_toiMaxRootIters;

struct b2SeparationFunction
{
    enum Type
    {
        e_points,
        e_faceA,
        e_faceB
    };

    // TODO_ERIN might not need to return the separation

    float32 Initialize(const b2SimplexCache* cache,
        const b2DistanceProxy* proxyA, const b2Sweep& sweepA,
        const b2DistanceProxy* proxyB, const b2Sweep& sweepB,
        float32 t1)
    {
        m_proxyA = proxyA;
        m_proxyB = proxyB;
        int32 count = cache->count;
        b2Assert(0 < count && count < 3);

        m_sweepA = sweepA;
        m_sweepB = sweepB;

        b2Transform xfA, xfB;
        m_sweepA.GetTransform(&xfA, t1);
        m_sweepB.GetTransform(&xfB, t1);

        if (count == 1)
        {
            m_type = e_points;
            b2Vec2 localPointA = m_proxyA->GetVertex(cache->indexA[0]);
            b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]);
            b2Vec2 pointA = b2Mul(xfA, localPointA);
            b2Vec2 pointB = b2Mul(xfB, localPointB);
            m_axis = pointB - pointA;
            float32 s = m_axis.Normalize();
            return s;
        }
        else if (cache->indexA[0] == cache->indexA[1])
        {
            // Two points on B and one on A.
            m_type = e_faceB;
            b2Vec2 localPointB1 = proxyB->GetVertex(cache->indexB[0]);
            b2Vec2 localPointB2 = proxyB->GetVertex(cache->indexB[1]);

            m_axis = b2Cross(localPointB2 - localPointB1, 1.0f);
            m_axis.Normalize();
            b2Vec2 normal = b2Mul(xfB.q, m_axis);

            m_localPoint = 0.5f * (localPointB1 + localPointB2);
            b2Vec2 pointB = b2Mul(xfB, m_localPoint);

            b2Vec2 localPointA = proxyA->GetVertex(cache->indexA[0]);
            b2Vec2 pointA = b2Mul(xfA, localPointA);

            float32 s = b2Dot(pointA - pointB, normal);
            if (s < 0.0f)
            {
                m_axis = -m_axis;
                s = -s;
            }
            return s;
        }
        else
        {
            // Two points on A and one or two points on B.
            m_type = e_faceA;
            b2Vec2 localPointA1 = m_proxyA->GetVertex(cache->indexA[0]);
            b2Vec2 localPointA2 = m_proxyA->GetVertex(cache->indexA[1]);
            
            m_axis = b2Cross(localPointA2 - localPointA1, 1.0f);
            m_axis.Normalize();
            b2Vec2 normal = b2Mul(xfA.q, m_axis);

            m_localPoint = 0.5f * (localPointA1 + localPointA2);
            b2Vec2 pointA = b2Mul(xfA, m_localPoint);

            b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]);
            b2Vec2 pointB = b2Mul(xfB, localPointB);

            float32 s = b2Dot(pointB - pointA, normal);
            if (s < 0.0f)
            {
                m_axis = -m_axis;
                s = -s;
            }
            return s;
        }
    }

    float32 FindMinSeparation(int32* indexA, int32* indexB, float32 t) const
    {
        b2Transform xfA, xfB;
        m_sweepA.GetTransform(&xfA, t);
        m_sweepB.GetTransform(&xfB, t);

        switch (m_type)
        {
        case e_points:
            {
                b2Vec2 axisA = b2MulT(xfA.q,  m_axis);
                b2Vec2 axisB = b2MulT(xfB.q, -m_axis);

                *indexA = m_proxyA->GetSupport(axisA);
                *indexB = m_proxyB->GetSupport(axisB);

                b2Vec2 localPointA = m_proxyA->GetVertex(*indexA);
                b2Vec2 localPointB = m_proxyB->GetVertex(*indexB);
                
                b2Vec2 pointA = b2Mul(xfA, localPointA);
                b2Vec2 pointB = b2Mul(xfB, localPointB);

                float32 separation = b2Dot(pointB - pointA, m_axis);
                return separation;
            }

        case e_faceA:
            {
                b2Vec2 normal = b2Mul(xfA.q, m_axis);
                b2Vec2 pointA = b2Mul(xfA, m_localPoint);

                b2Vec2 axisB = b2MulT(xfB.q, -normal);
                
                *indexA = -1;
                *indexB = m_proxyB->GetSupport(axisB);

                b2Vec2 localPointB = m_proxyB->GetVertex(*indexB);
                b2Vec2 pointB = b2Mul(xfB, localPointB);

                float32 separation = b2Dot(pointB - pointA, normal);
                return separation;
            }

        case e_faceB:
            {
                b2Vec2 normal = b2Mul(xfB.q, m_axis);
                b2Vec2 pointB = b2Mul(xfB, m_localPoint);

                b2Vec2 axisA = b2MulT(xfA.q, -normal);

                *indexB = -1;
                *indexA = m_proxyA->GetSupport(axisA);

                b2Vec2 localPointA = m_proxyA->GetVertex(*indexA);
                b2Vec2 pointA = b2Mul(xfA, localPointA);

                float32 separation = b2Dot(pointA - pointB, normal);
                return separation;
            }

        default:
            b2Assert(false);
            *indexA = -1;
            *indexB = -1;
            return 0.0f;
        }
    }

    float32 Evaluate(int32 indexA, int32 indexB, float32 t) const
    {
        b2Transform xfA, xfB;
        m_sweepA.GetTransform(&xfA, t);
        m_sweepB.GetTransform(&xfB, t);

        switch (m_type)
        {
        case e_points:
            {
                b2Vec2 axisA = b2MulT(xfA.q,  m_axis);
                b2Vec2 axisB = b2MulT(xfB.q, -m_axis);

                b2Vec2 localPointA = m_proxyA->GetVertex(indexA);
                b2Vec2 localPointB = m_proxyB->GetVertex(indexB);

                b2Vec2 pointA = b2Mul(xfA, localPointA);
                b2Vec2 pointB = b2Mul(xfB, localPointB);
                float32 separation = b2Dot(pointB - pointA, m_axis);

                return separation;
            }

        case e_faceA:
            {
                b2Vec2 normal = b2Mul(xfA.q, m_axis);
                b2Vec2 pointA = b2Mul(xfA, m_localPoint);

                b2Vec2 axisB = b2MulT(xfB.q, -normal);

                b2Vec2 localPointB = m_proxyB->GetVertex(indexB);
                b2Vec2 pointB = b2Mul(xfB, localPointB);

                float32 separation = b2Dot(pointB - pointA, normal);
                return separation;
            }

        case e_faceB:
            {
                b2Vec2 normal = b2Mul(xfB.q, m_axis);
                b2Vec2 pointB = b2Mul(xfB, m_localPoint);

                b2Vec2 axisA = b2MulT(xfA.q, -normal);

                b2Vec2 localPointA = m_proxyA->GetVertex(indexA);
                b2Vec2 pointA = b2Mul(xfA, localPointA);

                float32 separation = b2Dot(pointA - pointB, normal);
                return separation;
            }

        default:
            b2Assert(false);
            return 0.0f;
        }
    }

    const b2DistanceProxy* m_proxyA;
    const b2DistanceProxy* m_proxyB;
    b2Sweep m_sweepA, m_sweepB;
    Type m_type;
    b2Vec2 m_localPoint;
    b2Vec2 m_axis;
};

// CCD via the local separating axis method. This seeks progression
// by computing the largest time at which separation is maintained.
void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input)
{
    ++b2_toiCalls;

    output->state = b2TOIOutput::e_unknown;
    output->t = input->tMax;

    const b2DistanceProxy* proxyA = &input->proxyA;
    const b2DistanceProxy* proxyB = &input->proxyB;

    b2Sweep sweepA = input->sweepA;
    b2Sweep sweepB = input->sweepB;

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

    float32 tMax = input->tMax;

    float32 totalRadius = proxyA->m_radius + proxyB->m_radius;
    float32 target = b2Max(b2_linearSlop, totalRadius - 3.0f * b2_linearSlop);
    float32 tolerance = 0.25f * b2_linearSlop;
    b2Assert(target > tolerance);

    float32 t1 = 0.0f;
    const int32 k_maxIterations = 20;    // TODO_ERIN b2Settings
    int32 iter = 0;

    // Prepare input for distance query.
    b2SimplexCache cache;
    cache.count = 0;
    b2DistanceInput 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(;;)
    {
        b2Transform xfA, xfB;
        sweepA.GetTransform(&xfA, t1);
        sweepB.GetTransform(&xfB, t1);

        // Get the distance between shapes. We can also use the results
        // to get a separating axis.
        distanceInput.transformA = xfA;
        distanceInput.transformB = xfB;
        b2DistanceOutput distanceOutput;
        b2Distance(&distanceOutput, &cache, &distanceInput);

        // If the shapes are overlapped, we give up on continuous collision.
        if (distanceOutput.distance <= 0.0f)
        {
            // Failure!
            output->state = b2TOIOutput::e_overlapped;
            output->t = 0.0f;
            break;
        }

        if (distanceOutput.distance < target + tolerance)
        {
            // Victory!
            output->state = b2TOIOutput::e_touching;
            output->t = t1;
            break;
        }

        // Initialize the separating axis.
        b2SeparationFunction fcn;
        fcn.Initialize(&cache, proxyA, sweepA, proxyB, sweepB, t1);
#if 0
        // Dump the curve seen by the root finder
        {
            const int32 N = 100;
            float32 dx = 1.0f / N;
            float32 xs[N+1];
            float32 fs[N+1];

            float32 x = 0.0f;

            for (int32 i = 0; i <= N; ++i)
            {
                sweepA.GetTransform(&xfA, x);
                sweepB.GetTransform(&xfB, x);
                float32 f = fcn.Evaluate(xfA, xfB) - target;

                printf("%g %g\n", x, f);

                xs[i] = x;
                fs[i] = f;

                x += dx;
            }
        }
#endif

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

            // Is the final configuration separated?
            if (s2 > target + tolerance)
            {
                // Victory!
                output->state = b2TOIOutput::e_separated;
                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.
            float32 s1 = fcn.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 = b2TOIOutput::e_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 = b2TOIOutput::e_touching;
                output->t = t1;
                done = true;
                break;
            }

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

                float32 s = fcn.Evaluate(indexA, indexB, t);

                if (b2Abs(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;
                ++b2_toiRootIters;

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

            b2_toiMaxRootIters = b2Max(b2_toiMaxRootIters, rootIterCount);

            ++pushBackIter;

            if (pushBackIter == b2_maxPolygonVertices)
            {
                break;
            }
        }

        ++iter;
        ++b2_toiIters;

        if (done)
        {
            break;
        }

        if (iter == k_maxIterations)
        {
            // Root finder got stuck. Semi-victory.
            output->state = b2TOIOutput::e_failed;
            output->t = t1;
            break;
        }
    }

    b2_toiMaxIters = b2Max(b2_toiMaxIters, iter);
}