ot_algorithme_geometrique.cpp

Aller à la documentation de ce fichier.

//####//------------------------------------------------------------
//####//------------------------------------------------------------
//####//  MAGiC
//####//  Jean Christophe Cuilliere et Vincent FRANCOIS
//####//  Departement de Genie Mecanique - UQTR
//####//------------------------------------------------------------
//####//  MAGIC est un projet de recherche de l equipe ERICCA
//####//  du departement de genie mecanique de l Universite du Quebec a Trois Rivieres
//####//  http://www.uqtr.ca/ericca
//####//  http://www.uqtr.ca/
//####//------------------------------------------------------------
//####//------------------------------------------------------------
//####//
//####//       ot_algorithme_geometrique.cpp
//####//
//####//------------------------------------------------------------
//####//------------------------------------------------------------
//####//    COPYRIGHT 2000-2024
//####//  jeu 13 jun 2024 11:53:59 EDT
//####//------------------------------------------------------------
//####//------------------------------------------------------------
 
#include <math.h>
 
#pragma hdrstop
 
 
#include "ot_algorithme_geometrique.h"
#pragma package(smart_init)
 
double OT_ALGORITHME_GEOMETRIQUE::Dist3D_Point_Plane
(double *norm, double *root, double *pnt)
{
    double res[3];
    return Closest_Point_to_Plane_3d(norm,root,pnt,res);
}
 
double OT_ALGORITHME_GEOMETRIQUE::Closest_Point_to_Plane_3d(double *norm, double *root, double *pnt, double *res)
{
    double pr[3], sn,sd,sb;
 
    // pr = pnt - root
    VEC3_MINUS_VEC3(pnt,root,pr);
    // sn = -(norm * pr)
    sn = - VEC3_DOT_VEC3(norm,pr);
    // sd = norm*norm
    sd = VEC3_NORM2(norm);
    // sb = sn/sd
    sb = sn/sd;
    // res = pnt + sb * norm
    VEC3_MULTIPLY_SCALAR(norm,sb,res);
    VEC3_PLUS_VEC3(res,pnt,res);
    // return dist(res, pnt)
    return VEC3_DISTANCE_VEC3(res,pnt);
}
 
void
OT_ALGORITHME_GEOMETRIQUE::Dist3D_Segment_Segment( double *S11, double *S12, double *S21, double *S22, double *dist, double *P1, double *P2)
{
    static const double SMALL_NUM  = 1E-12;
    int i;
    double u[3], v[3], w[3];
    double a,b,c,d,e;
    double D;
    double sc, sN, sD;
    double tc, tN, tD;
    double dP[3];
 
    for (i=0;i<3;i++)
    {
        u[i]=S12[i]-S11[i];
        v[i]=S22[i]-S21[i];
        w[i]=S11[i]-S21[i];
    }
    a=VEC3_DOT_VEC3(u,u);// always >= 0
    b=VEC3_DOT_VEC3(u,v);
    c=VEC3_DOT_VEC3(v,v);// always >= 0
    d=VEC3_DOT_VEC3(u,w);
    e=VEC3_DOT_VEC3(w,v);
    D = a*c - b*b;       // always >= 0
    sD = D;      // sc = sN / sD, default sD = D >= 0
    tD = D;      // tc = tN / tD, default tD = D >= 0
 
    // compute the line parameters of the two closest points
    if (D < SMALL_NUM) { // the lines are almost parallel
        sN = 0.0;        // force using point P0 on segment S1
        sD = 1.0;        // to prevent possible division by 0.0 later
        tN = e;
        tD = c;
    }
    else {                // get the closest points on the infinite lines
        sN = (b*e - c*d);
        tN = (a*e - b*d);
        if (sN < 0.0) {       // sc < 0 => the s=0 edge is visible
            sN = 0.0;
            tN = e;
            tD = c;
        }
        else if (sN > sD) {  // sc > 1 => the s=1 edge is visible
            sN = sD;
            tN = e + b;
            tD = c;
        }
    }
 
    if (tN < 0.0) {           // tc < 0 => the t=0 edge is visible
        tN = 0.0;
        // recompute sc for this edge
        if (-d < 0.0)
            sN = 0.0;
        else if (-d > a)
            sN = sD;
        else {
            sN = -d;
            sD = a;
        }
    }
    else if (tN > tD) {      // tc > 1 => the t=1 edge is visible
        tN = tD;
        // recompute sc for this edge
        if ((-d + b) < 0.0)
            sN = 0;
        else if ((-d + b) > a)
            sN = sD;
        else {
            sN = (-d + b);
            sD = a;
        }
    }
    // finally do the division to get sc and tc
    sc = (fabs(sN) < SMALL_NUM ? 0.0 : sN / sD);
    tc = (fabs(tN) < SMALL_NUM ? 0.0 : tN / tD);
 
    // get the difference of the two closest points
    for (i=0;i<3;i++)
    {
        P1[i] = S11[i] + sc * u[i];
        P2[i] = S21[i] + tc * v[i];
        dP[i] = w[i] + (sc * u[i]) - (tc * v[i]);  // = S1(sc) - S2(tc)
    }
 
    *dist = VEC3_NORM(dP);   // return the closest distance
}
 
