laCollider.cpp

/*

Jump'n'Run Engine
http://www.atanaslaskov.com/jnr/

BSD LICENSE
Copyright (c) 2007-2013, Atanas Laskov
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice,
this list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL ATANAS LASKOV BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

//
// FILE: laCollider.cpp
// Collision detection
//
// Copyright (C) 2007-2013 Atanas Laskov, <latanas@gmail.com>
//
#include "stdafx.h"
#include "Core.h"

// Find the distance to the intersection and the intersection point
// between a moving line and a static line
//
M_BOOL laCollider::intersection(const laLine2 &lnMovement, const laLine2 &lnCollision,
                                    double *pDistance, laPoint2 *pIntersection)
{
    PROFILE_COL(laCollider_intersection);
    double k, k_ln;

    // Is there an intersection?
    //
    if( lnMovement.intersection(lnCollision, &k, &k_ln) )
    {
        //Get the point of intersection
        *pIntersection = lnMovement.at(k);

        // Is the intersection within the bonds of the two line segments ?
        //
        if( (k>=0) && (k<=1) && (k_ln>=0) && (k_ln<=1) )
        {
            *pDistance = k * lnMovement.lenght_sq();
            return M_TRUE;
        }
    }

    return M_FALSE;
}


// Project velocity on collision geometry
//
void laCollider::response(const laPoint2 &position, const laPoint2 &vector, const laLine2 &lnCollisionCauser,
                             laPoint2* pModifiedVector)
{
    PROFILE_COL(laCollider_response);
    static double k, k_ln;

    //Project the destination point on the collision line
    //
    laPoint2 ptDestination = position + vector;
    laPoint2 normal =  lnCollisionCauser.normal();
    laLine2 lnNormalProjection(ptDestination, ptDestination + normal);

    lnCollisionCauser.intersection(lnNormalProjection, &k, &k_ln);
    laPoint2 ptIntersection = lnCollisionCauser.at(k);

    //Modify the movement vector
    *pModifiedVector = ptIntersection - position;
    //*pModifiedVector += normal * M_NORMAL_RESPONSE;
}

inline void safe_magin(const laLine2 &lnPositioned, laCollisionScenario *pScenario)
{
    PROFILE_COL(laCollider_safe_magin);
    laCollisionDomain *pDomain = pScenario->pFirstDomain;
    unsigned nCurrentDomain = 0, i;

    laRect2 rectBoundingLine, rectBoundingDomain;
    laLine2 *pDomainLine;

    static double d1, d2, d3, d4, d_min;
    static laPoint2  pt;

    rectBoundingLine.add( lnPositioned );
    rectBoundingLine.inflate( pScenario->ptModifiedVector );
    rectBoundingLine.inflate( laPoint2(M_NORMAL_RESPONSE, M_NORMAL_RESPONSE) );
    rectBoundingLine.inflate( laPoint2(-M_NORMAL_RESPONSE, -M_NORMAL_RESPONSE) );

    //Cylce through all collision domains
    //
    do{
        if( rectBoundingLine.intersecting( pDomain->boundingRect() ) )  //Quick test for the domain
        for(i=0; i<pDomain->countLines(); i++)
        {
            pDomainLine = pDomain->line(i);

            // Ignore backfacing
            //
            pt = pDomainLine->origin + *(pDomain->normal(i));

            if( (! pDomainLine->onSameSide(pt, lnPositioned.origin)) && (! pDomainLine->onSameSide(pt, lnPositioned.end())) )
                continue;

            // Safe margin
            //
            laLine2 lnMagin(lnPositioned.origin + pScenario->ptModifiedVector, lnPositioned.end() + pScenario->ptModifiedVector);
            laLine2 lnMagin2(pDomainLine->origin - pScenario->ptModifiedVector, pDomainLine->end() - pScenario->ptModifiedVector);
            d1 = pDomainLine->distance_sq( lnMagin.origin );
            d2 = pDomainLine->distance_sq( lnMagin.end() );
            d3 = lnPositioned.distance_sq( lnMagin2.origin );
            d4 = lnPositioned.distance_sq( lnMagin2.end() );
            d_min = sqrt( M_MIN(d1, M_MIN(d2, M_MIN(d3, d4))) );

            if( d_min < M_NORMAL_RESPONSE )
                pScenario->ptModifiedVector += *(pDomain->normal(i)) * ( M_NORMAL_RESPONSE - d_min );
        }
    } while( (pDomain = pDomain->nextDomain()) && (++nCurrentDomain < pScenario->nDomains) );
}


// Collision detection between a single line and a list of collision domains
//
void laCollider::collideLineWithDomain(const laLine2 &line, laCollisionScenario *pScenario,
                                          int nIterations, M_BOOL bResponse)
{
    PROFILE_COL(laCollider_collideLineWithDomain);
    unsigned nCurrentDomain = 0, i, j;
    laCollisionDomain *pDomain = pScenario->pFirstDomain;

    static laLine2 *pDomainLine, lnMovement[4];
    static laPoint2 ptIntersection, pt;
    laRect2 rectBoundingLine, rectBoundingDomain;

    // Build line; bounding box
    //
    laLine2 lnPositioned = line;
    lnPositioned.origin += pScenario->ptPosition;

    rectBoundingLine.add( lnPositioned );
    rectBoundingLine.inflate( pScenario->ptVector );
    rectBoundingLine.inflate( laPoint2(M_NORMAL_RESPONSE, M_NORMAL_RESPONSE) );
    rectBoundingLine.inflate( laPoint2(-M_NORMAL_RESPONSE, -M_NORMAL_RESPONSE) );

    /*::laSystemIntegrator::getRenderer()->styleLine( 1 );
    ::laSystemIntegrator::getRenderer()->styleSet( laColor(0,255,0) );
    rectBoundingLine.draw(::laSystemIntegrator::getRenderer());*/

    // Movement lines orignating form the endpoints of the line we're testing for
    //
    lnMovement[0].build_v( lnPositioned.origin, pScenario->ptVector );
    lnMovement[1].build_v( lnPositioned.end(), pScenario->ptVector );

    // Initial state
    //
    pScenario->bCollision       = M_FALSE;
    pScenario->ptModifiedVector = pScenario->ptVector;
    pScenario->pTrap            = NULL;

    double d, dNearestDistance = pScenario->ptVector.lenght_sq() + M_UNIT;

    //Cylce through all collision domains
    //
    do{
        /*::laSystemIntegrator::getRenderer()->styleSet( laColor(255,0,0) );
        ::laSystemIntegrator::getRenderer()->styleLine( 1 );
        pDomain->draw(::laSystemIntegrator::getRenderer(), pDomain->offset());*/

        if( rectBoundingLine.intersecting( pDomain->boundingRect() ) )  // Quick intersection test for the whole domain
        for(i=0; i<pDomain->countLines(); i++)
        {
            pDomainLine = pDomain->line(i);

            // Ignore backfacing
            //
            pt = pDomainLine->origin + *(pDomain->normal(i));

            if( (! pDomainLine->onSameSide(pt, lnPositioned.origin)) && (! pDomainLine->onSameSide(pt, lnPositioned.end())) )
                continue;

            /*::laSystemIntegrator::getRenderer()->styleSet( laColor(0,255,0) );
            ::laSystemIntegrator::getRenderer()->styleLine( 1 );
            pDomain->boundingRect().draw(::laSystemIntegrator::getRenderer());*/

            //Build inverted movement lines for the collision line's end-points
            lnMovement[2].build_v( pDomainLine->origin, pScenario->ptVector * (-1) );
            lnMovement[3].build_v( pDomainLine->end(), pScenario->ptVector * (-1) );

            // Check for intersections
            //
            for(j=0; j<4; j++){
                /*::laSystemIntegrator::getRenderer()->styleLine( 1 );
                ::laSystemIntegrator::getRenderer()->styleSet( laColor(0,0,255) );
                lnMovement[j].draw(::laSystemIntegrator::getRenderer(), laPoint2());*/

                if( intersection(lnMovement[j], (j<2)? *pDomainLine : lnPositioned, &d, &ptIntersection) && (d<dNearestDistance) )
                {
                    /*ptIntersection.draw( ::laSystemIntegrator::getRenderer());*/

                    dNearestDistance        = d;
                    pScenario->lnCollision  = *pDomainLine;

                    if( j<2 ) {
                        pScenario->ptCausing        = lnMovement[j].origin;
                        pScenario->ptIntersection   = ptIntersection;
                    }
                    else {
                        // Invert these, since we are projecting from the opposite side
                        pScenario->ptCausing        = ptIntersection;
                        pScenario->ptIntersection   = lnMovement[j].origin;
                    }

                    pScenario->pTrap        = pDomain->trap(i);
                }
            }

        }

    } while( (pDomain = pDomain->nextDomain()) && (++nCurrentDomain < pScenario->nDomains) );

    // Is there a collision with some of the lines?
    //
    if( dNearestDistance < pScenario->ptVector.lenght_sq() )
    {
        pScenario->bCollision = M_TRUE;

        // Project the movement vector on the collision line
        //
        if(bResponse)
            response(pScenario->ptCausing, pScenario->ptVector, pScenario->lnCollision, &(pScenario->ptModifiedVector));
        else
            pScenario->ptModifiedVector = pScenario->ptIntersection - pScenario->ptCausing;

        //safe_magin(lnPositioned, pScenario);

        // Next step
        //
        if(nIterations)
        {
            laPoint2 tv = pScenario->ptVector;
            pScenario->ptVector = pScenario->ptModifiedVector;

            collideLineWithDomain(line, pScenario, nIterations-1);
            pScenario->ptVector = tv;
            pScenario->bCollision = M_TRUE;
        }
    }
    //else safe_magin(lnPositioned, pScenario);
}

