edge_collide.cpp

Go to the documentation of this file.

/* Copyright, 2001 Nevrax Ltd.
 *
 * This file is part of NEVRAX NEL.
 * NEVRAX NEL is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2, or (at your option)
 * any later version.

 * NEVRAX NEL is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
 * General Public License for more details.

 * You should have received a copy of the GNU General Public License
 * along with NEVRAX NEL; see the file COPYING. If not, write to the
 * Free Software Foundation, Inc., 59 Temple Place - Suite 330, Boston,
 * MA 02111-1307, USA.
 */

#include "stdpacs.h"

#include "edge_collide.h"

using namespace NLMISC;
using namespace std;



namespace NLPACS
{


static  const   float   EdgeCollideEpsilon= 1e-5f;


// ***************************************************************************
void        CEdgeCollide::make(const CVector2f &p0, const CVector2f &p1)
{
    P0= p0;
    P1= p1;
    // translation axis of the edge.
    Dir= P1-P0;
    Dir.normalize();
    A0= P0*Dir;
    A1= P1*Dir;
    // line equation.
    Norm.x=  Dir.y;
    Norm.y= -Dir.x;
    C= - P0*Norm;
}


// ***************************************************************************
CRational64 CEdgeCollide::testPointMove(const CVector2f &start, const CVector2f &end, TPointMoveProblem &moveBug)
{
    /*
        To have a correct test (with no float precision problem):
            - test first if there is collision beetween the 2 edges:
                - test if first edge collide the other line.
                - test if second edge collide the other line.
                - if both true, yes, there is a collision.
            - compute time of collision.
    */


    // *this must be a correct edge.
    if(P0==P1)
    {
        moveBug= EdgeNull;
        return -1;
    }

    // if no movement, no collision.
    if(start==end)
        return 1;

    // NB those edges are snapped (1/256 for edgeCollide, and 1/1024 for start/end), so no float problem here.
    // precision is 20 bits.
    CVector2f   normEdge;
    CVector2f   normMove;
    CVector2f   deltaEdge;
    CVector2f   deltaMove;

    // compute normal of the edge (not normalized, because no need, and for precision problem).
    deltaEdge= P1-P0;
    normEdge.x= -deltaEdge.y;
    normEdge.y= deltaEdge.x;

    // compute normal of the movement (not normalized, because no need, and for precision problem).
    deltaMove= end-start;
    normMove.x= -deltaMove.y;
    normMove.y= deltaMove.x;

    // distance from points of movment against edge line. Use double, because of multiplication.
    // precision is now 43 bits.
    double  moveD0= (double)normEdge.x*(double)(start.x-P0.x) + (double)normEdge.y*(double)(start.y-P0.y);
    double  moveD1= (double)normEdge.x*(double)(end.x  -P0.x) + (double)normEdge.y*(double)(end.y  -P0.y);

    // distance from points of edge against movement line. Use double, because of multiplication.
    // precision is now 43 bits.
    double  edgeD0= (double)normMove.x*(double)(P0.x-start.x) + (double)normMove.y*(double)(P0.y-start.y);
    double  edgeD1= (double)normMove.x*(double)(P1.x-start.x) + (double)normMove.y*(double)(P1.y-start.y);


    // If both edges intersect lines (including endpoints), there is a collision, else none.
    sint    sgnMove0, sgnMove1;
    sgnMove0= fsgn(moveD0);
    sgnMove1= fsgn(moveD1);

    // special case if the 2 edges lies on the same line.
    if(sgnMove0==0 && sgnMove1==0)
    {
        // must test if there is a collision. if yes, problem.
        // project all the points on the line of the edge.
        // Use double because of multiplication. precision is now 43 bits.
        double  moveA0= (double)deltaEdge.x*(double)(start.x-P0.x) + (double)deltaEdge.y*(double)(start.y-P0.y);
        double  moveA1= (double)deltaEdge.x*(double)(end.x  -P0.x) + (double)deltaEdge.y*(double)(end.y  -P0.y);
        double  edgeA0= 0;
        double  edgeA1= (double)deltaEdge.x*(double)deltaEdge.x + (double)deltaEdge.y*(double)deltaEdge.y;

        // Test is there is intersection (endpoints included). if yes, return -1. else return 1 (no collision at all).
        if(moveA1>=edgeA0 && edgeA1>=moveA0)
        {
            moveBug= ParallelEdges;
            return -1;
        }
        else
            return 1;
    }

    // if on same side of the line=> there is no collision.
    if( sgnMove0==sgnMove1)
        return 1;

    // test edge against move line.
    sint    sgnEdge0, sgnEdge1;
    sgnEdge0= fsgn(edgeD0);
    sgnEdge1= fsgn(edgeD1);

    // should not have this case, because tested before with (sgnMove==0 && sgnMove1==0).
    nlassert(sgnEdge0!=0 || sgnEdge1!=0);


    // if on same side of the line, no collision against this edge.
    if( sgnEdge0==sgnEdge1 )
        return 1;

    // Here the edges intersect, but ensure that there is no limit problem.
    if(sgnEdge0==0 || sgnEdge1==0)
    {
        moveBug= TraverseEndPoint;
        return -1;
    }
    else if(sgnMove1==0)
    {
        moveBug= StopOnEdge;
        return -1;
    }
    else if(sgnMove0==0)
    {
        // this should not arrive.
        moveBug= StartOnEdge;
        return -1;
    }


    // Here, there is a normal collision, just compute it.
    // Because of Division, there is a precision lost in double. So compute a CRational64.
    // First, compute numerator and denominator in the highest precision. this is 1024*1024 because of prec multiplication.
    double      numerator= (0-moveD0)*1024*1024;
    double      denominator= (moveD1-moveD0)*1024*1024;
    sint64      numeratorInt= (sint64)numerator;
    sint64      denominatorInt= (sint64)denominator;
/*
    nlassert(numerator == numeratorInt);
    nlassert(denominator == denominatorInt);
*/
/*
    if (numerator != numeratorInt)
        nlwarning("numerator(%f) != numeratorInt(%"NL_I64"d)", numerator, numeratorInt);
    if (denominator != denominatorInt)
        nlwarning("denominator(%f) != denominatorInt(%"NL_I64"d)", denominator, denominatorInt);
*/
    return CRational64(numeratorInt, denominatorInt);
}


// ***************************************************************************
static  inline float        testCirclePoint(const CVector2f &start, const CVector2f &delta, float radius, const CVector2f &point)
{
    // factors of the qaudratic: at² + bt + c=0
    float       a,b,c;
    float       dta;
    float       r0, r1, res;
    CVector2f   relC, relV;

    // As long as delta is not NULL (ensured in testCircleMove() ), this code should not generate Divide by 0.

    // compute quadratic..
    relC= start-point;
    relV= delta;
    a= relV.x*relV.x + relV.y*relV.y;
    // a should be >0. BUT BECAUSE OF PRECISION PROBLEM, it may be ==0, and then cause
    // divide by zero (because b may be near 0, but not 0)
    if(a==0)
    {
        // in this case the move is very small. return 0 if the point is in the circle and if we go toward the point
        if(relC.norm()<radius && relC*delta<0)
            return 0;
        else
            return 1;
    }
    else
    {
        b= 2* (relC.x*relV.x + relC.y*relV.y);
        c= relC.x*relC.x + relC.y*relC.y - radius*radius;
        // compute delta of the quadratic.
        dta= b*b - 4*a*c;   // b²-4ac
        if(dta>=0)
        {
            dta= (float)sqrt(dta);
            r0= (-b -dta)/(2*a);
            r1= (-b +dta)/(2*a);
            // since a>0, r0<=r1.
            if(r0>r1)
                swap(r0,r1);
            // if r1 is negative, then we are out and go away from this point. OK.
            if(r1<=0)
            {
                res= 1;
            }
            // if r0 is positive, then we may collide this point.
            else if(r0>=0)
            {
                res= min(1.f, r0);
            }
            else    // r0<0 && r1>0. the point is already in the sphere!!
            {
                //nlinfo("COL: Point problem: %.2f, %.2f.  b=%.2f", r0, r1, b);
                // we allow the movement only if we go away from this point.
                // this is true if the derivative at t=0 is >=0 (because a>0).
                if(b>0)
                    res= 1; // go out.
                else
                    res=0;
            }
        }
        else
        {
            // never hit this point along this movement.
            res= 1;
        }
    }

    return res;
}


// ***************************************************************************
float       CEdgeCollide::testCircleMove(const CVector2f &start, const CVector2f &delta, float radius, CVector2f &normal)
{
    // If the movement is NULL, return 1 (no collision!)
    if( delta.isNull() )
        return 1;

    // distance from point to line.
    double  dist= start*Norm + C;
    // projection of speed on normal.
    double  speed= delta*Norm;

    // test if the movement is against the line or not.
    bool    sensPos= dist>0;
    bool    sensSpeed= speed>0;

    // Does the point intersect the line?
    dist= fabs(dist) - radius;
    speed= fabs(speed);
    if( dist > speed )
        return 1;

    // if not already in collision with the line, test when it collides.
    // ===============================
    if(dist>=0)
    {
        // if signs are equals, same side of the line, so we allow the circle to leave the line.
        if(sensPos==sensSpeed )
            return 1;

        // if speed is 0, it means that movement is parralel to the line => never collide.
        if(speed==0)
            return 1;

        // collide the line, at what time.
        double  t= dist/speed;


        // compute the collision position of the Circle on the edge.
        // this gives the center of the sphere at the collision point.
        CVector2d   proj= CVector2d(start) + CVector2d(delta)*t;
        // must add radius vector.
        proj+= Norm * (sensSpeed?radius:-radius);
        // compute projection on edge.
        double      aProj= proj*Dir;

        // if on the interval of the edge.
        if( aProj>=A0 && aProj<=A1)
        {
            // collision occurs on interior of the edge. the normal to return is +- Norm.
            if(sensPos) // if algebric distance of start position was >0.
                normal= Norm;
            else
                normal= -Norm;

            // return time of collision.
            return (float)t;
        }
    }
    // else, must test if circle collide segment at t=0. if yes, return 0.
    // ===============================
    else
    {
        // There is just need to test if projection of circle's center onto the line hit the segment.
        // This is not a good test to know if a circle intersect a segment, but other cases are
        // managed with test of endPoints of the segment after.
        float       aProj= start*Dir;

        // if on the interval of the edge.
        if( aProj>=A0 && aProj<=A1)
        {
            // if signs are equals, same side of the line, so we allow the circle to leave the edge.
            /* Special case: do not allow to leave the edge if we are too much in the edge.
             It is important for CGlobalRetriever::testCollisionWithCollisionChains() because of the
             "SURFACEMOVE NOT DETECTED" Problem.
             Suppose we can walk on this chain SA/SB (separate Surface A/SurfaceB). Suppose we are near this edge,
             and on Surface SA, and suppose there is an other chain SB/SC the circle collide with. If we
             return 1 (no collision), SB/SC won't be detected (because only SA/?? chains will be tested) and
             so the cylinder will penetrate SB/SC...
             This case arise at best if chains SA/SB and chain SB/SC do an angle of 45deg
            */
            if(sensPos==sensSpeed && (-dist)<0.5*radius)
            {
                return 1;
            }
            else
            {
                // hit the interior of the edge, and sensPos!=sensSpeed. So must stop now!!
                // collision occurs on interior of the edge. the normal to return is +- Norm.
                if(sensPos) // if algebric distance of start position was >0.
                    normal= Norm;
                else
                    normal= -Norm;

                return 0;
            }
        }
    }

    // In this case, the Circle do not hit the edge on the interior, but may hit on borders.
    // ===============================
    // Then, we must compute collision sphere-points.
    float       tmin, ttmp;
    // first point.
    tmin= testCirclePoint(start, delta, radius, P0);
    // second point.
    ttmp= testCirclePoint(start, delta, radius, P1);
    tmin= min(tmin, ttmp);

    // if collision occurs, compute normal of collision.
    if(tmin<1)
    {
        // to which point we collide?
        CVector2f   colPoint= tmin==ttmp? P1 : P0;
        // compute position of the entity at collision.
        CVector2f   colPos= start + delta*tmin;

        // and so we have this normal (the perpendicular of the tangent at this point).
        normal= colPos - colPoint;
        normal.normalize();
    }

    return tmin;
}



// ***************************************************************************
bool        CEdgeCollide::testEdgeMove(const CVector2f &q0, const CVector2f &q1, const CVector2f &delta, float &tMin, float &tMax, bool &normalOnBox)
{
    double  a,b,cv,cc,  d,e,f;
    CVector2d   tmp;

    // compute D1 line equation of q0q1. bx - ay + c(t)=0, where c is function of time [0,1].
    // ===========================
    tmp= q1 - q0;       // NB: along time, the direction doesn't change.
    // Divide by norm()², so that  a projection on this edge is true if the proj is in interval [0,1].
    tmp/= tmp.sqrnorm();
    a= tmp.x;
    b= tmp.y;
    // c= - q0(t)*CVector2d(b,-a).  but since q0(t) is a function of time t (q0+delta*t), compute cv, and cc.
    // so c= cv*t + cc.
    cv= - CVector2d(b,-a)*delta;
    cc= - CVector2d(b,-a)*q0;

    // compute D2 line equation of P0P1. ex - dy + f=0.
    // ===========================
    tmp= P1 - P0;
    // Divide by norm()², so that  a projection on this edge is true if the proj is in interval [0,1].
    tmp/= tmp.sqrnorm();
    d= tmp.x;
    e= tmp.y;
    f= - CVector2d(e,-d)*P0;


    // Solve system.
    // ===========================
    /*
        Compute the intersection I of 2 lines across time.
        We have the system:
            bx - ay + c(t)=0
            ex - dy + f=0

        which solve for:
            det= ae-bd  (0 <=> // lines)
            x(t)= (d*c(t) - fa) / det
            y(t)= (e*c(t) - fb) / det
    */

    // determinant of matrix2x2.
    double  det= a*e - b*d;
    // if to near of 0. (take delta for reference of test).
    if(det==0 || fabs(det)<delta.norm()*EdgeCollideEpsilon)
        return false;

    // intersection I(t)= pInt + vInt*t.
    CVector2d       pInt, vInt;
    pInt.x= ( d*cc - f*a ) / det;
    pInt.y= ( e*cc - f*b ) / det;
    vInt.x= ( d*cv ) / det;
    vInt.y= ( e*cv ) / det;


    // Project Intersection.
    // ===========================
    /*
        Now, we project x,y onto each line D1 and D2, which gives  u(t) and v(t), each one giving the parameter of
        the parametric line function. When it is in [0,1], we are on the edge.

        u(t)= (I(t)-q0(t)) * CVector2d(a,b) = uc + uv*t
        v(t)= (I(t)-P0) * CVector2d(d,e)    = vc + vv*t
    */
    double  uc, uv;
    double  vc, vv;
    // NB: q0(t)= q0+delta*t
    uc= (pInt-q0) * CVector2d(a,b);
    uv= (vInt-delta) * CVector2d(a,b);
    vc= (pInt-P0) * CVector2d(d,e);
    vv= (vInt) * CVector2d(d,e);


    // Compute intervals.
    // ===========================
    /*
        Now, for each edge, compute time interval where parameter is in [0,1]. If intervals overlap, there is a collision.
    */
    double  tu0, tu1, tv0, tv1;
    // infinite interval.
    bool    allU=false, allV=false;

    // compute time interval for u(t).
    if(uv==0 || fabs(uv)<EdgeCollideEpsilon)
    {
        // The intersection does not move along D1. Always projected on u(t)=uc. so if in [0,1], OK, else never collide.
        if(uc<0 || uc>1)
            return false;
        // else suppose "always valid".
        tu0 =tu1= 0;
        allU= true;
    }
    else
    {
        tu0= (0-uc)/uv; // t for u(t)=0
        tu1= (1-uc)/uv; // t for u(t)=1
    }

    // compute time interval for v(t).
    if(vv==0 || fabs(vv)<EdgeCollideEpsilon)
    {
        // The intersection does not move along D2. Always projected on v(t)=vc. so if in [0,1], OK, else never collide.
        if(vc<0 || vc>1)
            return false;
        // else suppose "always valid".
        tv0 =tv1= 0;
        allV= true;
    }
    else
    {
        tv0= (0-vc)/vv; // t for v(t)=0
        tv1= (1-vc)/vv; // t for v(t)=1
    }


    // clip intervals.
    // ===========================
    // order time interval.
    if(tu0>tu1)
        swap(tu0, tu1);     // now, [tu0, tu1] represent the time interval where line D2 hit the edge D1.
    if(tv0>tv1)
        swap(tv0, tv1);     // now, [tv0, tv1] represent the time interval where line D1 hit the edge D2.

    normalOnBox= false;
    if(!allU && !allV)
    {
        // if intervals do not overlap, no collision.
        if(tu0>tv1 || tv0>tu1)
            return false;
        else
        {
            // compute intersection of intervals.
            tMin= (float)max(tu0, tv0);
            tMax= (float)min(tu1, tv1);
            // if collision of edgeCollide against the bbox.
            if(tv0>tu0)
                normalOnBox= true;
        }
    }
    else if(allU)
    {
        // intersection of Infinite and V interval.
        tMin= (float)tv0;
        tMax= (float)tv1;
        // if collision of edgeCollide against the bbox.
        normalOnBox= true;
    }
    else if(allV)
    {
        // intersection of Infinite and U interval.
        tMin= (float)tu0;
        tMax= (float)tu1;
    }
    else
    {
        // if allU && allV, this means delta is near 0, and so there is always collision.
        tMin= -1000;
        tMax= 1000;
    }

    return true;
}


// ***************************************************************************
float       CEdgeCollide::testBBoxMove(const CVector2f &start, const CVector2f &delta, const CVector2f bbox[4], CVector2f &normal)
{
    // distance from center to line.
    float   dist= start*Norm + C;

    // test if the movement is against the line or not.
    bool    sensPos= dist>0;
    // if signs are equals, same side of the line, so we allow the circle to leave the line.
    /*if(sensPos==sensSpeed)
        return 1;*/


    // Else, do 4 test edge/edge, and return Tmin.
    float   tMin = 0.f, tMax = 0.f;
    bool    edgeCollided= false;
    bool    normalOnBox= false;
    CVector2f   boxNormal(0.f, 0.f);
    for(sint i=0;i<4;i++)
    {
        float   t0, t1;
        bool    nob;
        CVector2f   a= bbox[i];
        CVector2f   b= bbox[(i+1)&3];

        // test move against this edge.
        if(testEdgeMove(a, b, delta, t0, t1, nob))
        {
            if(edgeCollided)
            {
                tMin= min(t0, tMin);
                tMax= max(t1, tMax);
            }
            else
            {
                edgeCollided= true;
                tMin= t0;
                tMax= t1;
            }

            // get normal of box against we collide.
            if(tMin==t0)
            {
                normalOnBox= nob;
                if(nob)
                {
                    CVector2f   dab;
                    // bbox must be CCW.
                    dab= b-a;
                    // the normal is computed so that the vector goes In the bbox.
                    boxNormal.x= -dab.y;
                    boxNormal.y= dab.x;
                }
            }
        }
    }

    // if collision occurs,and int the [0,1] interval...
    if(edgeCollided && tMin<1 && tMax>0)
    {
        // compute normal of collision.
        if(normalOnBox)
        {
            // assume collsion is an endpoint of the edge against the bbox.
            normal= boxNormal;
        }
        else
        {
            // assume collision occurs on interior of the edge. the normal to return is +- Norm.
            if(sensPos) // if algebric distance of start position was >0.
                normal= Norm;
            else
                normal= -Norm;
        }

        // compute time of collison.
        if(tMin>0)
            // return time of collision.
            return tMin;
        else
        {
            // The bbox is inside the edge, at t==0. test if it goes out or not.
            // accept only if we are much near the exit than the enter.
            /* NB: 0.2 is an empirical value "which works well". Normally, 1 is the good value, but because of the
                "SURFACEMOVE NOT DETECTED" Problem (see testCircleMove()), we must be more restrictive.
            */
            if( tMax<0.2*(-tMin) )
                return 1;
            else
                return 0;
        }
    }
    else
        return 1;

}


// ***************************************************************************
bool        CEdgeCollide::testBBoxCollide(const CVector2f bbox[4])
{
    // clip the edge against the edge of the bbox.
    CVector2f       p0= P0, p1= P1;

    for(sint i=0; i<4; i++)
    {
        CVector2f   a= bbox[i];
        CVector2f   b= bbox[(i+1)&3];
        CVector2f   delta= b-a, norm;
        // sign is important. bbox is CCW. normal goes OUT the bbox.
        norm.x= delta.y;
        norm.y= -delta.x;

        float   d0= (p0-a)*norm;
        float   d1= (p1-a)*norm;

        // if boths points are out this plane, no collision.
        if( d0>0 && d1>0)
            return false;
        // if difference, must clip.
        if( d0>0 || d1>0)
        {
            CVector2f   intersect= p0 + (p1-p0)* ((0-d0)/(d1-d0));
            if(d1>0)
                p1= intersect;
            else
                p0= intersect;
        }
    }

    // if a segment is still in the bbox, collision occurs.
    return true;
}



} // NLPACS