source: issm/trunk/src/c/modules/MeshProfileIntersectionx/SegmentIntersect.cpp@ 12330

/*! \file  SegmentIntersect.cpp
*/

#include "./MeshProfileIntersectionx.h"

int SegmentIntersect(double* palpha, double* pbeta, double* x1, double* y1, double* x2, double* y2){

        /*See ISSM_DIR/src/m/utils/Geometry/SegIntersect.m for matlab routine from which we take this routine: */

        /*output: */
        double alpha=-1;
        double beta=-1;
        
        double xA,xB,xC,xD,yA,yB,yC,yD;
        double O2A[2],O2B[2],O1C[2],O1D[2];
        double n1[2],n2[2];
        double test1, test2, test3, test4;
        double det;
        double O2O1[2];
        double pO1A,pO1B,pO1C,pO1D;

        xA=x1[0]; yA=y1[0];
        xB=x1[1]; yB=y1[1];
        xC=x2[0]; yC=y2[0];
        xD=x2[1]; yD=y2[1];

        O2A[0]=xA -(xD/2+xC/2); O2A[1]=yA -(yD/2+yC/2);
        O2B[0]=xB -(xD/2+xC/2); O2B[1]=yB -(yD/2+yC/2);
        O1C[0]=xC -(xA/2+xB/2); O1C[1]=yC -(yA/2+yB/2);
        O1D[0]=xD -(xA/2+xB/2); O1D[1]=yD -(yA/2+yB/2);


        n1[0]=yA-yB; n1[1]=xB-xA;  //normal vector to segA
        n2[0]=yC-yD; n2[1]=xD-xC;  //normal vector to segB

        test1=n2[0]*O2A[0]+n2[1]*O2A[1];
        test2=n2[0]*O2B[0]+n2[1]*O2B[1];

        if (test1*test2>0){
                return SeparateEnum;
        }

        test3=n1[0]*O1C[0]+n1[1]*O1C[1];
        test4=n1[0]*O1D[0]+n1[1]*O1D[1];

        if (test3*test4>0){
                return SeparateEnum;
        }

        /*If colinear: */
        det=n1[0]*n2[1]-n2[0]*n1[1];

        if(test1*test2==0 && test3*test4==0 && det==0){

                //projection on the axis O1O2
                O2O1[0]=(xA/2+xB/2)-(xD/2+xC/2);
                O2O1[1]=(yA/2+yB/2)-(yD/2+yC/2);

                pO1A=O2O1[0]*(O2A[0]-O2O1[0])+O2O1[1]*(O2A[1]-O2O1[1]);
                pO1B=O2O1[0]*(O2B[0]-O2O1[0])+O2O1[1]*(O2B[1]-O2O1[1]);
                pO1C=O2O1[0]*O1C[0]+O2O1[1]*O1C[1];
                pO1D=O2O1[0]*O1D[0]+O2O1[1]*O1D[1];

                //test if one point is included in the other segment (->intersects=true)
                if ((pO1C-pO1A)*(pO1D-pO1A)<0){
                        alpha=0; beta=0;
                        *palpha=alpha;*pbeta=beta;
                        return ColinearEnum;
                }
                if ((pO1C-pO1B)*(pO1D-pO1B)<0){
                        alpha=0; beta=0;
                        *palpha=alpha;*pbeta=beta;
                        return ColinearEnum;
                }
                if ((pO1A-pO1C)*(pO1B-pO1C)<0){
                        alpha=0; beta=0;
                        *palpha=alpha;*pbeta=beta;
                        return ColinearEnum;
                }
                if ((pO1A-pO1D)*(pO1B-pO1D)<0){
                        alpha=0; beta=0;
                        *palpha=alpha;*pbeta=beta;
                        return ColinearEnum;
                }

                //test if the 2 segments have the same middle (->intersects=true)
                if (O2O1==0){
                        alpha=0; beta=0;
                        *palpha=alpha;*pbeta=beta;
                        return ColinearEnum;
                }

                //if we are here, both segments are colinear, but do not interset:
                alpha=-1; beta=-1;
                *palpha=alpha;*pbeta=beta;
                return SeparateEnum;
        }

        /*if we are here, both segments intersect. Determine where in the segment coordinate 
         * system: */
        beta=-1;
        alpha=-(xA*yB-xC*yB+yC*xB-yC*xA+xC*yA-yA*xB)/(-xD*yB+xD*yA+xC*yB-xC*yA-yD*xA+yD*xB+yC*xA-yC*xB); //from intersect.m in formal calculus

        *palpha=alpha;*pbeta=beta;
        return IntersectEnum;
}