// Collision detection between multiple lines and a list of collision domains
//
void laCollider::collideLinesWithDomain(laLine2* arLines, unsigned nCount, laCollisionScenario *pScenario,
                                           int nIterations, M_BOOL bResponse)
{
    PROFILE_COL(laCollider_collideLinesWithDomain);
    laCollisionScenario scenarioClosest = *pScenario;
    double d, dNearestDistance  = pScenario->ptVector.lenght_sq() + M_UNIT;
    unsigned nLine = 0;

    // Cycle through all lines
    //
    for(unsigned i=0; i<nCount; i++)
    {
        collideLineWithDomain(arLines[i], pScenario, 0, M_FALSE);
        d = pScenario->ptModifiedVector.lenght_sq();

        // Is this collision the closest so far?
        //
        if( d < dNearestDistance ) {
            nLine = i;
            dNearestDistance    = d;
            scenarioClosest     = *pScenario;
        }
    }

    // Is there a collision?
    //
    if( dNearestDistance < pScenario->ptVector.lenght_sq() )
    {
        *pScenario = scenarioClosest;

        if(bResponse)
            response(pScenario->ptCausing, pScenario->ptVector, pScenario->lnCollision, &(pScenario->ptModifiedVector) );
        else
            pScenario->ptModifiedVector = pScenario->ptIntersection - pScenario->ptCausing;

        laLine2 ln = laLine2(arLines[nLine].origin + pScenario->ptPosition, arLines[nLine].end() + pScenario->ptPosition);
        safe_magin(ln, pScenario);

        // Next step
        //
        if(nIterations) {
            laPoint2 tv = pScenario->ptVector;
            pScenario->ptVector = pScenario->ptModifiedVector;

            collideLinesWithDomain(arLines, nCount, pScenario, nIterations-1);

            pScenario->ptVector = tv;
            pScenario->bCollision = M_TRUE;
            //if(pTrap) pScenario->pTrap      = pTrap;
        }
    }
    else for(unsigned j=0; j<nIterations; j++)
    {
        laLine2 ln = laLine2(arLines[nLine].origin + pScenario->ptPosition, arLines[nLine].end() + pScenario->ptPosition);
        safe_magin(ln, pScenario);
    }
}