int OT_ALGORITHME_GEOMETRIQUE::Intr3D_Plane_Plane(double __N1[3],double __P1[3],double __N2[3],double __P2[3],double __D[3])
{
    VEC3_CROSS_VEC3(__N1,__N2,__D);
 
    if (VEC3_NORM2(__D) == 0)
        return 0;
 
    VEC3_NORMALIZE(__D);
    return 1;
}
double OT_ALGORITHME_GEOMETRIQUE::Dist3D_Point_Plan
(double *norm, double *root, double *pnt)
{
    double D = -(norm[0]*root[0]+norm[1]*root[1]+norm[2]*root[2]);
 
    return (norm[0]*pnt[0]+norm[1]*pnt[1]+norm[2]*pnt[2]+D)/sqrt(fabs(norm[0]*norm[0]+norm[1]*norm[1]+norm[2]*norm[2]));
}
 
int OT_ALGORITHME_GEOMETRIQUE::Closest_Point_to_Line_3d (double __lineOrigin[3], double __lineDirection[3], double __p[3], double __r[3])
{
    double ab[3];
    VEC3_ASSIGN_VEC3(__lineDirection,ab);
    double ac[3];
    VEC3_MINUS_VEC3(__p,__lineOrigin,ac);
    double c1 = VEC3_DOT_VEC3(ac,ab);
    double c2 = VEC3_NORM2 (ab);
    double t = c1/c2;
    VEC3_MULTIPLY_SCALAR(ab,t,__r);
    VEC3_PLUS_VEC3(__r,__lineOrigin,__r);
    return 0;
}
 
int OT_ALGORITHME_GEOMETRIQUE::Closest_Point_to_Segment_3d (double a[3], double b[3], double c[3], double d[3])
{
    double ab[3];
    VEC3_MINUS_VEC3(b,a,ab);
 
    double ac[3];
    VEC3_MINUS_VEC3(c,a,ac);
 
    double c1 = VEC3_DOT_VEC3(ac,ab);
 
    if (c1 <= 0)
    {
        VEC3_ASSIGN_VEC3(a,d);
        return 1;
    }
 
    double c2 = VEC3_NORM2 (ab);
    if (c2 <= c1)
    {
        VEC3_ASSIGN_VEC3(b,d);
        return 2;
    }
 
    double t = c1/c2;
    {
        VEC3_MULTIPLY_SCALAR(ab,t,d);
        VEC3_PLUS_VEC3(d,a,d);
        return 0;
    }
}
int OT_ALGORITHME_GEOMETRIQUE::Closest_Point_to_Segment_3d (double a[3], double b[3], double c[3], double d[3], double *t)
{
    double ab[3];
    VEC3_MINUS_VEC3(b,a,ab);
 
    double ac[3];
    VEC3_MINUS_VEC3(c,a,ac);
 
    double c1 = VEC3_DOT_VEC3(ac,ab);
    double c2 = VEC3_NORM2 (ab);
    if (c2 == 0) // degenerated segment
    {
        *t=1E300;
        return -1;
    }
 
    if (c1 <= 0)
    {
        VEC3_ASSIGN_VEC3(a,d);
        *t=0;
        return 1;
    }
 
    *t = c1/c2;
    if (c2 <= c1)
    {
        VEC3_ASSIGN_VEC3(b,d);
        return 2;
    }
 
    {
        VEC3_MULTIPLY_SCALAR(ab,*t,d);
        VEC3_PLUS_VEC3(d,a,d);
        return 0;
    }
}
 
 
 
