Change Encyclo button name and macros icon

[ryzomcore.git] / nel / src / pacs / edge_collide.cpp

// NeL - MMORPG Framework <http://dev.ryzom.com/projects/nel/>
// Copyright (C) 2010  Winch Gate Property Limited
//
// This source file has been modified by the following contributors:
// Copyright (C) 2016  Jan BOON (Kaetemi) <jan.boon@kaetemi.be>
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU Affero General Public License as
// published by the Free Software Foundation, either version 3 of the
// License, or (at your option) any later version.
//
// This program 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 Affero General Public License for more details.
//
// You should have received a copy of the GNU Affero General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.

#include "stdpacs.h"

#include "nel/pacs/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^2 + 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^2-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 another 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()^2, 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()^2, 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