// Collision detection between a single point and a list of collision domains
//
void laCollider::collidePointWithDomain(laCollisionScenario *pScenario, int nIterations, M_BOOL bResponse)
{
    PROFILE_COL(laCollider_collidePointWithDomain);
    unsigned nCurrentDomain = 0;
    laCollisionDomain *pDomain = pScenario->pFirstDomain;

    static laLine2 *pDomainLine, lnMovement;
    laRect2 rect;

    static laPoint2 ptIntersection, pt;
    static double     d, dNearestDistance;

    rect.add( pScenario->ptPosition );
    rect.add( pScenario->ptPosition + pScenario->ptVector );
    rect.inflate( pScenario->ptVector );
    rect.inflate( laPoint2(M_NORMAL_RESPONSE, M_NORMAL_RESPONSE) );
    rect.inflate( laPoint2(-M_NORMAL_RESPONSE, -M_NORMAL_RESPONSE) );

    lnMovement.build_v( pScenario->ptPosition, pScenario->ptVector);
    dNearestDistance = lnMovement.lenght_sq() + M_UNIT*2;

    //Cylce through all collision domains
    //
    do{
        if( rect.intersecting(pDomain->boundingRect()) )    //Simple test
        for(unsigned i=0; i<pDomain->countLines(); i++)
        {
            pDomainLine = pDomain->line(i);

            //Ignore backfacing
            //
            pt = pDomainLine->origin + *(pDomain->normal(i));
            if( pDomainLine->direction(pt)!=pDomainLine->direction(pScenario->ptPosition) ) continue;

            //Is it intersecting with the movement vector?
            //Mark the intersection point as our best candidate for collision
            //
            if( intersection(lnMovement, *pDomainLine, &d, &ptIntersection) && (d<dNearestDistance) )
            {
                dNearestDistance = d;
                pScenario->lnCollision = *pDomainLine;
                pScenario->ptIntersection = ptIntersection;

                pScenario->pTrap = pDomain->trap(i);
            }
        }
    } while( (pDomain = pDomain->nextDomain()) && (++nCurrentDomain < pScenario->nDomains) );

    //Was there a collision with some of the lines?
    //
    if( dNearestDistance < lnMovement.lenght_sq() )
    {
        pScenario->bCollision = M_TRUE;

        //Project the movement vector on the collision line
        if(bResponse)
            response(pScenario->ptPosition, pScenario->ptVector, pScenario->lnCollision, &(pScenario->ptModifiedVector));
        else
            pScenario->ptModifiedVector = pScenario->ptIntersection - pScenario->ptPosition;

        //Unless this is the last step of the recursion, prepare for the next one
        //
        // This crashese sometimes; Also if nIter == 0, then -- overflows and it crashes
        //
        /*if( (nIterations>0) && (--nIterations) )
        {
            //Spoof the movement vector
            laPoint2 OriginalVector = pScenario->ptVector;
            pScenario->ptVector = ModifiedVector;

            //Recurse
            collidePointWithDomain(pScenario, nIterations);

            //Restore the movement vector, in case the calling function uses it somehow
            pScenario->ptVector = OriginalVector;
        }*/
    }
    else
    {
        pScenario->bCollision = M_FALSE;
        pScenario->ptModifiedVector = pScenario->ptVector;
        pScenario->pTrap    = NULL;
    }

}