int OT_ALGORITHME_GEOMETRIQUE::Closest_Point_to_Segment_2d( double * SP0, double * SP1, double * P, double * t, double result[2])
{
    double v[2];
    for (int i = 0; i < 2; i++)
        v[i] = SP1[i] - SP0[i];
 
    double w[2];
    for (int i = 0; i < 2; i++)
        w[i] = P[i] - SP0[i];
 
    double c1 = v[0]*w[0]+v[1]*w[1];
    double c2 = v[0]*v[0]+v[1]*v[1];
 
    if (c2 < 1E-6*fabs(c1) || c2 == 0.0f )
    {
        *t=1E6;
        return -1;
    }
    else
        *t = c1 / c2;
 
    if ( c1 <= 0 )
    {
        for (int i = 0; i < 2; i++)
            result[i] = SP0[i];
        return 1;
    }
 
    if ( c2 <= c1 )
    {
        for (int i = 0; i < 2; i++)
            result[i] = SP1[i];
        return 2;
    }
 
    for (int i = 0; i < 2; i++)
        result[i] = SP0[i] + t[0] * v[i];
 
    return 0;
}
 
 
double OT_ALGORITHME_GEOMETRIQUE::Dist2D_Point_Segment (double a[2], double b[2], double c[2])
 {
    double closestPoint[2], tmpt;
 
    Closest_Point_to_Segment_2d(a,b,c,&tmpt, closestPoint);
 
    return VEC2_DISTANCE_VEC2(closestPoint, c);
}
 
 
double OT_ALGORITHME_GEOMETRIQUE::Dist3D_Point_Segment
(double a[3], double b[3], double c[3])
{
    double closestPoint[3];
 
    Closest_Point_to_Segment_3d(a,b,c,closestPoint);
 
    return VEC3_DISTANCE_VEC3(closestPoint, c);
}
 
void OT_ALGORITHME_GEOMETRIQUE::Proj3D_Point_Plan
(double *norm, double *root, double *pnt, double *proj_pnt)
{
    double d = Dist3D_Point_Plan
(norm, root, pnt);
 
    for (int i=0; i<3; i++)
        proj_pnt[i] = pnt[i] - d*norm[i];
}
 
 
int
OT_ALGORITHME_GEOMETRIQUE::Intr3D_Segment_Plan
(double P0[3], double P1[3], double O[3], double V[3], double *p_coef, double EPSILON)
{
    int i=0; // variable de boucle
    double u[3]; // u: vecteur P1 - P0
    double w[3]; // P0 - 0 (vecteur = premier pt segt - point appartenant au plan)
    double coef = 0; // coefficient d'intersection du edge
    double     D = .0;
    double     N = .0;
 
    for (i = 0; i < 3; i++) u[i] = P1[i] - P0[i];
    for (i = 0; i < 3; i++) w[i] = P0[i] - O[i];
 
    D =  V[0]*u[0]+V[1]*u[1]+V[2]*u[2]; // prod scalaire normale plan * u
    N =  -(V[0]*w[0]+V[1]*w[1]+V[2]*w[2]);
    // prod scalaire normale plan * w : positif
    // si il est // plan) dans le direction normale au plan
 
    if (fabs(D) < EPSILON) {          // edge is parallel to plane
        if (fabs(N) < EPSILON)                     // edge lies in plane
            return 2;
        else
            return 0;                   // no intersection
    }
 
    // they are not parallel
    // compute intersect param
    coef = N / D;
    if ( coef+EPSILON < 0.0 || coef-EPSILON > 1.0)
    {
        *p_coef = coef;
        return 0;                       // no intersection
    }
 
    /*
    for (i = 0; i < 3; i++)
        I[i] = P0[i] + coef * u[i];     // compute edge-plane intersect point
    */
 
    // met la valeur de coef dans la variable de sortie
    *p_coef = coef;
 
    return 1;
}
 
