[Toon-members] tag/tag intersection.h

[Gerhard Reitmayr]

CVSROOT:        /cvsroot/toon
Module name:    tag
Changes by:     Gerhard Reitmayr <gerhard>      06/07/21 10:45:04

Modified files:
        tag            : intersection.h 

Log message:
        fixed triangle-triangle intersection

CVSWeb URLs:
http://cvs.savannah.gnu.org/viewcvs/tag/tag/intersection.h?cvsroot=toon&r1=1.1&r2=1.2

Patches:
Index: intersection.h
===================================================================
RCS file: /cvsroot/toon/tag/tag/intersection.h,v
retrieving revision 1.1
retrieving revision 1.2
diff -u -b -r1.1 -r1.2
--- intersection.h      19 Jul 2006 08:53:56 -0000      1.1
+++ intersection.h      21 Jul 2006 10:45:04 -0000      1.2
@@ -17,7 +17,8 @@
 /// test for plane and line being parallel might depend on the magnitude
 /// of these vectors.
 /// @ingroup intersection
-inline bool intersect_plane_line( const TooN::Vector<3> & normal, const double 
d, const TooN::Vector<3> & p1, const TooN::Vector<3> & p2, TooN::Vector<3> & i){
+template<typename A, typename B, typename C, typename D>
+inline bool intersect_plane_line( const TooN::FixedVector<3,A> & normal, const 
double d, const TooN::FixedVector<3,B> & p1, const TooN::FixedVector<3,C> & p2, 
TooN::FixedVector<3,D> & i){
     const double EPSILON = 0.000001;
 
     TooN::Vector<3> dir = p2 - p1;
@@ -136,9 +137,11 @@
 
     // normal vector of plane of triangle v
     TooN::Vector<3> nv = (v2 - v1) ^ ( v3 - v1 );
-    double t1w = nv * w1;
-    double t2w = nv * w2;
-    double t3w = nv * w3;
+    TooN::normalize(nv);
+    double dv = v1 * nv;
+    double t1w = nv * w1 - dv;
+    double t2w = nv * w2 - dv;
+    double t3w = nv * w3 - dv;
     // all of triangle w on one side of plane of v ?
     if( (t1w < -EPSILON && t2w < -EPSILON && t3w < -EPSILON) ||
         (t1w >  EPSILON && t2w >  EPSILON && t3w >  EPSILON) ) {
@@ -147,17 +150,17 @@
 
     // normal vector of plane of triangle w
     TooN::Vector<3> nw = (w2 - w1) ^ ( w3 - w1 );
-    double t1v = nw * v1;
-    double t2v = nw * v2;
-    double t3v = nw * v3;
+    TooN::normalize(nw);
+    double dw = w1 * nw;
+    double t1v = nw * v1 - dw;
+    double t2v = nw * v2 - dw;
+    double t3v = nw * v3 - dw;
     // all of triangle v on one side of plane of w ?
     if( (t1v < -EPSILON && t2v < -EPSILON && t3v < -EPSILON) ||
         (t1v >  EPSILON && t2v >  EPSILON && t3v >  EPSILON) ) {
         return false;
     }
 
-    TooN::normalize(nv);
-    TooN::normalize(nw);
     // direction of line of intersection of planes
     TooN::Vector<3> d = nv ^ nw;
     // are the supporting planes almost parallel ? -> not dealing with this 
case
@@ -165,7 +168,7 @@
         return false;
     }
 
-    // find out which one is alone
+    // find out which corner is alone on one side of the
     int iv, iw;
     if( t1v * t2v > 0 )
         iv = 2;
@@ -182,51 +185,72 @@
 
     // compute interval points by intersecting with respective planes
     // we know that they intersect, therefore no test for failure
-    double dw = w1 * nw;
-    TooN::Vector<3> iv1, iv2;
-    intersect_plane_line( nw, dw, *tv[iv], *tv[(iv+1)%3], iv1 );
-    intersect_plane_line( nw, dw, *tv[iv], *tv[(iv+2)%3], iv2 );
-    double dv = v1 * nv;
-    TooN::Vector<3> iw1, iw2;
-    intersect_plane_line( nv, dv, *tw[iw], *tw[(iw+1)%3], iw1 );
-    intersect_plane_line( nv, dv, *tw[iw], *tw[(iw+2)%3], iw2 );
+    TooN::Matrix<4,3> intersections;
+    intersect_plane_line( nw, dw, *tv[iv], *tv[(iv+1)%3], intersections[0] );
+    intersect_plane_line( nw, dw, *tv[iv], *tv[(iv+2)%3], intersections[1] );
+    intersect_plane_line( nv, dv, *tw[iw], *tw[(iw+1)%3], intersections[2] );
+    intersect_plane_line( nv, dv, *tw[iw], *tw[(iw+2)%3], intersections[3] );
 
     // project onto line
-    double tiv1 = d * iv1,
-           tiv2 = d * iv2;
-    double tiw1 = d * iw1,
-           tiw2 = d * iw2;
+    TooN::Vector<4> proj = intersections * d;
 
     // calculate interval extensions
-    double tvmin = std::min(tiv1, tiv2),
-           tvmax = std::max(tiv1, tiv2);
-    double twmin = std::min(tiw1, tiw2),
-           twmax = std::max(tiw1, tiw2);
-    double intmin = std::max(tvmin, twmin),
-           intmax = std::min(tvmax, twmax);
-
-    // no intersection!
-    if( intmin > intmax )
-        return false;
-
-    // determine intersection points
-    if( intmin == tiv1 )
-        p1 = iv1;
-    else if( intmin == tiv2)
-        p1 = iv2;
-    else if( intmin == tiw1 )
-        p1 = iw1;
-    else if( intmin == tiw2 )
-        p1 = iw2;
-    if( intmax == tiv1 )
-        p2 = iv1;
-    else if( intmax == tiv2)
-        p2 = iv2;
-    else if( intmax == tiw1 )
-        p2 = iw1;
-    else if( intmax == tiw2 )
-        p2 = iw2;
+    int minIndex, maxIndex;
+    if( proj[0] < proj[1] ){         // min1  = proj[0], max1 = proj[1]
+        if( proj[2] < proj[3] ){     // min2  = proj[2], max2 = proj[3]
+            if( proj[0] > proj[2]){
+                minIndex = 0;
+            } else {
+                minIndex = 2;
+            }
+            if( proj[1] < proj[3] ){
+                maxIndex = 1;
+            } else {
+                maxIndex = 3;
+            }
+        } else {
+            if( proj[0] > proj[3]){  // min2 = proj[3], max2 = proj[2]
+                minIndex = 0;
+            } else {
+                minIndex = 3;
+            }
+            if( proj[1] < proj[2] ){
+                maxIndex = 1;
+            } else {
+                maxIndex = 2;
+            }
+        }
+    } else {                         // min1  = proj[1], max1 = proj[0]
+        if( proj[2] < proj[3] ){     // min2  = proj[2], max2 = proj[3]
+            if( proj[1] > proj[2]){
+                minIndex = 1;
+            } else {
+                minIndex = 2;
+            }
+            if( proj[0] < proj[3] ){
+                maxIndex = 0;
+            } else {
+                maxIndex = 3;
+            }
+        } else {
+            if( proj[1] > proj[3]){  // min2 = proj[3], max2 = proj[2]
+                minIndex = 1;
+            } else {
+                minIndex = 3;
+            }
+            if( proj[0] < proj[2] ){
+                maxIndex = 0;
+            } else {
+                maxIndex = 2;
+            }
+        }
+    }
+    // no intersection
+    if( proj[minIndex] > proj[maxIndex] )
+        return false;
 
+    p1 = intersections[minIndex];
+    p2 = intersections[maxIndex];
     return true;
 }