// Collision detection between multiple points and a list of collision domains
//
void laCollider::collidePointsWithDomain(laPoint2* arPoint, unsigned nCount, laCollisionScenario *pScenario, int nIterations)
{
    ASSERT(M_FALSE, "Not Implelemted");
    /*PROFILE_COL(laCollider_collidePointsWithDomain);
    laCollisionScenario Scenario;

    laLine2 lnMovement, lnNearest;
    laPoint2 ptNearest;

    //Initial state
    //
    pScenario->bCollision       = M_FALSE;
    pScenario->ptModifiedVector = pScenario->ptVector;

    double dNearestDistance     = pScenario->ptVector.lenght() + 1;

    //Cycle through all points
    for(unsigned i=0; i<nCount; i++)
    {
        //Copy the scanario and modify the position
        Scenario = *pScenario;
        Scenario.ptPosition += arPoint[i];

        //Cut the movement vector at the nearest collision (without collision response)
        collidePointWithDomain(&Scenario, 1, M_FALSE);

        //If this intersection is closer than any other that w eknow of
        if(Scenario.ptModifiedVector.lenght()<dNearestDistance)
        {
            //Then mark it as such
            dNearestDistance = Scenario.ptModifiedVector.lenght();
            lnNearest      = Scenario.lnCollision;
            ptNearest     = Scenario.ptPosition;
        }
    }

    //Was there a collision with some of the lines?
    if(dNearestDistance < pScenario->ptVector.lenght())
    {
        laPoint2 ModifiedVector;

        //Project the movement vector on the collision line
        response(ptNearest, pScenario->ptVector, lnNearest, &ModifiedVector);

        //Indicate that there is a collision
        pScenario->bCollision = M_TRUE;
        pScenario->lnCollision = lnNearest;

        //Unless this is the last step of the recursion, prepare for the next one
        if(nIterations)
        {
            //Spoof the movement vector
            laPoint2 OriginalVector = pScenario->ptVector;
            pScenario->ptVector = ModifiedVector;

            //Recurse
            collidePointsWithDomain(arPoint, nCount, pScenario, nIterations-1);

            //Restore the movement vector, in case the calling function uses it somehow
            pScenario->ptVector = OriginalVector;
        }
        else pScenario->ptModifiedVector = ModifiedVector;
    }*/
}