int
OT_ALGORITHME_GEOMETRIQUE::Dist3D_Segment_Plan
(double P0[3], double P1[3], double O[3], double V[3], double *p_coef, double *distance, double EPSILON)
{
    double dist_P0 = Dist3D_Point_Plan
(V, O, P0);
    if ( fabs(dist_P0) < EPSILON )
        dist_P0 = 0;
 
    double dist_P1 = Dist3D_Point_Plan
(V, O, P1);
    if ( fabs(dist_P1) < EPSILON )
        dist_P1 = 0;
 
    double prodDist =  dist_P0 * dist_P1;
    if ( prodDist < 0 )
    {
        // The segment passes through the plane.
        double direction[3];
        VEC3_MINUS_VEC3(P1,P0,direction);
 
        *p_coef = -dist_P0/(VEC3_DOT_VEC3(direction, V));
        *distance = 0;
 
        return 1;
    }
 
    if ( prodDist > 0 )
    {
        // The segment is on one side of the plane.
        double direction[3];
        VEC3_MINUS_VEC3(P1,P0,direction);
 
        double dirDotNormal = VEC3_DOT_VEC3(direction, V);
        if (dirDotNormal > EPSILON)
            *p_coef = -dist_P0/dirDotNormal;
        else
            *p_coef = 1;
        *distance = (fabs(dist_P0) < fabs(dist_P1)) ? fabs(dist_P0) : fabs(dist_P1) ;
 
        return 0;
    }
 
    if ( dist_P0 != 0.0 || dist_P0 != 0.0 )
    {
        // A segment end point touches the plane.
 
        if (dist_P0 == 0)
            *p_coef = 0;
        if (dist_P1 == 0)
            *p_coef = 1;
 
        *distance = 0;
        return 1;
    }
 
    if ( dist_P0 == 0.0 && dist_P0 == 0.0 )
    {
        // The segment is coincident with the plane.
        *distance = 0;
        *p_coef = .5;
        return 2;
    }
 
    return 0;
}
 
double
OT_ALGORITHME_GEOMETRIQUE::pbase_Plane( double P[3], double planeNormal[3], double planeRootPoint[3], double B[3])
{
    // sn = -dot( PL.n, (P - PL.V0));
    double sn=0;
    for (int i=0;i<3;i++)
        sn += - (planeNormal[i] - (P[i] - planeRootPoint[i]));
 
    // sd = dot(PL.n, PL.n);
    double sd=0;
    for (int i=0;i<3;i++)
        sd += (planeNormal[i]*planeNormal[i]);
 
    //     sb = sn / sd;
    double sb = sn / sd;
 
    // *B = P + sb * PL.n;
    for (int i=0;i<3;i++)
        B[i] = P[i] + sb * planeNormal[i];
 
    // return d(P, *B);
    return sqrt(pow(P[0]-B[0],2)+pow(P[1]-B[1],2)+pow(P[2]-B[2],2));
}
 
