graspit: /opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit_source/include/triangle

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 bool triangleIntersection(const Triangle &t1, const Triangle &t2)
00071 {
00072   vec3 p1, p2, p3;
00073   vec3 q1, q2, q3;
00074   vec3 e1, e2, e3;
00075   vec3 f1, f2, f3;
00076   vec3 g1, g2, g3;
00077   vec3 h1, h2, h3;
00078   vec3 n1, m1;
00079 
00080   vec3 ef11, ef12, ef13;
00081   vec3 ef21, ef22, ef23;
00082   vec3 ef31, ef32, ef33;
00083   
00084   p1[0] = t1.v1[0] - t1.v1[0];  p1[1] = t1.v1[1] - t1.v1[1];  p1[2] = t1.v1[2] - t1.v1[2];
00085   p2[0] = t1.v2[0] - t1.v1[0];  p2[1] = t1.v2[1] - t1.v1[1];  p2[2] = t1.v2[2] - t1.v1[2];
00086   p3[0] = t1.v3[0] - t1.v1[0];  p3[1] = t1.v3[1] - t1.v1[1];  p3[2] = t1.v3[2] - t1.v1[2];
00087   
00088   q1[0] = t2.v1[0] - t1.v1[0];  q1[1] = t2.v1[1] - t1.v1[1];  q1[2] = t2.v1[2] - t1.v1[2];
00089   q2[0] = t2.v2[0] - t1.v1[0];  q2[1] = t2.v2[1] - t1.v1[1];  q2[2] = t2.v2[2] - t1.v1[2];
00090   q3[0] = t2.v3[0] - t1.v1[0];  q3[1] = t2.v3[1] - t1.v1[1];  q3[2] = t2.v3[2] - t1.v1[2];
00091   
00092   e1[0] = p2[0] - p1[0];  e1[1] = p2[1] - p1[1];  e1[2] = p2[2] - p1[2];
00093   e2[0] = p3[0] - p2[0];  e2[1] = p3[1] - p2[1];  e2[2] = p3[2] - p2[2];
00094   e3[0] = p1[0] - p3[0];  e3[1] = p1[1] - p3[1];  e3[2] = p1[2] - p3[2];
00095 
00096   f1[0] = q2[0] - q1[0];  f1[1] = q2[1] - q1[1];  f1[2] = q2[2] - q1[2];
00097   f2[0] = q3[0] - q2[0];  f2[1] = q3[1] - q2[1];  f2[2] = q3[2] - q2[2];
00098   f3[0] = q1[0] - q3[0];  f3[1] = q1[1] - q3[1];  f3[2] = q1[2] - q3[2];
00099   
00100   n1= e1 * e2;
00101   m1= f1 * f2;
00102 
00103   g1= e1 * n1;
00104   g2= e2 * n1;
00105   g3= e3 * n1;
00106   h1= f1 * m1;
00107   h2= f2 * m1;
00108   h3= f3 * m1;
00109 
00110   ef11= e1 * f1;
00111   ef12= e1 * f2;
00112   ef13= e1 * f3;
00113   ef21= e2 * f1;
00114   ef22= e2 * f2;
00115   ef23= e2 * f3;
00116   ef31= e3 * f1;
00117   ef32= e3 * f2;
00118   ef33= e3 * f3;
00119   
00120   // now begin the series of tests
00121 
00122   if (!project6(n1, p1, p2, p3, q1, q2, q3)) return 0;
00123   if (!project6(m1, p1, p2, p3, q1, q2, q3)) return 0;
00124   
00125   if (!project6(ef11, p1, p2, p3, q1, q2, q3)) return 0;
00126   if (!project6(ef12, p1, p2, p3, q1, q2, q3)) return 0;
00127   if (!project6(ef13, p1, p2, p3, q1, q2, q3)) return 0;
00128   if (!project6(ef21, p1, p2, p3, q1, q2, q3)) return 0;
00129   if (!project6(ef22, p1, p2, p3, q1, q2, q3)) return 0;
00130   if (!project6(ef23, p1, p2, p3, q1, q2, q3)) return 0;
00131   if (!project6(ef31, p1, p2, p3, q1, q2, q3)) return 0;
00132   if (!project6(ef32, p1, p2, p3, q1, q2, q3)) return 0;
00133   if (!project6(ef33, p1, p2, p3, q1, q2, q3)) return 0;
00134 
00135   if (!project6(g1, p1, p2, p3, q1, q2, q3)) return 0;
00136   if (!project6(g2, p1, p2, p3, q1, q2, q3)) return 0;
00137   if (!project6(g3, p1, p2, p3, q1, q2, q3)) return 0;
00138   if (!project6(h1, p1, p2, p3, q1, q2, q3)) return 0;
00139   if (!project6(h2, p1, p2, p3, q1, q2, q3)) return 0;
00140   if (!project6(h3, p1, p2, p3, q1, q2, q3)) return 0;
00141 
00142   return 1;
00143 }
00144 
00148 position closestPtTriangle(const Triangle &t, const position &p)
00149 {
00150     // Check if P in vertex region outside t.v1
00151     vec3 ab = t.v2 - t.v1;
00152     vec3 ac = t.v3 - t.v1;
00153     vec3 ap = p - t.v1;
00154     double d1 = ab % ap;
00155     double d2 = ac % ap;
00156     if (d1 <= 0.0f && d2 <= 0.0f) return t.v1; // barycentric coordinates (1,0,0)
00157 
00158     // Check if P in vertex region outside t.v2
00159     vec3 bp = p - t.v2;
00160     double d3 = ab % bp;
00161     double d4 = ac % bp;
00162     if (d3 >= 0.0f && d4 <= d3) return t.v2; // barycentric coordinates (0,1,0)
00163 
00164     // Check if P in edge region of AB, if so return projection of P onto AB
00165     double vc = d1*d4 - d3*d2;
00166     if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f) {
00167         double v = d1 / (d1 - d3);
00168         return t.v1 + v * ab; // barycentric coordinates (1-v,v,0)
00169     }
00170 
00171     // Check if P in vertex region outside t.v3
00172     vec3 cp = p - t.v3;
00173     double d5 = ab % cp;
00174     double d6 = ac % cp;
00175     if (d6 >= 0.0f && d5 <= d6) return t.v3; // barycentric coordinates (0,0,1)
00176 
00177     // Check if P in edge region of AC, if so return projection of P onto AC
00178     double vb = d5*d2 - d1*d6;
00179     if (vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f) {
00180         double w = d2 / (d2 - d6);
00181         return t.v1 + w * ac; // barycentric coordinates (1-w,0,w)
00182     }
00183 
00184     // Check if P in edge region of BC, if so return projection of P onto BC
00185     double va = d3*d6 - d5*d4;
00186     if (va <= 0.0f && (d4 - d3) >= 0.0f && (d5 - d6) >= 0.0f) {
00187         double w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
00188         return t.v2 + w * (t.v3 - t.v2); // barycentric coordinates (0,1-w,w)
00189     }
00190 
00191     // P inside face region. Compute Q through its barycentric coordinates (u,v,w)
00192     double denom = 1.0f / (va + vb + vc);
00193     double v = vb * denom;
00194     double w = vc * denom;
00195     return t.v1 + ab * v + ac * w; // = u*t.v1 + v*t.v2 + w*t.v3, u = va * denom = 1.0f - v - w
00196 }
00197 
00198 // Clamp n to lie within the range [min, max]
00199 inline double Clamp(double n, double min, double max) {
00200     if (n < min) return min;
00201     if (n > max) return max;
00202     return n;
00203 }
00204 
00208 inline double segmSegmDistanceSq(const position &p1, const position &q1, 
00209                                                                  const position &p2, const position &q2,
00210                                                                  position &c1, position &c2)
00211 {
00212         double EPSILON = 1.0e-3;
00213     vec3 d1 = q1 - p1; // Direction vector of segment S1
00214     vec3 d2 = q2 - p2; // Direction vector of segment S2
00215     vec3 r = p1 - p2;
00216     double a = d1 % d1; // Squared length of segment S1, always nonnegative
00217     double e = d2 % d2; // Squared length of segment S2, always nonnegative
00218     double f = d2 % r;
00219         double s, t;
00220 
00221     // Check if either or both segments degenerate into points
00222     if (a <= EPSILON && e <= EPSILON) {
00223         // Both segments degenerate into points
00224         s = t = 0.0f;
00225         c1 = p1;
00226         c2 = p2;
00227         return (c1 - c2) % (c1 - c2);
00228     }
00229     if (a <= EPSILON) {
00230         // First segment degenerates into a point
00231         s = 0.0f;
00232         t = f / e; // s = 0 => t = (b*s + f) / e = f / e
00233         t = Clamp(t, 0.0f, 1.0f);
00234     } else {
00235         double c = d1 % r;
00236         if (e <= EPSILON) {
00237             // Second segment degenerates into a point
00238             t = 0.0f;
00239             s = Clamp(-c / a, 0.0f, 1.0f); // t = 0 => s = (b*t - c) / a = -c / a
00240         } else {
00241             // The general nondegenerate case starts here
00242             double b = d1 % d2;
00243             double denom = a*e-b*b; // Always nonnegative
00244 
00245             // If segments not parallel, compute closest point on L1 to L2, and
00246             // clamp to segment S1. Else pick arbitrary s (here 0)
00247             if (denom != 0.0f) {
00248                 s = Clamp((b*f - c*e) / denom, 0.0f, 1.0f);
00249             } else s = 0.0f;
00250 
00251             // Compute point on L2 closest to S1(s) using
00252             // t = Dot((P1+D1*s)-P2,D2) / Dot(D2,D2) = (b*s + f) / e
00253             t = (b*s + f) / e;
00254 
00255             // If t in [0,1] done. Else clamp t, recompute s for the new value
00256             // of t using s = Dot((P2+D2*t)-P1,D1) / Dot(D1,D1)= (t*b - c) / a
00257             // and clamp s to [0, 1]
00258             if (t < 0.0f) {
00259                 t = 0.0f;
00260                 s = Clamp(-c / a, 0.0f, 1.0f);
00261             } else if (t > 1.0f) {
00262                 t = 1.0f;
00263                 s = Clamp((b - c) / a, 0.0f, 1.0f);
00264             }
00265         }
00266     }
00267 
00268     c1 = p1 + d1 * s;
00269     c2 = p2 + d2 * t;
00270     return (c1 - c2) % (c1 - c2);
00271 }
00272 
00283 int
00284 triangleTriangleContact(const Triangle &t1, const Triangle &t2, double threshSq, 
00285                                                 std::vector< std::pair<position, position> >* contactPoints)
00286 {
00287         if (triangleIntersection(t1,t2)) return -1.0;
00288         position p1, p2;
00289         //vertices on triangle 2 and face on triangle 1
00290         //--
00291         p1 = closestPtTriangle(t1, t2.v1);
00292         p2 = t2.v1;
00293         if (((p2 - p1) % (p2 - p1)) < threshSq) {
00294                 contactPoints->push_back( std::pair<position, position>(p1,p2));
00295         }
00296         //--
00297         p1 = closestPtTriangle(t1, t2.v2);
00298         p2 = t2.v2;
00299         if (((p2 - p1) % (p2 - p1)) < threshSq) {
00300                 contactPoints->push_back( std::pair<position, position>(p1,p2));
00301         }
00302         //--
00303         p1 = closestPtTriangle(t1, t2.v3);
00304         p2 = t2.v3;
00305         if (((p2 - p1) % (p2 - p1)) < threshSq) {
00306                 contactPoints->push_back( std::pair<position, position>(p1,p2));
00307         }
00308         //vertices on triangle 1 and face on triangle 2
00309         //if closest point on triangle 2 is a vertex, skip this pair as we have already done
00310         //the vertices from triangle 2
00311         p2 = closestPtTriangle(t2, t1.v1);
00312         p1 = t1.v1;
00313         if (((p2 - p1) % (p2 - p1)) < threshSq) {
00314                 if ( !(p2==t2.v1) && !(p2==t2.v2) && !(p2==t2.v3) ) {
00315                         contactPoints->push_back( std::pair<position, position>(p1,p2));
00316                 }
00317         }
00318         //--
00319         p2 = closestPtTriangle(t2, t1.v2);
00320         p1 = t1.v2;
00321         if (((p2 - p1) % (p2 - p1)) < threshSq) {
00322                 if ( !(p2==t2.v1) && !(p2==t2.v2) && !(p2==t2.v3) ) {
00323                         contactPoints->push_back( std::pair<position, position>(p1,p2));
00324                 }
00325         }
00326         //--
00327         p2 = closestPtTriangle(t2, t1.v3);
00328         p1 = t1.v3;
00329         if (((p2 - p1) % (p2 - p1)) < threshSq) {
00330                 if ( !(p2==t2.v1) && !(p2==t2.v2) && !(p2==t2.v3) ) {
00331                         contactPoints->push_back( std::pair<position, position>(p1,p2));
00332                 }
00333         }
00334         //------------------------------------------------------------------
00335 
00336         //nine edge-edge tests
00337         //if one of the closest points on an edge is actually a vertex, skip this pair
00338         //as we have already done the vertex closest points
00339         double d;
00340         d = segmSegmDistanceSq(t1.v1, t1.v2, t2.v1, t2.v2, p1, p2);
00341         if (d < threshSq) {
00342                 if ( !(p1==t1.v1) && !(p1==t1.v2) && !(p2==t2.v1) && !(p2==t2.v2) ) {
00343                         contactPoints->push_back( std::pair<position, position>(p1,p2));
00344                 }
00345         }
00346         //--
00347         d = segmSegmDistanceSq(t1.v1, t1.v2, t2.v2, t2.v3, p1, p2);
00348         if (d < threshSq) {
00349                 if ( !(p1==t1.v1) && !(p1==t1.v2) && !(p2==t2.v2) && !(p2==t2.v3) ) {
00350                         contactPoints->push_back( std::pair<position, position>(p1,p2));
00351                 }
00352         }
00353         //--
00354         d = segmSegmDistanceSq(t1.v1, t1.v2, t2.v3, t2.v1, p1, p2);
00355         if (d < threshSq) {
00356                 if ( !(p1==t1.v1) && !(p1==t1.v2) && !(p2==t2.v3) && !(p2==t2.v1) ) {
00357                         contactPoints->push_back( std::pair<position, position>(p1,p2));
00358                 }
00359         }
00360         //------------------
00361         d = segmSegmDistanceSq(t1.v2, t1.v3, t2.v1, t2.v2, p1, p2);
00362         if (d < threshSq) {
00363                 if ( !(p1==t1.v2) && !(p1==t1.v3) && !(p2==t2.v1) && !(p2==t2.v2) ) {
00364                         contactPoints->push_back( std::pair<position, position>(p1,p2));
00365                 }
00366         }
00367         //--
00368         d = segmSegmDistanceSq(t1.v2, t1.v3, t2.v2, t2.v3, p1, p2);
00369         if (d < threshSq) {
00370                 if ( !(p1==t1.v2) && !(p1==t1.v3) && !(p2==t2.v2) && !(p2==t2.v3) ) {
00371                         contactPoints->push_back( std::pair<position, position>(p1,p2));
00372                 }
00373         }
00374         //--
00375         d = segmSegmDistanceSq(t1.v2, t1.v3, t2.v3, t2.v1, p1, p2);
00376         if (d < threshSq) {
00377                 if ( !(p1==t1.v2) && !(p1==t1.v3) && !(p2==t2.v3) && !(p2==t2.v1) ) {
00378                         contactPoints->push_back( std::pair<position, position>(p1,p2));
00379                 }
00380         }
00381         //------------------
00382         d = segmSegmDistanceSq(t1.v3, t1.v1, t2.v1, t2.v2, p1, p2);
00383         if (d < threshSq) {
00384                 if ( !(p1==t1.v3) && !(p1==t1.v1) && !(p2==t2.v1) && !(p2==t2.v2) ) {
00385                         contactPoints->push_back( std::pair<position, position>(p1,p2));
00386                 }
00387         }
00388         //--
00389         d = segmSegmDistanceSq(t1.v3, t1.v1, t2.v2, t2.v3, p1, p2);
00390         if (d < threshSq) {
00391                 if ( !(p1==t1.v3) && !(p1==t1.v1) && !(p2==t2.v2) && !(p2==t2.v3) ) {
00392                         contactPoints->push_back( std::pair<position, position>(p1,p2));
00393                 }
00394         }
00395         //--
00396         d = segmSegmDistanceSq(t1.v3, t1.v1, t2.v3, t2.v1, p1, p2);
00397         if (d < threshSq) {
00398                 if ( !(p1==t1.v3) && !(p1==t1.v1) && !(p2==t2.v3) && !(p2==t2.v1) ) {
00399                         contactPoints->push_back( std::pair<position, position>(p1,p2));
00400                 }
00401         }
00402         return contactPoints->size();
00403 }
00404 
00405 double triangleTriangleDistanceSq(const Triangle &t1, const Triangle &t2,
00406                                                                   position &p1, position &p2)
00407 {
00408         if (triangleIntersection(t1,t2)) return -1.0;
00409 
00410         double dtmp, dmin;
00411         position tmp1, tmp2;
00412         //six vertex - face tests
00413         //--
00414         p1 = closestPtTriangle(t1, t2.v1);
00415         p2 = t2.v1;
00416         dmin = (p2 - p1) % (p2 - p1);
00417         //--
00418         tmp1 = closestPtTriangle(t1, t2.v2);
00419         tmp2 = t2.v2;
00420         dtmp = (tmp2 - tmp1) % (tmp2 - tmp1);
00421         if (dtmp < dmin) {
00422                 dmin = dtmp;
00423                 p1 = tmp1; p2 = tmp2;
00424         }
00425         //--
00426         tmp1 = closestPtTriangle(t1, t2.v3);
00427         tmp2 = t2.v3;
00428         dtmp = (tmp2 - tmp1) % (tmp2 - tmp1);
00429         if (dtmp < dmin) {
00430                 dmin = dtmp;
00431                 p1 = tmp1; p2 = tmp2;
00432         }
00433         //--
00434         tmp2 = closestPtTriangle(t2, t1.v1);
00435         tmp1 = t1.v1;
00436         dtmp = (tmp2 - tmp1) % (tmp2 - tmp1);
00437         if (dtmp < dmin) {
00438                 dmin = dtmp;
00439                 p1 = tmp1; p2 = tmp2;
00440         }
00441         //--
00442         tmp2 = closestPtTriangle(t2, t1.v2);
00443         tmp1 = t1.v2;
00444         dtmp = (tmp2 - tmp1) % (tmp2 - tmp1);
00445         if (dtmp < dmin) {
00446                 dmin = dtmp;
00447                 p1 = tmp1; p2 = tmp2;
00448         }
00449         //--
00450         tmp2 = closestPtTriangle(t2, t1.v3);
00451         tmp1 = t1.v3;
00452         dtmp = (tmp2 - tmp1) % (tmp2 - tmp1);
00453         if (dtmp < dmin) {
00454                 dmin = dtmp;
00455                 p1 = tmp1; p2 = tmp2;
00456         }
00457         //------------------------------------------------------------------
00458 
00459         //nine edge-edge tests
00460         dtmp = segmSegmDistanceSq(t1.v1, t1.v2, t2.v1, t2.v2, tmp1, tmp2);
00461         if (dtmp < dmin) {
00462                 dmin = dtmp;
00463                 p1 = tmp1; p2 = tmp2;
00464         }
00465         //--
00466         dtmp = segmSegmDistanceSq(t1.v1, t1.v2, t2.v2, t2.v3, tmp1, tmp2);
00467         if (dtmp < dmin) {
00468                 dmin = dtmp;
00469                 p1 = tmp1; p2 = tmp2;
00470         }
00471         //--
00472         dtmp = segmSegmDistanceSq(t1.v1, t1.v2, t2.v3, t2.v1, tmp1, tmp2);
00473         if (dtmp < dmin) {
00474                 dmin = dtmp;
00475                 p1 = tmp1; p2 = tmp2;
00476         }
00477         //-------------------------------
00478         dtmp = segmSegmDistanceSq(t1.v2, t1.v3, t2.v1, t2.v2, tmp1, tmp2);
00479         if (dtmp < dmin) {
00480                 dmin = dtmp;
00481                 p1 = tmp1; p2 = tmp2;
00482         }
00483         //--
00484         dtmp = segmSegmDistanceSq(t1.v2, t1.v3, t2.v2, t2.v3, tmp1, tmp2);
00485         if (dtmp < dmin) {
00486                 dmin = dtmp;
00487                 p1 = tmp1; p2 = tmp2;
00488         }
00489         //--
00490         dtmp = segmSegmDistanceSq(t1.v2, t1.v3, t2.v3, t2.v1, tmp1, tmp2);
00491         if (dtmp < dmin) {
00492                 dmin = dtmp;
00493                 p1 = tmp1; p2 = tmp2;
00494         }
00495         //-----------------------------------
00496         dtmp = segmSegmDistanceSq(t1.v3, t1.v1, t2.v1, t2.v2, tmp1, tmp2);
00497         if (dtmp < dmin) {
00498                 dmin = dtmp;
00499                 p1 = tmp1; p2 = tmp2;
00500         }
00501         //--
00502         dtmp = segmSegmDistanceSq(t1.v3, t1.v1, t2.v2, t2.v3, tmp1, tmp2);
00503         if (dtmp < dmin) {
00504                 dmin = dtmp;
00505                 p1 = tmp1; p2 = tmp2;
00506         }
00507         //--
00508         dtmp = segmSegmDistanceSq(t1.v3, t1.v1, t2.v3, t2.v1, tmp1, tmp2);
00509         if (dtmp < dmin) {
00510                 dmin = dtmp;
00511                 p1 = tmp1; p2 = tmp2;
00512         }
00513         //--------
00514 
00515 
00516         return dmin;
00517 }
/opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit_source/include/triangle_inl.h
