graspit: triangle_inl.h Source File

00001 //######################################################################
00002 //
00003 // GraspIt!
00004 // Copyright (C) 2002-2009  Columbia University in the City of New York.
00005 // All rights reserved.
00006 //
00007 // GraspIt! is free software: you can redistribute it and/or modify
00008 // it under the terms of the GNU General Public License as published by
00009 // the Free Software Foundation, either version 3 of the License, or
00010 // (at your option) any later version.
00011 //
00012 // GraspIt! is distributed in the hope that it will be useful,
00013 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00014 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015 // GNU General Public License for more details.
00016 //
00017 // You should have received a copy of the GNU General Public License
00018 // along with GraspIt!.  If not, see <http://www.gnu.org/licenses/>.
00019 //
00020 // Author(s): Matei T. Ciocarlie
00021 //
00022 // $Id: triangle_inl.h,v 1.2 2009/04/09 16:03:02 cmatei Exp $
00023 //
00024 //######################################################################
00025 
00032 inline double gmax(double d1, double d2, double d3)
00033 {
00034         double m = d1;
00035         if (d2 > m) m = d2;
00036         if (d3 > m) m = d3;
00037         return m;
00038 }
00039 
00040 inline double gmin(double d1, double d2, double d3)
00041 {
00042         double m = d1;
00043         if (d2 < m) m = d2;
00044         if (d3 < m) m = d3;
00045         return m;
00046 }
00047 
00048 inline int
00049 project6(const vec3 &ax, 
00050          const vec3 &p1, const vec3 &p2, const vec3 &p3, 
00051          const vec3 &q1, const vec3 &q2, const vec3 &q3)
00052 {
00053   double P1 = ax % p1;
00054   double P2 = ax % p2;
00055   double P3 = ax % p3;
00056   double Q1 = ax % q1;
00057   double Q2 = ax % q2;
00058   double Q3 = ax % q3;
00059   
00060   double mx1 = gmax(P1, P2, P3);
00061   double mn1 = gmin(P1, P2, P3);
00062   double mx2 = gmax(Q1, Q2, Q3);
00063   double mn2 = gmin(Q1, Q2, Q3);
00064 
00065   if (mn1 > mx2) return 0;
00066   if (mn2 > mx1) return 0;
00067   return 1;
00068 }
00069 
00070 inline
00071 bool triangleIntersection(const Triangle &t1, const Triangle &t2)
00072 {
00073   vec3 p1, p2, p3;
00074   vec3 q1, q2, q3;
00075   vec3 e1, e2, e3;
00076   vec3 f1, f2, f3;
00077   vec3 g1, g2, g3;
00078   vec3 h1, h2, h3;
00079   vec3 n1, m1;
00080 
00081   vec3 ef11, ef12, ef13;
00082   vec3 ef21, ef22, ef23;
00083   vec3 ef31, ef32, ef33;
00084   
00085   p1[0] = t1.v1[0] - t1.v1[0];  p1[1] = t1.v1[1] - t1.v1[1];  p1[2] = t1.v1[2] - t1.v1[2];
00086   p2[0] = t1.v2[0] - t1.v1[0];  p2[1] = t1.v2[1] - t1.v1[1];  p2[2] = t1.v2[2] - t1.v1[2];
00087   p3[0] = t1.v3[0] - t1.v1[0];  p3[1] = t1.v3[1] - t1.v1[1];  p3[2] = t1.v3[2] - t1.v1[2];
00088   
00089   q1[0] = t2.v1[0] - t1.v1[0];  q1[1] = t2.v1[1] - t1.v1[1];  q1[2] = t2.v1[2] - t1.v1[2];
00090   q2[0] = t2.v2[0] - t1.v1[0];  q2[1] = t2.v2[1] - t1.v1[1];  q2[2] = t2.v2[2] - t1.v1[2];
00091   q3[0] = t2.v3[0] - t1.v1[0];  q3[1] = t2.v3[1] - t1.v1[1];  q3[2] = t2.v3[2] - t1.v1[2];
00092   
00093   e1[0] = p2[0] - p1[0];  e1[1] = p2[1] - p1[1];  e1[2] = p2[2] - p1[2];
00094   e2[0] = p3[0] - p2[0];  e2[1] = p3[1] - p2[1];  e2[2] = p3[2] - p2[2];
00095   e3[0] = p1[0] - p3[0];  e3[1] = p1[1] - p3[1];  e3[2] = p1[2] - p3[2];
00096 
00097   f1[0] = q2[0] - q1[0];  f1[1] = q2[1] - q1[1];  f1[2] = q2[2] - q1[2];
00098   f2[0] = q3[0] - q2[0];  f2[1] = q3[1] - q2[1];  f2[2] = q3[2] - q2[2];
00099   f3[0] = q1[0] - q3[0];  f3[1] = q1[1] - q3[1];  f3[2] = q1[2] - q3[2];
00100   
00101   n1= e1 * e2;
00102   m1= f1 * f2;
00103 
00104   g1= e1 * n1;
00105   g2= e2 * n1;
00106   g3= e3 * n1;
00107   h1= f1 * m1;
00108   h2= f2 * m1;
00109   h3= f3 * m1;
00110 
00111   ef11= e1 * f1;
00112   ef12= e1 * f2;
00113   ef13= e1 * f3;
00114   ef21= e2 * f1;
00115   ef22= e2 * f2;
00116   ef23= e2 * f3;
00117   ef31= e3 * f1;
00118   ef32= e3 * f2;
00119   ef33= e3 * f3;
00120   
00121   // now begin the series of tests
00122 
00123   if (!project6(n1, p1, p2, p3, q1, q2, q3)) return 0;
00124   if (!project6(m1, p1, p2, p3, q1, q2, q3)) return 0;
00125   
00126   if (!project6(ef11, p1, p2, p3, q1, q2, q3)) return 0;
00127   if (!project6(ef12, p1, p2, p3, q1, q2, q3)) return 0;
00128   if (!project6(ef13, p1, p2, p3, q1, q2, q3)) return 0;
00129   if (!project6(ef21, p1, p2, p3, q1, q2, q3)) return 0;
00130   if (!project6(ef22, p1, p2, p3, q1, q2, q3)) return 0;
00131   if (!project6(ef23, p1, p2, p3, q1, q2, q3)) return 0;
00132   if (!project6(ef31, p1, p2, p3, q1, q2, q3)) return 0;
00133   if (!project6(ef32, p1, p2, p3, q1, q2, q3)) return 0;
00134   if (!project6(ef33, p1, p2, p3, q1, q2, q3)) return 0;
00135 
00136   if (!project6(g1, p1, p2, p3, q1, q2, q3)) return 0;
00137   if (!project6(g2, p1, p2, p3, q1, q2, q3)) return 0;
00138   if (!project6(g3, p1, p2, p3, q1, q2, q3)) return 0;
00139   if (!project6(h1, p1, p2, p3, q1, q2, q3)) return 0;
00140   if (!project6(h2, p1, p2, p3, q1, q2, q3)) return 0;
00141   if (!project6(h3, p1, p2, p3, q1, q2, q3)) return 0;
00142 
00143   return 1;
00144 }
00145 
00149 position closestPtTriangle(const Triangle &t, const position &p)
00150 {
00151     // Check if P in vertex region outside t.v1
00152     vec3 ab = t.v2 - t.v1;
00153     vec3 ac = t.v3 - t.v1;
00154     vec3 ap = p - t.v1;
00155     double d1 = ab % ap;
00156     double d2 = ac % ap;
00157     if (d1 <= 0.0f && d2 <= 0.0f) return t.v1; // barycentric coordinates (1,0,0)
00158 
00159     // Check if P in vertex region outside t.v2
00160     vec3 bp = p - t.v2;
00161     double d3 = ab % bp;
00162     double d4 = ac % bp;
00163     if (d3 >= 0.0f && d4 <= d3) return t.v2; // barycentric coordinates (0,1,0)
00164 
00165     // Check if P in edge region of AB, if so return projection of P onto AB
00166     double vc = d1*d4 - d3*d2;
00167     if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f) {
00168         double v = d1 / (d1 - d3);
00169         return t.v1 + v * ab; // barycentric coordinates (1-v,v,0)
00170     }
00171 
00172     // Check if P in vertex region outside t.v3
00173     vec3 cp = p - t.v3;
00174     double d5 = ab % cp;
00175     double d6 = ac % cp;
00176     if (d6 >= 0.0f && d5 <= d6) return t.v3; // barycentric coordinates (0,0,1)
00177 
00178     // Check if P in edge region of AC, if so return projection of P onto AC
00179     double vb = d5*d2 - d1*d6;
00180     if (vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f) {
00181         double w = d2 / (d2 - d6);
00182         return t.v1 + w * ac; // barycentric coordinates (1-w,0,w)
00183     }
00184 
00185     // Check if P in edge region of BC, if so return projection of P onto BC
00186     double va = d3*d6 - d5*d4;
00187     if (va <= 0.0f && (d4 - d3) >= 0.0f && (d5 - d6) >= 0.0f) {
00188         double w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
00189         return t.v2 + w * (t.v3 - t.v2); // barycentric coordinates (0,1-w,w)
00190     }
00191 
00192     // P inside face region. Compute Q through its barycentric coordinates (u,v,w)
00193     double denom = 1.0f / (va + vb + vc);
00194     double v = vb * denom;
00195     double w = vc * denom;
00196     return t.v1 + ab * v + ac * w; // = u*t.v1 + v*t.v2 + w*t.v3, u = va * denom = 1.0f - v - w
00197 }
00198 
00199 // Clamp n to lie within the range [min, max]
00200 inline double Clamp(double n, double min, double max) {
00201     if (n < min) return min;
00202     if (n > max) return max;
00203     return n;
00204 }
00205 
00209 inline double segmSegmDistanceSq(const position &p1, const position &q1, 
00210                                                                  const position &p2, const position &q2,
00211                                                                  position &c1, position &c2)
00212 {
00213         double EPSILON = 1.0e-3;
00214     vec3 d1 = q1 - p1; // Direction vector of segment S1
00215     vec3 d2 = q2 - p2; // Direction vector of segment S2
00216     vec3 r = p1 - p2;
00217     double a = d1 % d1; // Squared length of segment S1, always nonnegative
00218     double e = d2 % d2; // Squared length of segment S2, always nonnegative
00219     double f = d2 % r;
00220         double s, t;
00221 
00222     // Check if either or both segments degenerate into points
00223     if (a <= EPSILON && e <= EPSILON) {
00224         // Both segments degenerate into points
00225         s = t = 0.0f;
00226         c1 = p1;
00227         c2 = p2;
00228         return (c1 - c2) % (c1 - c2);
00229     }
00230     if (a <= EPSILON) {
00231         // First segment degenerates into a point
00232         s = 0.0f;
00233         t = f / e; // s = 0 => t = (b*s + f) / e = f / e
00234         t = Clamp(t, 0.0f, 1.0f);
00235     } else {
00236         double c = d1 % r;
00237         if (e <= EPSILON) {
00238             // Second segment degenerates into a point
00239             t = 0.0f;
00240             s = Clamp(-c / a, 0.0f, 1.0f); // t = 0 => s = (b*t - c) / a = -c / a
00241         } else {
00242             // The general nondegenerate case starts here
00243             double b = d1 % d2;
00244             double denom = a*e-b*b; // Always nonnegative
00245 
00246             // If segments not parallel, compute closest point on L1 to L2, and
00247             // clamp to segment S1. Else pick arbitrary s (here 0)
00248             if (denom != 0.0f) {
00249                 s = Clamp((b*f - c*e) / denom, 0.0f, 1.0f);
00250             } else s = 0.0f;
00251 
00252             // Compute point on L2 closest to S1(s) using
00253             // t = Dot((P1+D1*s)-P2,D2) / Dot(D2,D2) = (b*s + f) / e
00254             t = (b*s + f) / e;
00255 
00256             // If t in [0,1] done. Else clamp t, recompute s for the new value
00257             // of t using s = Dot((P2+D2*t)-P1,D1) / Dot(D1,D1)= (t*b - c) / a
00258             // and clamp s to [0, 1]
00259             if (t < 0.0f) {
00260                 t = 0.0f;
00261                 s = Clamp(-c / a, 0.0f, 1.0f);
00262             } else if (t > 1.0f) {
00263                 t = 1.0f;
00264                 s = Clamp((b - c) / a, 0.0f, 1.0f);
00265             }
00266         }
00267     }
00268 
00269     c1 = p1 + d1 * s;
00270     c2 = p2 + d2 * t;
00271     return (c1 - c2) % (c1 - c2);
00272 }
00273 
00284 int
00285 triangleTriangleContact(const Triangle &t1, const Triangle &t2, double threshSq, 
00286                                                 std::vector< std::pair<position, position> >* contactPoints)
00287 {
00288         if (triangleIntersection(t1,t2)) return -1.0;
00289         position p1, p2;
00290         //vertices on triangle 2 and face on triangle 1
00291         //--
00292         p1 = closestPtTriangle(t1, t2.v1);
00293         p2 = t2.v1;
00294         if (((p2 - p1) % (p2 - p1)) < threshSq) {
00295                 contactPoints->push_back( std::pair<position, position>(p1,p2));
00296         }
00297         //--
00298         p1 = closestPtTriangle(t1, t2.v2);
00299         p2 = t2.v2;
00300         if (((p2 - p1) % (p2 - p1)) < threshSq) {
00301                 contactPoints->push_back( std::pair<position, position>(p1,p2));
00302         }
00303         //--
00304         p1 = closestPtTriangle(t1, t2.v3);
00305         p2 = t2.v3;
00306         if (((p2 - p1) % (p2 - p1)) < threshSq) {
00307                 contactPoints->push_back( std::pair<position, position>(p1,p2));
00308         }
00309         //vertices on triangle 1 and face on triangle 2
00310         //if closest point on triangle 2 is a vertex, skip this pair as we have already done
00311         //the vertices from triangle 2
00312         p2 = closestPtTriangle(t2, t1.v1);
00313         p1 = t1.v1;
00314         if (((p2 - p1) % (p2 - p1)) < threshSq) {
00315                 if ( !(p2==t2.v1) && !(p2==t2.v2) && !(p2==t2.v3) ) {
00316                         contactPoints->push_back( std::pair<position, position>(p1,p2));
00317                 }
00318         }
00319         //--
00320         p2 = closestPtTriangle(t2, t1.v2);
00321         p1 = t1.v2;
00322         if (((p2 - p1) % (p2 - p1)) < threshSq) {
00323                 if ( !(p2==t2.v1) && !(p2==t2.v2) && !(p2==t2.v3) ) {
00324                         contactPoints->push_back( std::pair<position, position>(p1,p2));
00325                 }
00326         }
00327         //--
00328         p2 = closestPtTriangle(t2, t1.v3);
00329         p1 = t1.v3;
00330         if (((p2 - p1) % (p2 - p1)) < threshSq) {
00331                 if ( !(p2==t2.v1) && !(p2==t2.v2) && !(p2==t2.v3) ) {
00332                         contactPoints->push_back( std::pair<position, position>(p1,p2));
00333                 }
00334         }
00335         //------------------------------------------------------------------
00336 
00337         //nine edge-edge tests
00338         //if one of the closest points on an edge is actually a vertex, skip this pair
00339         //as we have already done the vertex closest points
00340         double d;
00341         d = segmSegmDistanceSq(t1.v1, t1.v2, t2.v1, t2.v2, p1, p2);
00342         if (d < threshSq) {
00343                 if ( !(p1==t1.v1) && !(p1==t1.v2) && !(p2==t2.v1) && !(p2==t2.v2) ) {
00344                         contactPoints->push_back( std::pair<position, position>(p1,p2));
00345                 }
00346         }
00347         //--
00348         d = segmSegmDistanceSq(t1.v1, t1.v2, t2.v2, t2.v3, p1, p2);
00349         if (d < threshSq) {
00350                 if ( !(p1==t1.v1) && !(p1==t1.v2) && !(p2==t2.v2) && !(p2==t2.v3) ) {
00351                         contactPoints->push_back( std::pair<position, position>(p1,p2));
00352                 }
00353         }
00354         //--
00355         d = segmSegmDistanceSq(t1.v1, t1.v2, t2.v3, t2.v1, p1, p2);
00356         if (d < threshSq) {
00357                 if ( !(p1==t1.v1) && !(p1==t1.v2) && !(p2==t2.v3) && !(p2==t2.v1) ) {
00358                         contactPoints->push_back( std::pair<position, position>(p1,p2));
00359                 }
00360         }
00361         //------------------
00362         d = segmSegmDistanceSq(t1.v2, t1.v3, t2.v1, t2.v2, p1, p2);
00363         if (d < threshSq) {
00364                 if ( !(p1==t1.v2) && !(p1==t1.v3) && !(p2==t2.v1) && !(p2==t2.v2) ) {
00365                         contactPoints->push_back( std::pair<position, position>(p1,p2));
00366                 }
00367         }
00368         //--
00369         d = segmSegmDistanceSq(t1.v2, t1.v3, t2.v2, t2.v3, p1, p2);
00370         if (d < threshSq) {
00371                 if ( !(p1==t1.v2) && !(p1==t1.v3) && !(p2==t2.v2) && !(p2==t2.v3) ) {
00372                         contactPoints->push_back( std::pair<position, position>(p1,p2));
00373                 }
00374         }
00375         //--
00376         d = segmSegmDistanceSq(t1.v2, t1.v3, t2.v3, t2.v1, p1, p2);
00377         if (d < threshSq) {
00378                 if ( !(p1==t1.v2) && !(p1==t1.v3) && !(p2==t2.v3) && !(p2==t2.v1) ) {
00379                         contactPoints->push_back( std::pair<position, position>(p1,p2));
00380                 }
00381         }
00382         //------------------
00383         d = segmSegmDistanceSq(t1.v3, t1.v1, t2.v1, t2.v2, p1, p2);
00384         if (d < threshSq) {
00385                 if ( !(p1==t1.v3) && !(p1==t1.v1) && !(p2==t2.v1) && !(p2==t2.v2) ) {
00386                         contactPoints->push_back( std::pair<position, position>(p1,p2));
00387                 }
00388         }
00389         //--
00390         d = segmSegmDistanceSq(t1.v3, t1.v1, t2.v2, t2.v3, p1, p2);
00391         if (d < threshSq) {
00392                 if ( !(p1==t1.v3) && !(p1==t1.v1) && !(p2==t2.v2) && !(p2==t2.v3) ) {
00393                         contactPoints->push_back( std::pair<position, position>(p1,p2));
00394                 }
00395         }
00396         //--
00397         d = segmSegmDistanceSq(t1.v3, t1.v1, t2.v3, t2.v1, p1, p2);
00398         if (d < threshSq) {
00399                 if ( !(p1==t1.v3) && !(p1==t1.v1) && !(p2==t2.v3) && !(p2==t2.v1) ) {
00400                         contactPoints->push_back( std::pair<position, position>(p1,p2));
00401                 }
00402         }
00403         return contactPoints->size();
00404 }
00405 
00406 double triangleTriangleDistanceSq(const Triangle &t1, const Triangle &t2,
00407                                                                   position &p1, position &p2)
00408 {
00409         if (triangleIntersection(t1,t2)) return -1.0;
00410 
00411         double dtmp, dmin;
00412         position tmp1, tmp2;
00413         //six vertex - face tests
00414         //--
00415         p1 = closestPtTriangle(t1, t2.v1);
00416         p2 = t2.v1;
00417         dmin = (p2 - p1) % (p2 - p1);
00418         //--
00419         tmp1 = closestPtTriangle(t1, t2.v2);
00420         tmp2 = t2.v2;
00421         dtmp = (tmp2 - tmp1) % (tmp2 - tmp1);
00422         if (dtmp < dmin) {
00423                 dmin = dtmp;
00424                 p1 = tmp1; p2 = tmp2;
00425         }
00426         //--
00427         tmp1 = closestPtTriangle(t1, t2.v3);
00428         tmp2 = t2.v3;
00429         dtmp = (tmp2 - tmp1) % (tmp2 - tmp1);
00430         if (dtmp < dmin) {
00431                 dmin = dtmp;
00432                 p1 = tmp1; p2 = tmp2;
00433         }
00434         //--
00435         tmp2 = closestPtTriangle(t2, t1.v1);
00436         tmp1 = t1.v1;
00437         dtmp = (tmp2 - tmp1) % (tmp2 - tmp1);
00438         if (dtmp < dmin) {
00439                 dmin = dtmp;
00440                 p1 = tmp1; p2 = tmp2;
00441         }
00442         //--
00443         tmp2 = closestPtTriangle(t2, t1.v2);
00444         tmp1 = t1.v2;
00445         dtmp = (tmp2 - tmp1) % (tmp2 - tmp1);
00446         if (dtmp < dmin) {
00447                 dmin = dtmp;
00448                 p1 = tmp1; p2 = tmp2;
00449         }
00450         //--
00451         tmp2 = closestPtTriangle(t2, t1.v3);
00452         tmp1 = t1.v3;
00453         dtmp = (tmp2 - tmp1) % (tmp2 - tmp1);
00454         if (dtmp < dmin) {
00455                 dmin = dtmp;
00456                 p1 = tmp1; p2 = tmp2;
00457         }
00458         //------------------------------------------------------------------
00459 
00460         //nine edge-edge tests
00461         dtmp = segmSegmDistanceSq(t1.v1, t1.v2, t2.v1, t2.v2, tmp1, tmp2);
00462         if (dtmp < dmin) {
00463                 dmin = dtmp;
00464                 p1 = tmp1; p2 = tmp2;
00465         }
00466         //--
00467         dtmp = segmSegmDistanceSq(t1.v1, t1.v2, t2.v2, t2.v3, tmp1, tmp2);
00468         if (dtmp < dmin) {
00469                 dmin = dtmp;
00470                 p1 = tmp1; p2 = tmp2;
00471         }
00472         //--
00473         dtmp = segmSegmDistanceSq(t1.v1, t1.v2, t2.v3, t2.v1, tmp1, tmp2);
00474         if (dtmp < dmin) {
00475                 dmin = dtmp;
00476                 p1 = tmp1; p2 = tmp2;
00477         }
00478         //-------------------------------
00479         dtmp = segmSegmDistanceSq(t1.v2, t1.v3, t2.v1, t2.v2, tmp1, tmp2);
00480         if (dtmp < dmin) {
00481                 dmin = dtmp;
00482                 p1 = tmp1; p2 = tmp2;
00483         }
00484         //--
00485         dtmp = segmSegmDistanceSq(t1.v2, t1.v3, t2.v2, t2.v3, tmp1, tmp2);
00486         if (dtmp < dmin) {
00487                 dmin = dtmp;
00488                 p1 = tmp1; p2 = tmp2;
00489         }
00490         //--
00491         dtmp = segmSegmDistanceSq(t1.v2, t1.v3, t2.v3, t2.v1, tmp1, tmp2);
00492         if (dtmp < dmin) {
00493                 dmin = dtmp;
00494                 p1 = tmp1; p2 = tmp2;
00495         }
00496         //-----------------------------------
00497         dtmp = segmSegmDistanceSq(t1.v3, t1.v1, t2.v1, t2.v2, tmp1, tmp2);
00498         if (dtmp < dmin) {
00499                 dmin = dtmp;
00500                 p1 = tmp1; p2 = tmp2;
00501         }
00502         //--
00503         dtmp = segmSegmDistanceSq(t1.v3, t1.v1, t2.v2, t2.v3, tmp1, tmp2);
00504         if (dtmp < dmin) {
00505                 dmin = dtmp;
00506                 p1 = tmp1; p2 = tmp2;
00507         }
00508         //--
00509         dtmp = segmSegmDistanceSq(t1.v3, t1.v1, t2.v3, t2.v1, tmp1, tmp2);
00510         if (dtmp < dmin) {
00511                 dmin = dtmp;
00512                 p1 = tmp1; p2 = tmp2;
00513         }
00514         //--------
00515 
00516 
00517         return dmin;
00518 }