double
OT_ALGORITHME_GEOMETRIQUE::Angle3D_Segment_Segment
(double __v0[3], double __v1[3], double __v2[3])
{
    double s01[3], s12[3];
    VEC3_MINUS_VEC3(__v1, __v0, s01);
    VEC3_MINUS_VEC3(__v2, __v1, s12);
 
    double s01Dots12 = VEC3_DOT_VEC3(s01, s12);
    double norm_s01 = VEC3_NORM(s01);
    double norm_s12 = VEC3_NORM(s12);
 
    if (norm_s01*norm_s12 < 1E-28)
        return 0;
 
    double proj = s01Dots12/(norm_s01*norm_s12);
    if (proj > 1.0)
        proj = 1.0;
    if (proj < -1.0)
        proj = -1.0;
 
    double angle = acos (proj);
 
    return angle;
}
void OT_ALGORITHME_GEOMETRIQUE::Rot3D_Point_Axe_Angle
(double p[3],double theta, double r[3], double q[3])
{
    double costheta,sintheta;
 
    VEC3_NORMALIZE(r);
    costheta = cos(theta);
    sintheta = sin(theta);
 
    q[0] = q[1] = q[2] = 0;
 
    q[0] += (costheta + (1 - costheta) * r[0] * r[0]) * p[0];
    q[0] += ((1 - costheta) * r[0] * r[1] - r[2] * sintheta) * p[1];
    q[0] += ((1 - costheta) * r[0] * r[2] + r[1] * sintheta) * p[2];
 
    q[1] += ((1 - costheta) * r[0] * r[1] + r[2] * sintheta) * p[0];
    q[1] += (costheta + (1 - costheta) * r[1] * r[1]) * p[1];
    q[1] += ((1 - costheta) * r[1] * r[2] - r[0] * sintheta) * p[2];
 
    q[2] += ((1 - costheta) * r[0] * r[2] - r[1] * sintheta) * p[0];
    q[2] += ((1 - costheta) * r[1] * r[2] + r[0] * sintheta) * p[1];
    q[2] += (costheta + (1 - costheta) * r[2] * r[2]) * p[2];
}
 
#define SMALL_NUM  0.00000001 // anything that avoids division overflow
int OT_ALGORITHME_GEOMETRIQUE::intersect2D_SegSeg( double S1P0[2], double S1P1[2], double S2P0[2], double S2P1[2], double __intrPoint[2], double __intrEndPointSeg[2])
{
    double u[2],v[2],w[2];
    // Vector    u = S1.P1 - S1.P0;
    VEC2_MINUS_VEC2(S1P1,S1P0,u);
    // Vector    v = S2.P1 - S2.P0;
    VEC2_MINUS_VEC2(S2P1,S2P0,v);
    //Vector    w = S1.P0 - S2.P0;
    VEC2_MINUS_VEC2(S1P0,S2P0,w);
 
    double     D = PERP_PROD2(u,v);
 
    // test if they are parallel (includes either being a point)
    if (fabs(D) < SMALL_NUM) {          // S1 and S2 are parallel
        if (PERP_PROD2(u,w) != 0 || PERP_PROD2(v,w) != 0) {
            return 0;                   // they are NOT collinear
        }
        // they are collinear or degenerate
        // check if they are degenerate points
        double du = VEC2_DOT_VEC2(u,u);
        double dv = VEC2_DOT_VEC2(v,v);
        if (du==0 && dv==0) {           // both segments are points
            if (VEC2_DISTANCE2_VEC2(S1P0,S2P0) > 0) // they are distinct points
                return 0;
            VEC2_ASSIGN_VEC2(S1P0, __intrPoint);                // they are the same point
            return 1;
        }
        if (du==0) {                    // S1 is a single point
            if (Dist2D_Point_Segment(S2P0,S2P1,S1P0) > 0)  // but is not in S2
                return 0;
            VEC2_ASSIGN_VEC2(S1P0, __intrPoint);
            return 1;
        }
        if (dv==0) {                    // S2 a single point
            if (Dist2D_Point_Segment(S1P0,S1P1,S2P0) > 0)  // but is not in S1
                return 0;
            VEC2_ASSIGN_VEC2(S2P0, __intrPoint);
            return 1;
        }
        // they are collinear segments - get overlap (or not)
        double t0, t1;                   // endpoints of S1 in eqn for S2
        //Vector w2 = S1.P1 - S2.P0;
        double w2[2];
        VEC2_MINUS_VEC2(S1P1, S2P0, w2);
 
        if (v[0] != 0) {
            t0 = w[0] / v[0];
            t1 = w2[0] / v[0];
        }
        else {
            t0 = w[1] / v[1];
            t1 = w2[1] / v[1];
        }
        if (t0 > t1) {                  // must have t0 smaller than t1
            double t=t0;
            t0=t1;
            t1=t;    // swap if not
        }
        if (t0 > 1 || t1 < 0) {
            return 0;     // NO overlap
        }
        t0 = t0<0? 0 : t0;              // clip to min 0
        t1 = t1>1? 1 : t1;              // clip to max 1
        if (t0 == t1) {                 // intersect is a point
            for (int i=0; i<2; i++)
                __intrPoint[i] = S2P0[i] + t0 * v[i];
            return 1;
        }
 
        // they overlap in a valid subsegment
        for (int i=0; i<2; i++)
            __intrPoint[i] = S2P0[i] + t0 * v[i];
 
        for (int i=0; i<2; i++)
            __intrEndPointSeg[i] = S2P0[i] + t1 * v[i];
 
        return 2;
    }
 
    // the segments are skew and may intersect in a point
    // get the intersect parameter for S1
    double     sI = PERP_PROD2(v,w) / D;
    if (sI < 0 || sI > 1)               // no intersect with S1
        return 0;
 
    // get the intersect parameter for S2
    double     tI = PERP_PROD2(u,w) / D;
    if (tI < 0 || tI > 1)               // no intersect with S2
        return 0;
 
    for (int i=0; i<2; i++)        // compute S1 intersect point
        __intrPoint[i] = S1P0[i] + sI * u[i];
 
    return 1;
}
 
