HUMIM Tracker: /home/hauberg/Dokumenter/Capture/humim-tracker-0.1/src/OpenTissue/OpenTissue/collision/intersect/intersect_triangle_triangle_interval

Go to the documentation of this file.
00001 #ifndef OPENTISSUE_COLLISION_INTERSECT_INTERSECT_TRIANGLE_TRIANGLE_INTERVAL_OVERLAP_H
00002 #define OPENTISSUE_COLLISION_INTERSECT_INTERSECT_TRIANGLE_TRIANGLE_INTERVAL_OVERLAP_H
00003 //
00004 // OpenTissue Template Library
00005 // - A generic toolbox for physics-based modeling and simulation.
00006 // Copyright (C) 2008 Department of Computer Science, University of Copenhagen.
00007 //
00008 // OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php
00009 //
00010 #include <OpenTissue/configuration.h>
00011 
00012 #include <OpenTissue/core/math/math_precision.h>
00013 
00014 #include <cmath>
00015 
00016 namespace OpenTissue
00017 {
00018   namespace intersect
00019   {
00020     /* Triangle/triangle intersection test routine,
00021     * by Tomas Moller, 1997.
00022     * See article "A Fast Triangle-Triangle Intersection Test",
00023     * Journal of Graphics Tools, 2(2), 1997
00024     * updated: 2001-06-20 (added line of intersection)
00025     *
00026     * int tri_tri_intersect(float V0[3],float V1[3],float V2[3],
00027     *                       float U0[3],float U1[3],float U2[3])
00028     *
00029     * parameters: vertices of triangle 1: V0,V1,V2
00030     *             vertices of triangle 2: U0,U1,U2
00031     * result    : returns 1 if the triangles intersect, otherwise 0
00032     *
00033     * Here is a version withouts divisions (a little faster)
00034     * int NoDivTriTriIsect(float V0[3],float V1[3],float V2[3],
00035     *                      float U0[3],float U1[3],float U2[3]);
00036     *
00037     * This version computes the line of intersection as well (if they are not coplanar):
00038     * int tri_tri_intersect_with_isectline(float V0[3],float V1[3],float V2[3],
00039     *               float U0[3],float U1[3],float U2[3],int *coplanar,
00040     *               float isectpt1[3],float isectpt2[3]);
00041     * coplanar returns whether the tris are coplanar
00042     * isectpt1, isectpt2 are the endpoints of the line of intersection
00043     */
00044 
00045     namespace Moller
00046     {
00047 
00048       namespace detail
00049       {
00050         // this edge to edge test is based on Franlin Antonio's gem:
00051         // "Faster Line Segment Intersection", in Graphics Gems III, pp. 199-202
00052 
00053         template <typename vector3_type, typename real_type>
00054         bool edge_edge_test(vector3_type const& V0, vector3_type const& U0, vector3_type const& U1,
00055           unsigned int i0, unsigned int i1, real_type Ax, real_type Ay)
00056         {
00057           real_type Bx=U0[i0]-U1[i0];
00058           real_type By=U0[i1]-U1[i1];
00059           real_type Cx=V0[i0]-U0[i0];
00060           real_type Cy=V0[i1]-U0[i1];
00061           real_type f=Ay*Bx-Ax*By;
00062           real_type d=By*Cx-Bx*Cy;
00063           if((f>0 && d>=0 && d<=f) || (f<0 && d<=0 && d>=f))
00064           {
00065             real_type e=Ax*Cy-Ay*Cx;
00066             if(f>0)
00067             {
00068               if(e>=0 && e<=f) return true;
00069             }
00070             else
00071             {
00072               if(e<=0 && e>=f) return true;
00073             }
00074           }
00075           else
00076             return false;
00077         }
00078 
00079         template <typename vector3_type, typename real_type>
00080         bool edge_against_tri_edges(vector3_type const& V0, vector3_type const& V1,
00081           vector3_type const& U0, vector3_type const& U1, vector3_type const& U2,
00082           unsigned int i0, unsigned int i1)
00083         {
00084           real_type Ax=V1[i0]-V0[i0];
00085           real_type Ay=V1[i1]-V0[i1];
00086           // test edge U0,U1 against V0,V1
00087           if ( edge_edge_test(V0,U0,U1,i0,i1,Ax,Ay) ) return true;
00088           // test edge U1,U2 against V0,V1
00089           if ( edge_edge_test(V0,U1,U2,i0,i1,Ax,Ay) ) return true;
00090           // test edge U2,U1 against V0,V1
00091           if ( edge_edge_test(V0,U2,U0,i0,i1,Ax,Ay) ) return true;
00092           return false;
00093         }
00094 
00095         template <typename vector3_type, typename real_type>
00096         bool point_in_tri(vector3_type const& V0
00097           vector3_type const& U0, vector3_type const& U1, vector3_type const& U2,
00098           unsigned int i0, unsigned int i1)
00099         {
00100           // is T1 completly inside T2?
00101           // check if V0 is inside tri(U0,U1,U2)
00102           real_type a=U1[i1]-U0[i1];
00103           real_type b=-(U1[i0]-U0[i0]);
00104           real_type c=-a*U0[i0]-b*U0[i1];
00105           real_type d0=a*V0[i0]+b*V0[i1]+c;
00106 
00107           a=U2[i1]-U1[i1];
00108           b=-(U2[i0]-U1[i0]);
00109           c=-a*U1[i0]-b*U1[i1];
00110           real_type d1=a*V0[i0]+b*V0[i1]+c;
00111 
00112           a=U0[i1]-U2[i1];
00113           b=-(U0[i0]-U2[i0]);
00114           c=-a*U2[i0]-b*U2[i1];
00115           real_type d2=a*V0[i0]+b*V0[i1]+c;
00116           if(d0*d1>0.0)
00117           {
00118             if(d0*d2>0.0) return true;
00119           }
00120           return false;
00121         }
00122 
00123         template <typename vector3_type>
00124         bool coplanar_tri_tri(vector3_type const& N,
00125           vector3_type const& V0, vector3_type const& V1, vector3_type const& V2,
00126           vector3_type const& U0, vector3_type const& U1, vector3_type const& U2)
00127         {
00128           typedef typename vector3_type::value_type real_type;
00129 
00130           unsigned int i0,i1;
00131           // first project onto an axis-aligned plane, that maximizes the area
00132           // of the triangles, compute indices: i0,i1.
00133           real_type A0=std::fabs(N[0]);
00134           real_type A1=std::fabs(N[1]);
00135           real_type A2=std::fabs(N[2]);
00136           if(A0>A1)
00137           {
00138             if(A0>A2)
00139             {
00140               i0=1;      // A[0] is greatest
00141               i1=2;
00142             }
00143             else
00144             {
00145               i0=0;      // A[2] is greatest
00146               i1=1;
00147             }
00148           }
00149           else   // A[0]<=A[1]
00150           {
00151             if(A2>A1)
00152             {
00153               i0=0;      // A[2] is greatest
00154               i1=1;
00155             }
00156             else
00157             {
00158               i0=0;      // A[1] is greatest
00159               i1=2;
00160             }
00161           }
00162 
00163           // test all edges of triangle 1 against the edges of triangle 2
00164           if( edge_against_tri_edges(V0,V1,U0,U1,U2,i0,i1) ) return true;
00165           if( edge_against_tri_edges(V1,V2,U0,U1,U2,i0,i1) ) return true;
00166           if( edge_against_tri_edges(V2,V0,U0,U1,U2,i0,i1) ) return true;
00167 
00168           // finally, test if tri1 is totally contained in tri2 or vice versa
00169           if( point_in_tri(V0,U0,U1,U2,i0,i1) ) return true;
00170           if( point_in_tri(U0,V0,V1,V2,i0,i1) ) return true;
00171 
00172           return false;
00173         }
00174 
00175         template <typename vector3_type, typename real_type>
00176         void isect2(vector3_type const& VTX0, vector3_type const& VTX1, vector3_type const& VTX2,
00177           real_type VV0, real_type VV1, real_type VV2,
00178           real_type D0, real_type D1, real_type D2,
00179           real_type & isect0, real_type & isect1, vector3_type & isectpoint0, vector3_type & isectpoint1)
00180         {
00181           real_type tmp;
00182           vector3_type diff;
00183           tmp=D0/(D0-D1);
00184           isect0 = VV0+(VV1-VV0)*tmp;
00185           diff = VTX1-VTX2;
00186           diff *= tmp;
00187           isectpoint0 = diff+VTX0;
00188           tmp=D0/(D0-D2);
00189           isect1 = VV0+(VV2-VV0)*tmp;
00190           diff = VTX2-VTX0;
00191           diff *= tmp;
00192           isectpoint1 = VTX0 + diff;
00193         }
00194 
00195         template <typename vector3_type, typename real_type>
00196         bool compute_intervals_isectline(vector3_type const& VERT0, vector3_type const& VERT1, vector3_type const& VERT2,
00197           real_type VV0, real_type VV1, real_type VV2, real_type D0, real_type D1, real_type D2,
00198           real_type D0D1, real_type D0D2,
00199           real_type & isect0, real_type & isect1,
00200           vector3_type & isectpoint0, vector3_type & isectpoint1)
00201         {
00202           if(D0D1>0.0f)
00203           {
00204             // here we know that D0D2<=0.0
00205             // that is D0, D1 are on the same side, D2 on the other or on the plane
00206             isect2(VERT2,VERT0,VERT1,VV2,VV0,VV1,D2,D0,D1,isect0,isect1,isectpoint0,isectpoint1);
00207           }
00208           else if(D0D2>0.0f)
00209           {
00210             // here we know that d0d1<=0.0
00211             isect2(VERT1,VERT0,VERT2,VV1,VV0,VV2,D1,D0,D2,isect0,isect1,isectpoint0,isectpoint1);
00212           }
00213           else if(D1*D2>0.0f || D0!=0.0f)
00214           {
00215             // here we know that d0d1<=0.0 or that D0!=0.0
00216             isect2(VERT0,VERT1,VERT2,VV0,VV1,VV2,D0,D1,D2,isect0,isect1,isectpoint0,isectpoint1);
00217           }
00218           else if(D1!=0.0f)
00219           {
00220             isect2(VERT1,VERT0,VERT2,VV1,VV0,VV2,D1,D0,D2,isect0,isect1,isectpoint0,isectpoint1);
00221           }
00222           else if(D2!=0.0f)
00223           {
00224             isect2(VERT2,VERT0,VERT1,VV2,VV0,VV1,D2,D0,D1,isect0,isect1,isectpoint0,isectpoint1);
00225           }
00226           else
00227           {
00228             // triangles are coplanar
00229             return true;
00230           }
00231           return false;
00232         }
00233 
00234         template <typename vector3_type>
00235         bool triangle_triangle_interval_overlap(   vector3_type const & V0, vector3_type const & V1, vector3_type const & V2
00236           , vector3_type const & U0, vector3_type const & U1, vector3_type const & U2
00237           , bool & coplanar        , vector3_type & isectpt1, vector3_type & isectpt2
00238           , bool epsilon_test = true )
00239         {
00240           typedef typename vector3_type::value_type real_type;
00241 
00242           // compute plane equation of triangle(V0,V1,V2)
00243           vector3_type E1 = V1-V0;
00244           vector3_type E2 = V2-V0;
00245           vector3_type N1 = cross(E1,E2);
00246           real_type d1 = -dot(N1,V0);
00247           // plane equation 1: N1.X+d1=0
00248 
00249           // put U0,U1,U2 into plane equation 1 to compute signed distances to the plane
00250           real_type du0 = dot(N1,U0)+d1;
00251           real_type du1 = dot(N1,U1)+d1;
00252           real_type du2 = dot(N1,U2)+d1;
00253 
00254           // coplanarity robustness check
00255           if (epsilon_test)
00256           {
00257             if( std::fabs(du0) < math::working_precision<real_type>() ) du0=0.0;
00258             if( std::fabs(du1) < math::working_precision<real_type>() ) du1=0.0;
00259             if( std::fabs(du2) < math::working_precision<real_type>() ) du2=0.0;
00260           }
00261 
00262           real_type du0du1=du0*du1;
00263           real_type du0du2=du0*du2;
00264 
00265           if(du0du1>0.0f && du0du2>0.0f) // same sign on all of them + not equal 0 ?
00266             return false;                // no intersection occurs
00267 
00268           // compute plane of triangle (U0,U1,U2)
00269           E1 = U1-U0;
00270           E2 = U2,U0;
00271           vector3_type N2 = cross(E1,E2);
00272           real_type d2 = -dot(N2,U0);
00273           // plane equation 2: N2.X+d2=0
00274 
00275           // put V0,V1,V2 into plane equation 2
00276           real_type dv0 = dot(N2,V0)+d2;
00277           real_type dv1 = dot(N2,V1)+d2;
00278           real_type dv2 = dot(N2,V2)+d2;
00279 
00280           // coplanarity robustness check
00281           if (epsilon_test)
00282           {
00283             if( std::fabs(dv0) < math::working_precision<real_type>() ) dv0=0.0;
00284             if( std::fabs(dv1) < math::working_precision<real_type>() ) dv1=0.0;
00285             if( std::fabs(dv2) < math::working_precision<real_type>() ) dv2=0.0;
00286           }
00287 
00288           real_type dv0dv1 = dv0*dv1;
00289           real_type dv0dv2 = dv0*dv2;
00290 
00291           if(dv0dv1>0.0f && dv0dv2>0.0f) // same sign on all of them + not equal 0 ?
00292             return false;                // no intersection occurs
00293 
00294           // compute direction of intersection line
00295           vector3_type D = cross(N1,N2);
00296 
00297           // compute and index to the largest component of D
00298           real_type max=std::fabs(D[0]);
00299           unsigned int index=0;
00300           real_type b=std::fabs(D[1]);
00301           real_type c=std::fabs(D[2]);
00302           if(b>max) max=b,index=1;
00303           if(c>max) max=c,index=2;
00304 
00305           // this is the simplified projection onto L
00306           real_type vp0 = V0[index];
00307           real_type vp1 = V1[index];
00308           real_type vp2 = V2[index];
00309 
00310           real_type up0 = U0[index];
00311           real_type up1 = U1[index];
00312           real_type up2 = U2[index];
00313 
00314           real_type isect1[2], isect2[2];
00315           vector3_type isectpointA1, isectpointA2;
00316 
00317           // compute interval for triangle 1
00318           coplanar=compute_intervals_isectline(V0,V1,V2,vp0,vp1,vp2,dv0,dv1,dv2,
00319             dv0dv1,dv0dv2,&isect1[0],&isect1[1],isectpointA1,isectpointA2);
00320           if(coplanar) return coplanar_tri_tri(N1,V0,V1,V2,U0,U1,U2);
00321 
00322           vector3_type isectpointB1, isectpointB2;
00323 
00324           // compute interval for triangle 2
00325           compute_intervals_isectline(U0,U1,U2,up0,up1,up2,du0,du1,du2,
00326             du0du1,du0du2,&isect2[0],&isect2[1],isectpointB1,isectpointB2);
00327 
00328           // sort so that a<=b
00329           unsigned int smallest1 = 0;
00330           if (isect1[0] > isect1[1])
00331           {
00332             swap(isect1[0], isect1[1]);
00333             smallest1=1;
00334           }
00335           unsigned int smallest2 = 0;
00336           if (isect2[0] > isect2[1])
00337           {
00338             swap(isect2[0], isect2[1]);
00339             smallest2=1;
00340           }
00341 
00342           if(isect1[1]<isect2[0] || isect2[1]<isect1[0]) return false;
00343 
00344           // at this point, we know that the triangles intersect
00345 
00346           if(isect2[0]<isect1[0])
00347           {
00348             isectpt1 = smallest1==0 ? isectpointA1 : isectpointA2;
00349 
00350             if(isect2[1]<isect1[1])
00351             {
00352               isectpt2 = smallest2==0 ? isectpointB2 : isectpointB1;
00353             }
00354             else
00355             {
00356               isectpt2 = smallest1==0 ? isectpointA2 : isectpointA1;
00357             }
00358           }
00359           else
00360           {
00361             isectpt1 = smallest2==0 ? isectpointB1 : isectpointB2;
00362 
00363             if(isect2[1]>isect1[1])
00364             {
00365               isectpt2 = smallest1==0 ? isectpointA2 : isectpointA1;
00366             }
00367             else
00368             {
00369               isectpt2 = smallest2==0 ? isectpointB2 : isectpointB1;
00370             }
00371           }
00372           return true;
00373         }
00374 
00375       } // namespace detail
00376 
00377 
00378       template <typename triangle_type>
00379       bool triangle_triangle_interval_overlap( triangle_type const & t0, triangle_type const & t1, bool epsilon_test = true )
00380       {
00381         return triangle_triangle_interval_overlap(t0.p0(), t0.p1(), t0.p2(), t1.p0(), t1.p1(), t1.p2(),epsilon_test);
00382       }
00383 
00384       template <typename triangle_type, typename vector3_type>
00385       bool triangle_triangle_interval_overlap(   triangle_type const & t0, triangle_type const & t1
00386         , bool & coplanar, vector3_type & p0, vector3_type & p1
00387         , bool epsilon_test = true )
00388       {
00389         return triangle_triangle_interval_overlap(t0.p0(), t0.p1(), t0.p2(), t1.p0(), t1.p1(), t1.p2(), coplanar, p0, p1, epsilon_test);
00390       }
00391 
00392       template <typename vector3_type>
00393       bool triangle_triangle_interval_overlap(   vector3_type const & v0, vector3_type const & v1, vector3_type const & v2
00394         , vector3_type const & u0, vector3_type const & u1, vector3_type const & u2
00395         , bool epsilon_test = true )
00396       {
00397         bool dummy_bool;
00398         vector3_type dummy_v0;
00399         vector3_type dummy_v1;
00400         return triangle_triangle_interval_overlap(v0,v1,v2,u0,u1,u2,dummy_bool,dummy_v1,dummy_v2,epsilon_test);
00401 
00402       }
00403       template <typename vector3_type>
00404       bool triangle_triangle_interval_overlap(   vector3_type const & v0, vector3_type const & v1, vector3_type const & v2
00405         , vector3_type const & u0, vector3_type const & u1, vector3_type const & u2
00406         , bool & coplanar        , vector3_type       & p0, vector3_type       & p1
00407         , bool epsilon_test = true )
00408       {
00409         return detail::triangle_triangle_interval_overlap(v0,v1,v2,u0,u1,u2,coplanar,p0,p1,epsilon_test);
00410       }
00411 
00412       // NOTE: This is the old macro-code. To be deleted when above has been tested.
00413       namespace old_stuff_soon_dies
00414       {
00415         bool wrapNoDivTriTriIntersect(const Triangle & triA,const Triangle & triB);
00416         bool wrapTriTriIntersectLine(const Triangle & triA,const Triangle & triB,bool & coplanar,Vector3<double> & pt1,Vector3<double> & pt2);
00417         bool wrapTriTriIntersect(const Triangle & triA,const Triangle & triB);
00418 
00419         bool NoDivTriTriIsect(float V0[3],float V1[3],float V2[3], float U0[3],float U1[3],float U2[3]);
00420         bool tri_tri_intersect_with_isectline(float V0[3],float V1[3],float V2[3], float U0[3],float U1[3],float U2[3],int *coplanar, float isectpt1[3],float isectpt2[3]);
00421         bool tri_tri_intersect(float V0[3],float V1[3],float V2[3], float U0[3],float U1[3],float U2[3]);
00422         /* Triangle/triangle intersection test routine,
00423         * by Tomas Moller, 1997.
00424         * See article "A Fast Triangle-Triangle Intersection Test",
00425         * Journal of Graphics Tools, 2(2), 1997
00426         * updated: 2001-06-20 (added line of intersection)
00427         *
00428         * int tri_tri_intersect(float V0[3],float V1[3],float V2[3],
00429         *                       float U0[3],float U1[3],float U2[3])
00430         *
00431         * parameters: vertices of triangle 1: V0,V1,V2
00432         *             vertices of triangle 2: U0,U1,U2
00433         * result    : returns 1 if the triangles intersect, otherwise 0
00434         *
00435         * Here is a version withouts divisions (a little faster)
00436         * int NoDivTriTriIsect(float V0[3],float V1[3],float V2[3],
00437         *                      float U0[3],float U1[3],float U2[3]);
00438         *
00439         * This version computes the line of intersection as well (if they are not coplanar):
00440         * int tri_tri_intersect_with_isectline(float V0[3],float V1[3],float V2[3],
00441         *               float U0[3],float U1[3],float U2[3],int *coplanar,
00442         *               float isectpt1[3],float isectpt2[3]);
00443         * coplanar returns whether the tris are coplanar
00444         * isectpt1, isectpt2 are the endpoints of the line of intersection
00445         */
00446 
00447         // TODO: Macros Everywhere. Yikes!
00448 
00449 #include <math.h>
00450 
00451 #define FABS(x) ((float)std::fabs(x))        /* implement as is fastest on your machine */
00452 
00453         /* if USE_EPSILON_TEST is true then we do a check:
00454         if |dv|<EPSILON then dv=0.0;
00455         else no check is done (which is less robust)
00456         */
00457 #define USE_EPSILON_TEST TRUE
00458 #define EPSILON 0.000001
00459 
00460 
00461         /* some macros */
00462 #define CROSS(dest,v1,v2)                      \
00463   dest[0]=v1[1]*v2[2]-v1[2]*v2[1]; \
00464   dest[1]=v1[2]*v2[0]-v1[0]*v2[2]; \
00465   dest[2]=v1[0]*v2[1]-v1[1]*v2[0];
00466 
00467 #define DOT(v1,v2) (v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2])
00468 
00469 #define SUB(dest,v1,v2) dest[0]=v1[0]-v2[0]; dest[1]=v1[1]-v2[1]; dest[2]=v1[2]-v2[2];
00470 
00471 #define ADD(dest,v1,v2) dest[0]=v1[0]+v2[0]; dest[1]=v1[1]+v2[1]; dest[2]=v1[2]+v2[2];
00472 
00473 #define MULT(dest,v,factor) dest[0]=factor*v[0]; dest[1]=factor*v[1]; dest[2]=factor*v[2];
00474 
00475 #define SET(dest,src) dest[0]=src[0]; dest[1]=src[1]; dest[2]=src[2];
00476 
00477         /* sort so that a<=b */
00478 #define SORT(a,b)       \
00479   if(a>b)    \
00480         {          \
00481         float MACROTEMP; \
00482         MACROTEMP=a;     \
00483         a=b;     \
00484         b=MACROTEMP;     \
00485         }
00486 
00487 #define ISECT(VV0,VV1,VV2,D0,D1,D2,isect0,isect1) \
00488   isect0=VV0+(VV1-VV0)*D0/(D0-D1);    \
00489   isect1=VV0+(VV2-VV0)*D0/(D0-D2);
00490 
00491 
00492 #define COMPUTE_INTERVALS(VV0,VV1,VV2,D0,D1,D2,D0D1,D0D2,isect0,isect1) \
00493   if(D0D1>0.0f)                                         \
00494         {                                                     \
00495         /* here we know that D0D2<=0.0 */                   \
00496         /* that is D0, D1 are on the same side, D2 on the other or on the plane */ \
00497         ISECT(VV2,VV0,VV1,D2,D0,D1,isect0,isect1);          \
00498         }                                                     \
00499   else if(D0D2>0.0f)                                    \
00500         {                                                     \
00501         /* here we know that d0d1<=0.0 */                   \
00502         ISECT(VV1,VV0,VV2,D1,D0,D2,isect0,isect1);          \
00503         }                                                     \
00504   else if(D1*D2>0.0f || D0!=0.0f)                       \
00505         {                                                     \
00506         /* here we know that d0d1<=0.0 or that D0!=0.0 */   \
00507         ISECT(VV0,VV1,VV2,D0,D1,D2,isect0,isect1);          \
00508         }                                                     \
00509   else if(D1!=0.0f)                                     \
00510         {                                                     \
00511         ISECT(VV1,VV0,VV2,D1,D0,D2,isect0,isect1);          \
00512         }                                                     \
00513   else if(D2!=0.0f)                                     \
00514         {                                                     \
00515         ISECT(VV2,VV0,VV1,D2,D0,D1,isect0,isect1);          \
00516         }                                                     \
00517   else                                                  \
00518         {                                                     \
00519         /* triangles are coplanar */                        \
00520         return coplanar_tri_tri(N1,V0,V1,V2,U0,U1,U2);      \
00521         }
00522 
00523 
00524 
00525         /* this edge to edge test is based on Franlin Antonio's gem:
00526         "Faster Line Segment Intersection", in Graphics Gems III,
00527         pp. 199-202 */
00528 #define EDGE_EDGE_TEST(V0,U0,U1)                      \
00529   Bx=U0[i0]-U1[i0];                                   \
00530   By=U0[i1]-U1[i1];                                   \
00531   Cx=V0[i0]-U0[i0];                                   \
00532   Cy=V0[i1]-U0[i1];                                   \
00533   f=Ay*Bx-Ax*By;                                      \
00534   d=By*Cx-Bx*Cy;                                      \
00535   if((f>0 && d>=0 && d<=f) || (f<0 && d<=0 && d>=f))  \
00536         {                                                   \
00537         e=Ax*Cy-Ay*Cx;                                    \
00538         if(f>0)                                           \
00539         {                                                 \
00540         if(e>=0 && e<=f) return 1;                      \
00541         }                                                 \
00542     else                                              \
00543         {                                                 \
00544         if(e<=0 && e>=f) return 1;                      \
00545         }                                                 \
00546         }
00547 
00548 #define EDGE_AGAINST_TRI_EDGES(V0,V1,U0,U1,U2) \
00549         {                                              \
00550         float Ax,Ay,Bx,By,Cx,Cy,e,d,f;               \
00551         Ax=V1[i0]-V0[i0];                            \
00552         Ay=V1[i1]-V0[i1];                            \
00553         /* test edge U0,U1 against V0,V1 */          \
00554         EDGE_EDGE_TEST(V0,U0,U1);                    \
00555         /* test edge U1,U2 against V0,V1 */          \
00556         EDGE_EDGE_TEST(V0,U1,U2);                    \
00557         /* test edge U2,U1 against V0,V1 */          \
00558         EDGE_EDGE_TEST(V0,U2,U0);                    \
00559         }
00560 
00561 #define POINT_IN_TRI(V0,U0,U1,U2)           \
00562         {                                           \
00563         float a,b,c,d0,d1,d2;                     \
00564         /* is T1 completly inside T2? */          \
00565         /* check if V0 is inside tri(U0,U1,U2) */ \
00566         a=U1[i1]-U0[i1];                          \
00567         b=-(U1[i0]-U0[i0]);                       \
00568         c=-a*U0[i0]-b*U0[i1];                     \
00569         d0=a*V0[i0]+b*V0[i1]+c;                   \
00570         \
00571         a=U2[i1]-U1[i1];                          \
00572         b=-(U2[i0]-U1[i0]);                       \
00573         c=-a*U1[i0]-b*U1[i1];                     \
00574         d1=a*V0[i0]+b*V0[i1]+c;                   \
00575         \
00576         a=U0[i1]-U2[i1];                          \
00577         b=-(U0[i0]-U2[i0]);                       \
00578         c=-a*U2[i0]-b*U2[i1];                     \
00579         d2=a*V0[i0]+b*V0[i1]+c;                   \
00580         if(d0*d1>0.0)                             \
00581         {                                         \
00582         if(d0*d2>0.0) return 1;                 \
00583         }                                         \
00584         }
00585 
00586         bool coplanar_tri_tri(float N[3],float V0[3],float V1[3],float V2[3],
00587           float U0[3],float U1[3],float U2[3])
00588         {
00589           float A[3];
00590           short i0,i1;
00591           /* first project onto an axis-aligned plane, that maximizes the area */
00592           /* of the triangles, compute indices: i0,i1. */
00593           A[0]=std::fabs(N[0]);
00594           A[1]=std::fabs(N[1]);
00595           A[2]=std::fabs(N[2]);
00596           if(A[0]>A[1])
00597           {
00598             if(A[0]>A[2])
00599             {
00600               i0=1;      /* A[0] is greatest */
00601               i1=2;
00602             }
00603             else
00604             {
00605               i0=0;      /* A[2] is greatest */
00606               i1=1;
00607             }
00608           }
00609           else   /* A[0]<=A[1] */
00610           {
00611             if(A[2]>A[1])
00612             {
00613               i0=0;      /* A[2] is greatest */
00614               i1=1;
00615             }
00616             else
00617             {
00618               i0=0;      /* A[1] is greatest */
00619               i1=2;
00620             }
00621           }
00622 
00623           /* test all edges of triangle 1 against the edges of triangle 2 */
00624           EDGE_AGAINST_TRI_EDGES(V0,V1,U0,U1,U2);
00625           EDGE_AGAINST_TRI_EDGES(V1,V2,U0,U1,U2);
00626           EDGE_AGAINST_TRI_EDGES(V2,V0,U0,U1,U2);
00627 
00628           /* finally, test if tri1 is totally contained in tri2 or vice versa */
00629           POINT_IN_TRI(V0,U0,U1,U2);
00630           POINT_IN_TRI(U0,V0,V1,V2);
00631 
00632           return false;
00633         }
00634 
00635 
00636 
00641         bool wrapTriTriIntersect(const Triangle & triA,const Triangle & triB)
00642         {
00643           float V0[3];
00644           V0[0] = static_cast<float>(triA.p0()[0]);
00645           V0[1] = static_cast<float>(triA.p0()[1]);
00646           V0[2] = static_cast<float>(triA.p0()[2]);
00647           float V1[3];
00648           V1[0] = static_cast<float>(triA.p1()[0]);
00649           V1[1] = static_cast<float>(triA.p1()[1]);
00650           V1[2] = static_cast<float>(triA.p1()[2]);
00651           float V2[3];
00652           V2[0] = static_cast<float>(triA.p2()[0]);
00653           V2[1] = static_cast<float>(triA.p2()[1]);
00654           V2[2] = static_cast<float>(triA.p2()[2]);
00655 
00656           float U0[3];
00657           U0[0] = static_cast<float>(triB.p0()[0]);
00658           U0[1] = static_cast<float>(triB.p0()[1]);
00659           U0[2] = static_cast<float>(triB.p0()[2]);
00660           float U1[3];
00661           U1[0] = static_cast<float>(triB.p1()[0]);
00662           U1[1] = static_cast<float>(triB.p1()[1]);
00663           U1[2] = static_cast<float>(triB.p1()[2]);
00664           float U2[3];
00665           U2[0] = static_cast<float>(triB.p2()[0]);
00666           U2[1] = static_cast<float>(triB.p2()[1]);
00667           U2[2] = static_cast<float>(triB.p2()[2]);
00668           return tri_tri_intersect(V0,V1,V2,U0,U1,U2);
00669         }
00670 
00671 
00672         bool tri_tri_intersect(float V0[3],float V1[3],float V2[3],
00673           float U0[3],float U1[3],float U2[3])
00674         {
00675           float E1[3],E2[3];
00676           float N1[3],N2[3],d1,d2;
00677           float du0,du1,du2,dv0,dv1,dv2;
00678           float D[3];
00679           float isect1[2], isect2[2];
00680           float du0du1,du0du2,dv0dv1,dv0dv2;
00681           short index;
00682           float vp0,vp1,vp2;
00683           float up0,up1,up2;
00684           float b,c,max;
00685 
00686           /* compute plane equation of triangle(V0,V1,V2) */
00687           SUB(E1,V1,V0);
00688           SUB(E2,V2,V0);
00689           CROSS(N1,E1,E2);
00690           d1=-DOT(N1,V0);
00691           /* plane equation 1: N1.X+d1=0 */
00692 
00693           /* put U0,U1,U2 into plane equation 1 to compute signed distances to the plane*/
00694           du0=DOT(N1,U0)+d1;
00695           du1=DOT(N1,U1)+d1;
00696           du2=DOT(N1,U2)+d1;
00697 
00698           /* coplanarity robustness check */
00699 #if USE_EPSILON_TEST==TRUE
00700           if(std::fabs(du0)<EPSILON) du0=0.0;
00701           if(std::fabs(du1)<EPSILON) du1=0.0;
00702           if(std::fabs(du2)<EPSILON) du2=0.0;
00703 #endif
00704           du0du1=du0*du1;
00705           du0du2=du0*du2;
00706 
00707           if(du0du1>0.0f && du0du2>0.0f) /* same sign on all of them + not equal 0 ? */
00708             return false;                    /* no intersection occurs */
00709 
00710           /* compute plane of triangle (U0,U1,U2) */
00711           SUB(E1,U1,U0);
00712           SUB(E2,U2,U0);
00713           CROSS(N2,E1,E2);
00714           d2=-DOT(N2,U0);
00715           /* plane equation 2: N2.X+d2=0 */
00716 
00717           /* put V0,V1,V2 into plane equation 2 */
00718           dv0=DOT(N2,V0)+d2;
00719           dv1=DOT(N2,V1)+d2;
00720           dv2=DOT(N2,V2)+d2;
00721 
00722 #if USE_EPSILON_TEST==TRUE
00723           if(std::fabs(dv0)<EPSILON) dv0=0.0;
00724           if(std::fabs(dv1)<EPSILON) dv1=0.0;
00725           if(std::fabs(dv2)<EPSILON) dv2=0.0;
00726 #endif
00727 
00728           dv0dv1=dv0*dv1;
00729           dv0dv2=dv0*dv2;
00730 
00731           if(dv0dv1>0.0f && dv0dv2>0.0f) /* same sign on all of them + not equal 0 ? */
00732             return false;                    /* no intersection occurs */
00733 
00734           /* compute direction of intersection line */
00735           CROSS(D,N1,N2);
00736 
00737           /* compute and index to the largest component of D */
00738           max=std::fabs(D[0]);
00739           index=0;
00740           b=std::fabs(D[1]);
00741           c=std::fabs(D[2]);
00742           if(b>max) max=b,index=1;
00743           if(c>max) max=c,index=2;
00744 
00745           /* this is the simplified projection onto L*/
00746           vp0=V0[index];
00747           vp1=V1[index];
00748           vp2=V2[index];
00749 
00750           up0=U0[index];
00751           up1=U1[index];
00752           up2=U2[index];
00753 
00754           /* compute interval for triangle 1 */
00755           // TODO: Comparing floats with ==
00756           COMPUTE_INTERVALS(vp0,vp1,vp2,dv0,dv1,dv2,dv0dv1,dv0dv2,isect1[0],isect1[1]);
00757 
00758           /* compute interval for triangle 2 */
00759           // TODO: Comparing floats with ==
00760           COMPUTE_INTERVALS(up0,up1,up2,du0,du1,du2,du0du1,du0du2,isect2[0],isect2[1]);
00761 
00762           SORT(isect1[0],isect1[1]);
00763           SORT(isect2[0],isect2[1]);
00764 
00765           if(isect1[1]<isect2[0] || isect2[1]<isect1[0]) return false;
00766           return true;
00767         }
00768 
00769 
00770 #define NEWCOMPUTE_INTERVALS(VV0,VV1,VV2,D0,D1,D2,D0D1,D0D2,A,B,C,X0,X1) \
00771         { \
00772         if(D0D1>0.0f) \
00773         { \
00774         /* here we know that D0D2<=0.0 */ \
00775         /* that is D0, D1 are on the same side, D2 on the other or on the plane */ \
00776         A=VV2; B=(VV0-VV2)*D2; C=(VV1-VV2)*D2; X0=D2-D0; X1=D2-D1; \
00777         } \
00778   else if(D0D2>0.0f)\
00779         { \
00780         /* here we know that d0d1<=0.0 */ \
00781         A=VV1; B=(VV0-VV1)*D1; C=(VV2-VV1)*D1; X0=D1-D0; X1=D1-D2; \
00782         } \
00783   else if(D1*D2>0.0f || D0!=0.0f) \
00784         { \
00785         /* here we know that d0d1<=0.0 or that D0!=0.0 */ \
00786         A=VV0; B=(VV1-VV0)*D0; C=(VV2-VV0)*D0; X0=D0-D1; X1=D0-D2; \
00787         } \
00788   else if(D1!=0.0f) \
00789         { \
00790         A=VV1; B=(VV0-VV1)*D1; C=(VV2-VV1)*D1; X0=D1-D0; X1=D1-D2; \
00791         } \
00792   else if(D2!=0.0f) \
00793         { \
00794         A=VV2; B=(VV0-VV2)*D2; C=(VV1-VV2)*D2; X0=D2-D0; X1=D2-D1; \
00795         } \
00796   else \
00797         { \
00798         /* triangles are coplanar */ \
00799         return coplanar_tri_tri(N1,V0,V1,V2,U0,U1,U2); \
00800         } \
00801         }
00802 
00803 
00804 
00805 
00810         bool wrapNoDivTriTriIntersect(const Triangle & triA,const Triangle & triB)
00811         {
00812           float V0[3];
00813           V0[0] = static_cast<float>(triA.p0()[0]);
00814           V0[1] = static_cast<float>(triA.p0()[1]);
00815           V0[2] = static_cast<float>(triA.p0()[2]);
00816           float V1[3];
00817           V1[0] = static_cast<float>(triA.p1()[0]);
00818           V1[1] = static_cast<float>(triA.p1()[1]);
00819           V1[2] = static_cast<float>(triA.p1()[2]);
00820           float V2[3];
00821           V2[0] = static_cast<float>(triA.p2()[0]);
00822           V2[1] = static_cast<float>(triA.p2()[1]);
00823           V2[2] = static_cast<float>(triA.p2()[2]);
00824 
00825           float U0[3];
00826           U0[0] = static_cast<float>(triB.p0()[0]);
00827           U0[1] = static_cast<float>(triB.p0()[1]);
00828           U0[2] = static_cast<float>(triB.p0()[2]);
00829           float U1[3];
00830           U1[0] = static_cast<float>(triB.p1()[0]);
00831           U1[1] = static_cast<float>(triB.p1()[1]);
00832           U1[2] = static_cast<float>(triB.p1()[2]);
00833           float U2[3];
00834           U2[0] = static_cast<float>(triB.p2()[0]);
00835           U2[1] = static_cast<float>(triB.p2()[1]);
00836           U2[2] = static_cast<float>(triB.p2()[2]);
00837           return NoDivTriTriIsect(V0,V1,V2,U0,U1,U2);
00838         }
00839 
00840 
00841 
00842         bool NoDivTriTriIsect(float V0[3],float V1[3],float V2[3],
00843           float U0[3],float U1[3],float U2[3])
00844         {
00845           float E1[3],E2[3];
00846           float N1[3],N2[3],d1,d2;
00847           float du0,du1,du2,dv0,dv1,dv2;
00848           float D[3];
00849           float isect1[2], isect2[2];
00850           float du0du1,du0du2,dv0dv1,dv0dv2;
00851           short index;
00852           float vp0,vp1,vp2;
00853           float up0,up1,up2;
00854           float bb,cc,max;
00855           float a,b,c,x0,x1;
00856           float d,e,f,y0,y1;
00857           float xx,yy,xxyy,tmp;
00858 
00859           /* compute plane equation of triangle(V0,V1,V2) */
00860           SUB(E1,V1,V0);
00861           SUB(E2,V2,V0);
00862           CROSS(N1,E1,E2);
00863           d1=-DOT(N1,V0);
00864           /* plane equation 1: N1.X+d1=0 */
00865 
00866           /* put U0,U1,U2 into plane equation 1 to compute signed distances to the plane*/
00867           du0=DOT(N1,U0)+d1;
00868           du1=DOT(N1,U1)+d1;
00869           du2=DOT(N1,U2)+d1;
00870 
00871           /* coplanarity robustness check */
00872 #if USE_EPSILON_TEST==TRUE
00873           if(FABS(du0)<EPSILON) du0=0.0;
00874           if(FABS(du1)<EPSILON) du1=0.0;
00875           if(FABS(du2)<EPSILON) du2=0.0;
00876 #endif
00877           du0du1=du0*du1;
00878           du0du2=du0*du2;
00879 
00880           if(du0du1>0.0f && du0du2>0.0f) /* same sign on all of them + not equal 0 ? */
00881             return false;                    /* no intersection occurs */
00882 
00883           /* compute plane of triangle (U0,U1,U2) */
00884           SUB(E1,U1,U0);
00885           SUB(E2,U2,U0);
00886           CROSS(N2,E1,E2);
00887           d2=-DOT(N2,U0);
00888           /* plane equation 2: N2.X+d2=0 */
00889 
00890           /* put V0,V1,V2 into plane equation 2 */
00891           dv0=DOT(N2,V0)+d2;
00892           dv1=DOT(N2,V1)+d2;
00893           dv2=DOT(N2,V2)+d2;
00894 
00895 #if USE_EPSILON_TEST==TRUE
00896           if(FABS(dv0)<EPSILON) dv0=0.0;
00897           if(FABS(dv1)<EPSILON) dv1=0.0;
00898           if(FABS(dv2)<EPSILON) dv2=0.0;
00899 #endif
00900 
00901           dv0dv1=dv0*dv1;
00902           dv0dv2=dv0*dv2;
00903 
00904           if(dv0dv1>0.0f && dv0dv2>0.0f) /* same sign on all of them + not equal 0 ? */
00905             return false;                    /* no intersection occurs */
00906 
00907           /* compute direction of intersection line */
00908           CROSS(D,N1,N2);
00909 
00910           /* compute and index to the largest component of D */
00911           max=(float)FABS(D[0]);
00912           index=0;
00913           bb=(float)FABS(D[1]);
00914           cc=(float)FABS(D[2]);
00915           if(bb>max) max=bb,index=1;
00916           if(cc>max) max=cc,index=2;
00917 
00918           /* this is the simplified projection onto L*/
00919           vp0=V0[index];
00920           vp1=V1[index];
00921           vp2=V2[index];
00922 
00923           up0=U0[index];
00924           up1=U1[index];
00925           up2=U2[index];
00926 
00927           /* compute interval for triangle 1 */
00928           // TODO: Comparing floats with ==
00929           NEWCOMPUTE_INTERVALS(vp0,vp1,vp2,dv0,dv1,dv2,dv0dv1,dv0dv2,a,b,c,x0,x1);
00930 
00931           /* compute interval for triangle 2 */
00932           // TODO: Comparing floats with ==
00933           NEWCOMPUTE_INTERVALS(up0,up1,up2,du0,du1,du2,du0du1,du0du2,d,e,f,y0,y1);
00934 
00935           xx=x0*x1;
00936           yy=y0*y1;
00937           xxyy=xx*yy;
00938 
00939           tmp=a*xxyy;
00940           isect1[0]=tmp+b*x1*yy;
00941           isect1[1]=tmp+c*x0*yy;
00942 
00943           tmp=d*xxyy;
00944           isect2[0]=tmp+e*xx*y1;
00945           isect2[1]=tmp+f*xx*y0;
00946 
00947           SORT(isect1[0],isect1[1]);
00948           SORT(isect2[0],isect2[1]);
00949 
00950           if(isect1[1]<isect2[0] || isect2[1]<isect1[0]) return false;
00951           return true;
00952         }
00953 
00954         /* sort so that a<=b */
00955 #define SORT2(a,b,smallest)       \
00956   if(a>b)         \
00957         {             \
00958         float MACRO_C;\
00959         MACRO_C=a;    \
00960         a=b;          \
00961         b=MACRO_C;    \
00962         smallest=1;   \
00963         }             \
00964   else smallest=0;
00965 
00966 
00967         void isect2(float VTX0[3],float VTX1[3],float VTX2[3],float VV0,float VV1,float VV2,
00968           float D0,float D1,float D2,float *isect0,float *isect1,float isectpoint0[3],float isectpoint1[3])
00969         {
00970           float tmp=D0/(D0-D1);
00971           float diff[3];
00972           *isect0=VV0+(VV1-VV0)*tmp;
00973           SUB(diff,VTX1,VTX0);
00974           MULT(diff,diff,tmp);
00975           ADD(isectpoint0,diff,VTX0);
00976           tmp=D0/(D0-D2);
00977           *isect1=VV0+(VV2-VV0)*tmp;
00978           SUB(diff,VTX2,VTX0);
00979           MULT(diff,diff,tmp);
00980           ADD(isectpoint1,VTX0,diff);
00981         }
00982 
00983 
00984 #if 0
00985 #define ISECT2(VTX0,VTX1,VTX2,VV0,VV1,VV2,D0,D1,D2,isect0,isect1,isectpoint0,isectpoint1) \
00986   tmp=D0/(D0-D1);                    \
00987   isect0=VV0+(VV1-VV0)*tmp;          \
00988   SUB(diff,VTX1,VTX0);               \
00989   MULT(diff,diff,tmp);               \
00990   ADD(isectpoint0,diff,VTX0);        \
00991   tmp=D0/(D0-D2);
00992         /*              isect1=VV0+(VV2-VV0)*tmp;          \ */
00993         /*              SUB(diff,VTX2,VTX0);               \     */
00994         /*              MULT(diff,diff,tmp);               \   */
00995         /*              ADD(isectpoint1,VTX0,diff);           */
00996 #endif
00997 
00998         bool compute_intervals_isectline(float VERT0[3],float VERT1[3],float VERT2[3],
00999           float VV0,float VV1,float VV2,float D0,float D1,float D2,
01000           float D0D1,float D0D2,float *isect0,float *isect1,
01001           float isectpoint0[3],float isectpoint1[3])
01002         {
01003           if(D0D1>0.0f)
01004           {
01005             /* here we know that D0D2<=0.0 */
01006             /* that is D0, D1 are on the same side, D2 on the other or on the plane */
01007             isect2(VERT2,VERT0,VERT1,VV2,VV0,VV1,D2,D0,D1,isect0,isect1,isectpoint0,isectpoint1);
01008           }
01009           else if(D0D2>0.0f)
01010           {
01011             /* here we know that d0d1<=0.0 */
01012             isect2(VERT1,VERT0,VERT2,VV1,VV0,VV2,D1,D0,D2,isect0,isect1,isectpoint0,isectpoint1);
01013           }
01014           // TODO: Comparing floats with !=
01015           else if(D1*D2>0.0f || D0!=0.0f)
01016           {
01017             /* here we know that d0d1<=0.0 or that D0!=0.0 */
01018             isect2(VERT0,VERT1,VERT2,VV0,VV1,VV2,D0,D1,D2,isect0,isect1,isectpoint0,isectpoint1);
01019           }
01020           // TODO: Comparing floats with !=
01021           else if(D1!=0.0f)
01022           {
01023             isect2(VERT1,VERT0,VERT2,VV1,VV0,VV2,D1,D0,D2,isect0,isect1,isectpoint0,isectpoint1);
01024           }
01025           // TODO: Comparing floats with !=
01026           else if(D2!=0.0f)
01027           {
01028             isect2(VERT2,VERT0,VERT1,VV2,VV0,VV1,D2,D0,D1,isect0,isect1,isectpoint0,isectpoint1);
01029           }
01030           else
01031           {
01032             /* triangles are coplanar */
01033             return true;
01034           }
01035           return false;
01036         }
01037 
01038 #define COMPUTE_INTERVALS_ISECTLINE(VERT0,VERT1,VERT2,VV0,VV1,VV2,D0,D1,D2,D0D1,D0D2,isect0,isect1,isectpoint0,isectpoint1) \
01039   if(D0D1>0.0f)                                         \
01040         {                                                     \
01041         /* here we know that D0D2<=0.0 */                   \
01042         /* that is D0, D1 are on the same side, D2 on the other or on the plane */ \
01043         isect2(VERT2,VERT0,VERT1,VV2,VV0,VV1,D2,D0,D1,&isect0,&isect1,isectpoint0,isectpoint1);          \
01044         }
01045 #if 0
01046   else if(D0D2>0.0f)                                    \
01047   {                                                     \
01048   /* here we know that d0d1<=0.0 */                   \
01049   isect2(VERT1,VERT0,VERT2,VV1,VV0,VV2,D1,D0,D2,&isect0,&isect1,isectpoint0,isectpoint1);          \
01050   }                                                     \
01051   else if(D1*D2>0.0f || D0!=0.0f)                       \
01052   {                                                     \
01053   /* here we know that d0d1<=0.0 or that D0!=0.0 */   \
01054   isect2(VERT0,VERT1,VERT2,VV0,VV1,VV2,D0,D1,D2,&isect0,&isect1,isectpoint0,isectpoint1);          \
01055   }                                                     \
01056   else if(D1!=0.0f)                                     \
01057   {                                                     \
01058   isect2(VERT1,VERT0,VERT2,VV1,VV0,VV2,D1,D0,D2,&isect0,&isect1,isectpoint0,isectpoint1);          \
01059   }                                                     \
01060   else if(D2!=0.0f)                                     \
01061   {                                                     \
01062   isect2(VERT2,VERT0,VERT1,VV2,VV0,VV1,D2,D0,D1,&isect0,&isect1,isectpoint0,isectpoint1);          \
01063   }                                                     \
01064   else                                                  \
01065   {                                                     \
01066   /* triangles are coplanar */                        \
01067   coplanar=1;                                         \
01068   return coplanar_tri_tri(N1,V0,V1,V2,U0,U1,U2);      \
01069   }
01070 #endif
01071 
01072 
01073 
01074 
01075 
01076 
01077 
01082   bool wrapTriTriIntersectLine(const Triangle & triA,const Triangle & triB,bool & coplanar,Vector3<double> & pt1,Vector3<double> & pt2)
01083   {
01084     float V0[3];
01085     V0[0] = static_cast<float>(triA.p0()[0]);
01086     V0[1] = static_cast<float>(triA.p0()[1]);
01087     V0[2] = static_cast<float>(triA.p0()[2]);
01088     float V1[3];
01089     V1[0] = static_cast<float>(triA.p1()[0]);
01090     V1[1] = static_cast<float>(triA.p1()[1]);
01091     V1[2] = static_cast<float>(triA.p1()[2]);
01092     float V2[3];
01093     V2[0] = static_cast<float>(triA.p2()[0]);
01094     V2[1] = static_cast<float>(triA.p2()[1]);
01095     V2[2] = static_cast<float>(triA.p2()[2]);
01096 
01097     float U0[3];
01098     U0[0] = static_cast<float>(triB.p0()[0]);
01099     U0[1] = static_cast<float>(triB.p0()[1]);
01100     U0[2] = static_cast<float>(triB.p0()[2]);
01101     float U1[3];
01102     U1[0] = static_cast<float>(triB.p1()[0]);
01103     U1[1] = static_cast<float>(triB.p1()[1]);
01104     U1[2] = static_cast<float>(triB.p1()[2]);
01105     float U2[3];
01106     U2[0] = static_cast<float>(triB.p2()[0]);
01107     U2[1] = static_cast<float>(triB.p2()[1]);
01108     U2[2] = static_cast<float>(triB.p2()[2]);
01109 
01110     int tmp = 0;
01111     float isectpt1[3];
01112     float isectpt2[3];
01113     bool result = tri_tri_intersect_with_isectline(V0,V1,V2,U0,U1,U2,&tmp,isectpt1,isectpt2);
01114 
01115     coplanar = false;
01116     if(tmp)
01117       coplanar = true;
01118     pt1 = Vector3<double>(isectpt1[0],isectpt1[1],isectpt1[2]);
01119     pt2 = Vector3<double>(isectpt2[0],isectpt2[1],isectpt2[2]);
01120     return result;
01121   }
01122 
01123   bool tri_tri_intersect_with_isectline(float V0[3],float V1[3],float V2[3],
01124     float U0[3],float U1[3],float U2[3],int *coplanar,
01125     float isectpt1[3],float isectpt2[3])
01126   {
01127     float E1[3],E2[3];
01128     float N1[3],N2[3],d1,d2;
01129     float du0,du1,du2,dv0,dv1,dv2;
01130     float D[3];
01131     float isect1[2], isect2[2];
01132     float isectpointA1[3],isectpointA2[3];
01133     float isectpointB1[3],isectpointB2[3];
01134     float du0du1,du0du2,dv0dv1,dv0dv2;
01135     short index;
01136     float vp0,vp1,vp2;
01137     float up0,up1,up2;
01138     float b,c,max;
01139     //float tmp,diff[3];   //--- KE 25-05-2004: Unreferenced local variable?
01140     int smallest1,smallest2;
01141 
01142     /* compute plane equation of triangle(V0,V1,V2) */
01143     SUB(E1,V1,V0);
01144     SUB(E2,V2,V0);
01145     CROSS(N1,E1,E2);
01146     d1=-DOT(N1,V0);
01147     /* plane equation 1: N1.X+d1=0 */
01148 
01149     /* put U0,U1,U2 into plane equation 1 to compute signed distances to the plane*/
01150     du0=DOT(N1,U0)+d1;
01151     du1=DOT(N1,U1)+d1;
01152     du2=DOT(N1,U2)+d1;
01153 
01154     /* coplanarity robustness check */
01155 #if USE_EPSILON_TEST==TRUE
01156     if(std::fabs(du0)<EPSILON) du0=0.0;
01157     if(std::fabs(du1)<EPSILON) du1=0.0;
01158     if(std::fabs(du2)<EPSILON) du2=0.0;
01159 #endif
01160     du0du1=du0*du1;
01161     du0du2=du0*du2;
01162 
01163     if(du0du1>0.0f && du0du2>0.0f) /* same sign on all of them + not equal 0 ? */
01164       return false;                    /* no intersection occurs */
01165 
01166     /* compute plane of triangle (U0,U1,U2) */
01167     SUB(E1,U1,U0);
01168     SUB(E2,U2,U0);
01169     CROSS(N2,E1,E2);
01170     d2=-DOT(N2,U0);
01171     /* plane equation 2: N2.X+d2=0 */
01172 
01173     /* put V0,V1,V2 into plane equation 2 */
01174     dv0=DOT(N2,V0)+d2;
01175     dv1=DOT(N2,V1)+d2;
01176     dv2=DOT(N2,V2)+d2;
01177 
01178 #if USE_EPSILON_TEST==TRUE
01179     if(std::fabs(dv0)<EPSILON) dv0=0.0;
01180     if(std::fabs(dv1)<EPSILON) dv1=0.0;
01181     if(std::fabs(dv2)<EPSILON) dv2=0.0;
01182 #endif
01183 
01184     dv0dv1=dv0*dv1;
01185     dv0dv2=dv0*dv2;
01186 
01187     if(dv0dv1>0.0f && dv0dv2>0.0f) /* same sign on all of them + not equal 0 ? */
01188       return false;                    /* no intersection occurs */
01189 
01190     /* compute direction of intersection line */
01191     CROSS(D,N1,N2);
01192 
01193     /* compute and index to the largest component of D */
01194     max=std::fabs(D[0]);
01195     index=0;
01196     b=std::fabs(D[1]);
01197     c=std::fabs(D[2]);
01198     if(b>max) max=b,index=1;
01199     if(c>max) max=c,index=2;
01200 
01201     /* this is the simplified projection onto L*/
01202     vp0=V0[index];
01203     vp1=V1[index];
01204     vp2=V2[index];
01205 
01206     up0=U0[index];
01207     up1=U1[index];
01208     up2=U2[index];
01209 
01210     /* compute interval for triangle 1 */
01211     *coplanar=compute_intervals_isectline(V0,V1,V2,vp0,vp1,vp2,dv0,dv1,dv2,
01212       dv0dv1,dv0dv2,&isect1[0],&isect1[1],isectpointA1,isectpointA2);
01213     if(*coplanar) return coplanar_tri_tri(N1,V0,V1,V2,U0,U1,U2);
01214 
01215 
01216     /* compute interval for triangle 2 */
01217     compute_intervals_isectline(U0,U1,U2,up0,up1,up2,du0,du1,du2,
01218       du0du1,du0du2,&isect2[0],&isect2[1],isectpointB1,isectpointB2);
01219 
01220     SORT2(isect1[0],isect1[1],smallest1);
01221     SORT2(isect2[0],isect2[1],smallest2);
01222 
01223     if(isect1[1]<isect2[0] || isect2[1]<isect1[0]) return false;
01224 
01225     /* at this point, we know that the triangles intersect */
01226 
01227     if(isect2[0]<isect1[0])
01228     {
01229       if(smallest1==0) { SET(isectpt1,isectpointA1); }
01230       else { SET(isectpt1,isectpointA2); }
01231 
01232       if(isect2[1]<isect1[1])
01233       {
01234         if(smallest2==0) { SET(isectpt2,isectpointB2); }
01235         else { SET(isectpt2,isectpointB1); }
01236       }
01237       else
01238       {
01239         if(smallest1==0) { SET(isectpt2,isectpointA2); }
01240         else { SET(isectpt2,isectpointA1); }
01241       }
01242     }
01243     else
01244     {
01245       if(smallest2==0) { SET(isectpt1,isectpointB1); }
01246       else { SET(isectpt1,isectpointB2); }
01247 
01248       if(isect2[1]>isect1[1])
01249       {
01250         if(smallest1==0) { SET(isectpt2,isectpointA2); }
01251         else { SET(isectpt2,isectpointA1); }
01252       }
01253       else
01254       {
01255         if(smallest2==0) { SET(isectpt2,isectpointB2); }
01256         else { SET(isectpt2,isectpointB1); }
01257       }
01258     }
01259     return true;
01260   }
01261 
01262 
01263 
01264       } // namespace old_stuff_soon_dies
01265 
01266 
01267 
01268 
01269     } //namespace Moller
01270 
01271   } // namespace intersect
01272 } // namespace OpenTissue
01273 
01274 // OPENTISSUE_COLLISION_INTERSECT_INTERSECT_TRIANGLE_TRIANGLE_INTERVAL_OVERLAP_H
01275 #endif
/home/hauberg/Dokumenter/Capture/humim-tracker-0.1/src/OpenTissue/OpenTissue/collision/intersect/intersect_triangle_triangle_interval_overlap.h