int OT_ALGORITHME_GEOMETRIQUE::intersect_RayTriangle( double RP0[3], double dir[3], double TV0[3], double TV1[3], double TV2[3], double I[3], double *IT )
{
    double u[3], v[3], n[3]; // triangle vectors
    double    w0[3], w[3];          // ray vectors
    double     r, a, b;             // params to calc ray-plane intersect
 
    // get triangle edge vectors and plane normal
    // u = T.V1 - T.V0;
    VEC3_MINUS_VEC3(TV1,TV0,u);
    // v = T.V2 - T.V0;
    VEC3_MINUS_VEC3(TV2,TV0,v);
    //n = u * v;             // cross product
    VEC3_CROSS_VEC3(u,v,n);
 
    if (VEC3_NORM2(n) == 0)            // triangle is degenerate
        return -1;                 // do not deal with this case
 
    // dir = ray direction vector
    //w0 = R.P0 - T.V0;
    VEC3_MINUS_VEC3(RP0, TV0, w0);
    //    a = -dot(n,w0);
    a = -VEC3_DOT_VEC3(n,w0);
    // b = dot(n,dir);
    b = VEC3_DOT_VEC3(n,dir);
    if (fabs(b) < SMALL_NUM) {     // ray is parallel to triangle plane
        if (a == 0)                // ray lies in triangle plane
            return 2;
        else return 0;             // ray disjoint from plane
    }
 
    // get intersect point of ray with triangle plane
    r = a / b;
    if (r < 0.0)                   // ray goes away from triangle
        return 0;                  // => no intersect
    // for a segment, also test if (r > 1.0) => no intersect
 
    for (int i=0; i<3; i++)
        I[i] = RP0[i] + r*dir[i];  // intersect point of ray and plane
 
    // is I inside T?
    double    uu, uv, vv, wu, wv, D;
    uu = VEC3_DOT_VEC3(u,u);
    uv = VEC3_DOT_VEC3(u,v);
    vv = VEC3_DOT_VEC3(v,v);
    for (int i=0; i<3; i++)
        w[i] = I[i] - TV0[i];
    wu = VEC3_DOT_VEC3(w,u);
    wv = VEC3_DOT_VEC3(w,v);
    D = uv * uv - uu * vv;
 
    // get and test parametric coords
    double s, t;
    s = (uv * wv - vv * wu) / D;
    if (s < 0.0 || s > 1.0)        // I is outside T
        return 0;
    t = (uv * wu - uu * wv) / D;
    if (t < 0.0 || (s + t) > 1.0)  // I is outside T
        return 0;
 
    if (IT)
        *IT = r;
 
    return 1;                      // I is in T
}
 
 
int OT_ALGORITHME_GEOMETRIQUE::project_PointTriangle( double P0[3], double TV0[3], double TV1[3], double TV2[3], double I[3], double *IT )
{
    double u[3], v[3], n[3]; // triangle vectors
    double    dir[3], RP0[3];          // ray vectors
    double     r, a, b;             // params to calc ray-plane intersect
 
    // get triangle edge vectors and plane normal
    // u = T.V1 - T.V0;
    VEC3_MINUS_VEC3(TV1,TV0,u);
    // v = T.V2 - T.V0;
    VEC3_MINUS_VEC3(TV2,TV1,v);
    //n = u * v;             // cross product
    VEC3_CROSS_VEC3(u,v,n);
 
    // signed squared distance between the point and the plane of the triangle
    double D=Dist3D_Point_Plane(n,TV0,P0);
 
    int signD = (D<0)?-1:+1;
 
    // D is used for the direction of the intersecting ray
    for (int i=0; i<3; i++)
        dir[i] = n[i]*signD;
    for (int i=0; i<3; i++)
        RP0[i] = P0[i]-dir[i];
 
    int res_intrRT = intersect_RayTriangle(RP0, dir, TV0, TV1, TV2, I, IT);
 
    // Signed distance from the point to the triangle
    *IT = VEC3_DISTANCE_VEC3(I,P0);
 
    return res_intrRT;
}
 
 
int OT_ALGORITHME_GEOMETRIQUE::intersect_SegTriangle( double SP0[3], double SP1[3], double TV0[3], double TV1[3], double TV2[3], double I[3], double *IT )
{
    double u[3], v[3], n[3]; // triangle vectors
    double    dir[3], RP0[3];          // ray vectors
    double     r, a, b;             // params to calc ray-plane intersect
 
    // get triangle edge vectors and plane normal
    // u = T.V1 - T.V0;
    VEC3_MINUS_VEC3(TV1,TV0,u);
    // v = T.V2 - T.V0;
    VEC3_MINUS_VEC3(TV2,TV0,v);
    //n = u * v;             // cross product
    VEC3_CROSS_VEC3(u,v,n);
 
    // D is used for the direction of the intersecting ray
    for (int i=0; i<3; i++)
        dir[i] = SP1[i]-SP0[i];
 
    int res_IntrRT = intersect_RayTriangle(SP0, dir, TV0, TV1, TV2, I, &r);
 
    switch (res_IntrRT)
    {
    case 1: // intersection ray/triangle is a point
    {
        if (r > 1+SMALL_NUM) // if the triangle intersect the segment outside range SP0 SP1
            return 0;
        break;
    }
    case 2: // the ray is coplanar to the triangle
    {
        // test is at least 1 extremity of the segment lies into the triangle's domain
        double ITSP0, ISP0[3], ITSP1, ISP1[3];
        int resSP0;
        resSP0 = project_PointTriangle(SP0, TV0, TV1, TV2, ISP0, &ITSP0);
        project_PointTriangle(SP1, TV0, TV1, TV2, ISP1, &ITSP1);
 
        if (resSP0!=0) // 1st extremity of the segment is inside the triangle
            VEC3_ASSIGN_VEC3(ISP0,I);
        return 0;
    }
    }
    return res_IntrRT;
}
 
 
double OT_ALGORITHME_GEOMETRIQUE::Area_Triangle(double TV0[3], double TV1[3], double TV2[3])
{
    double S01[3],S12[3];
    for (int i=0;i<3;i++)
        S01[i]=TV1[i]-TV0[i];
    for (int i=0;i<3;i++)
        S12[i]=TV2[i]-TV1[i];
    double crossProd[3];
    VEC3_CROSS_VEC3(S01,S12,crossProd);
    double area = VEC3_NORM(crossProd);
    return area;
}