All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends

narrowphase.cpp

00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  */
00034 
00037 #include "fcl/narrowphase/narrowphase.h"
00038 #include <boost/math/constants/constants.hpp>
00039 #include <vector>
00040 
00041 namespace fcl
00042 {
00043 
00044 namespace details
00045 {
00046 
00047 bool sphereSphereIntersect(const Sphere& s1, const Transform3f& tf1, 
00048                            const Sphere& s2, const Transform3f& tf2,
00049                            Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal)
00050 {
00051   Vec3f diff = tf1.transform(Vec3f()) - tf2.transform(Vec3f());
00052   FCL_REAL len = diff.length();
00053   if(len > s1.radius + s2.radius)
00054     return false;
00055 
00056   if(penetration_depth) 
00057     *penetration_depth = s1.radius + s2.radius - len;
00058   if(normal) 
00059   {
00060     if(len > 0)
00061       *normal = diff / len;
00062     else
00063       *normal = diff;
00064   }
00065 
00066   if(contact_points)
00067     *contact_points = tf1.transform(Vec3f()) + diff * 0.5;
00068   
00069   return true;
00070 }
00071 
00072 
00073 bool sphereSphereDistance(const Sphere& s1, const Transform3f& tf1,
00074                           const Sphere& s2, const Transform3f& tf2,
00075                           FCL_REAL* dist)
00076 {
00077   Vec3f diff = tf1.transform(Vec3f()) - tf2.transform(Vec3f());
00078   FCL_REAL len = diff.length();
00079   if(len > s1.radius + s2.radius)
00080   {
00081     *dist = len - (s1.radius + s2.radius);
00082     return true;
00083   }
00084 
00085   *dist = -1;
00086   return false;
00087 }
00088 
00090 FCL_REAL segmentSqrDistance(const Vec3f& from, const Vec3f& to,const Vec3f& p, Vec3f& nearest) 
00091 {
00092   Vec3f diff = p - from;
00093   Vec3f v = to - from;
00094   FCL_REAL t = v.dot(diff);
00095         
00096   if(t > 0) 
00097   {
00098     FCL_REAL dotVV = v.dot(v);
00099     if(t < dotVV) 
00100     {
00101       t /= dotVV;
00102       diff -= v * t;
00103     } 
00104     else 
00105     {
00106       t = 1;
00107       diff -= v;
00108     }
00109   } 
00110   else
00111     t = 0;
00112 
00113   nearest = from + v * t;
00114   return diff.dot(diff);        
00115 }
00116 
00118 bool projectInTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3, const Vec3f& normal, const Vec3f& p)
00119 {
00120   Vec3f edge1(p2 - p1);
00121   Vec3f edge2(p3 - p2);
00122   Vec3f edge3(p1 - p3);
00123 
00124   Vec3f p1_to_p(p - p1);
00125   Vec3f p2_to_p(p - p2);
00126   Vec3f p3_to_p(p - p3);
00127 
00128   Vec3f edge1_normal(edge1.cross(normal));
00129   Vec3f edge2_normal(edge2.cross(normal));
00130   Vec3f edge3_normal(edge3.cross(normal));
00131         
00132   FCL_REAL r1, r2, r3;
00133   r1 = edge1_normal.dot(p1_to_p);
00134   r2 = edge2_normal.dot(p2_to_p);
00135   r3 = edge3_normal.dot(p3_to_p);
00136   if ( ( r1 > 0 && r2 > 0 && r3 > 0 ) ||
00137        ( r1 <= 0 && r2 <= 0 && r3 <= 0 ) )
00138     return true;
00139   return false;
00140 }
00141 
00142 
00143 bool sphereTriangleIntersect(const Sphere& s, const Transform3f& tf,
00144                              const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal_)
00145 {
00146   Vec3f normal = (P2 - P1).cross(P3 - P1);
00147   normal.normalize();
00148   const Vec3f& center = tf.getTranslation();
00149   const FCL_REAL& radius = s.radius;
00150   FCL_REAL radius_with_threshold = radius + std::numeric_limits<FCL_REAL>::epsilon();
00151   Vec3f p1_to_center = center - P1;
00152   FCL_REAL distance_from_plane = p1_to_center.dot(normal);
00153 
00154   if(distance_from_plane < 0)
00155   {
00156     distance_from_plane *= -1;
00157     normal *= -1;
00158   }
00159 
00160   bool is_inside_contact_plane = (distance_from_plane < radius_with_threshold);
00161   
00162   bool has_contact = false;
00163   Vec3f contact_point;
00164   if(is_inside_contact_plane)
00165   {
00166     if(projectInTriangle(P1, P2, P3, center, normal))
00167     {
00168       has_contact = true;
00169       contact_point = center - normal * distance_from_plane;
00170     }
00171     else
00172     {
00173       FCL_REAL contact_capsule_radius_sqr = radius_with_threshold * radius_with_threshold;
00174       Vec3f nearest_on_edge;
00175       FCL_REAL distance_sqr;
00176       distance_sqr = segmentSqrDistance(P1, P2, center, nearest_on_edge);
00177       if(distance_sqr < contact_capsule_radius_sqr)
00178       {
00179         has_contact = true;
00180         contact_point = nearest_on_edge;
00181       }
00182 
00183       distance_sqr = segmentSqrDistance(P2, P3, center, nearest_on_edge);
00184       if(distance_sqr < contact_capsule_radius_sqr)
00185       {
00186         has_contact = true;
00187         contact_point = nearest_on_edge;
00188       }
00189 
00190       distance_sqr = segmentSqrDistance(P3, P1, center, nearest_on_edge);
00191       if(distance_sqr < contact_capsule_radius_sqr)
00192       {
00193         has_contact = true;
00194         contact_point = nearest_on_edge;
00195       }
00196     }
00197   }
00198 
00199   if(has_contact)
00200   {
00201     Vec3f contact_to_center = center - contact_point;
00202     FCL_REAL distance_sqr = contact_to_center.sqrLength();
00203 
00204     if(distance_sqr < radius_with_threshold * radius_with_threshold)
00205     {
00206       if(distance_sqr > 0)
00207       {
00208         FCL_REAL distance = std::sqrt(distance_sqr);
00209         if(normal_) *normal_ = normalize(contact_to_center);
00210         if(contact_points) *contact_points = contact_point;
00211         if(penetration_depth) *penetration_depth = -(radius - distance);
00212       }
00213       else
00214       {
00215         FCL_REAL distance = 0;
00216         if(normal_) *normal_ = normal;
00217         if(contact_points) *contact_points = contact_point;
00218         if(penetration_depth) *penetration_depth = -radius;
00219       }
00220 
00221       return true;
00222     }
00223   }
00224 
00225   return false;
00226 }
00227 
00228 bool sphereTriangleDistance(const Sphere& sp, const Transform3f& tf,
00229                             const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
00230                             FCL_REAL* dist)
00231 {
00232   // from geometric tools, very different from the collision code.
00233 
00234   const Vec3f& center = tf.getTranslation();
00235   FCL_REAL radius = sp.radius;
00236   Vec3f diff = P1 - center;
00237   Vec3f edge0 = P2 - P1;
00238   Vec3f edge1 = P3 - P1;
00239   FCL_REAL a00 = edge0.sqrLength();
00240   FCL_REAL a01 = edge0.dot(edge1);
00241   FCL_REAL a11 = edge1.sqrLength();
00242   FCL_REAL b0 = diff.dot(edge0);
00243   FCL_REAL b1 = diff.dot(edge1);
00244   FCL_REAL c = diff.sqrLength();
00245   FCL_REAL det = fabs(a00*a11 - a01*a01);
00246   FCL_REAL s = a01*b1 - a11*b0;
00247   FCL_REAL t = a01*b0 - a00*b1;
00248 
00249   FCL_REAL sqr_dist;
00250 
00251   if(s + t <= det)
00252   {
00253     if(s < 0)
00254     {
00255       if(t < 0)  // region 4
00256       {
00257         if(b0 < 0)
00258         {
00259           t = 0;
00260           if(-b0 >= a00)
00261           {
00262             s = 1;
00263             sqr_dist = a00 + 2*b0 + c;
00264           }
00265           else
00266           {
00267             s = -b0/a00;
00268             sqr_dist = b0*s + c;
00269           }
00270         }
00271         else
00272         {
00273           s = 0;
00274           if(b1 >= 0)
00275           {
00276             t = 0;
00277             sqr_dist = c;
00278           }
00279           else if(-b1 >= a11)
00280           {
00281             t = 1;
00282             sqr_dist = a11 + 2*b1 + c;
00283           }
00284           else
00285           {
00286             t = -b1/a11;
00287             sqr_dist = b1*t + c;
00288           }
00289         }
00290       }
00291       else  // region 3
00292       {
00293         s = 0;
00294         if(b1 >= 0)
00295         {
00296           t = 0;
00297           sqr_dist = c;
00298         }
00299         else if(-b1 >= a11)
00300         {
00301           t = 1;
00302           sqr_dist = a11 + 2*b1 + c;
00303         }
00304         else
00305         {
00306           t = -b1/a11;
00307           sqr_dist = b1*t + c;
00308         }
00309       }
00310     }
00311     else if(t < 0)  // region 5
00312     {
00313       t = 0;
00314       if(b0 >= 0)
00315       {
00316         s = 0;
00317         sqr_dist = c;
00318       }
00319       else if(-b0 >= a00)
00320       {
00321         s = 1;
00322         sqr_dist = a00 + 2*b0 + c;
00323       }
00324       else
00325       {
00326         s = -b0/a00;
00327         sqr_dist = b0*s + c;
00328       }
00329     }
00330     else  // region 0
00331     {
00332       // minimum at interior point
00333       FCL_REAL inv_det = (1)/det;
00334       s *= inv_det;
00335       t *= inv_det;
00336       sqr_dist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c;
00337     }
00338   }
00339   else
00340   {
00341     FCL_REAL tmp0, tmp1, numer, denom;
00342 
00343     if(s < 0)  // region 2
00344     {
00345       tmp0 = a01 + b0;
00346       tmp1 = a11 + b1;
00347       if(tmp1 > tmp0)
00348       {
00349         numer = tmp1 - tmp0;
00350         denom = a00 - 2*a01 + a11;
00351         if(numer >= denom)
00352         {
00353           s = 1;
00354           t = 0;
00355           sqr_dist = a00 + 2*b0 + c;
00356         }
00357         else
00358         {
00359           s = numer/denom;
00360           t = 1 - s;
00361           sqr_dist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c;
00362         }
00363       }
00364       else
00365       {
00366         s = 0;
00367         if(tmp1 <= 0)
00368         {
00369           t = 1;
00370           sqr_dist = a11 + 2*b1 + c;
00371         }
00372         else if(b1 >= 0)
00373         {
00374           t = 0;
00375           sqr_dist = c;
00376         }
00377         else
00378         {
00379           t = -b1/a11;
00380           sqr_dist = b1*t + c;
00381         }
00382       }
00383     }
00384     else if(t < 0)  // region 6
00385     {
00386       tmp0 = a01 + b1;
00387       tmp1 = a00 + b0;
00388       if(tmp1 > tmp0)
00389       {
00390         numer = tmp1 - tmp0;
00391         denom = a00 - 2*a01 + a11;
00392         if(numer >= denom)
00393         {
00394           t = 1;
00395           s = 0;
00396           sqr_dist = a11 + 2*b1 + c;
00397         }
00398         else
00399         {
00400           t = numer/denom;
00401           s = 1 - t;
00402           sqr_dist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c;
00403         }
00404       }
00405       else
00406       {
00407         t = 0;
00408         if(tmp1 <= 0)
00409         {
00410           s = 1;
00411           sqr_dist = a00 + 2*b0 + c;
00412         }
00413         else if(b0 >= 0)
00414         {
00415           s = 0;
00416           sqr_dist = c;
00417         }
00418         else
00419         {
00420           s = -b0/a00;
00421           sqr_dist = b0*s + c;
00422         }
00423       }
00424     }
00425     else  // region 1
00426     {
00427       numer = a11 + b1 - a01 - b0;
00428       if(numer <= 0)
00429       {
00430         s = 0;
00431         t = 1;
00432         sqr_dist = a11 + 2*b1 + c;
00433       }
00434       else
00435       {
00436         denom = a00 - 2*a01 + a11;
00437         if(numer >= denom)
00438         {
00439           s = 1;
00440           t = 0;
00441           sqr_dist = a00 + 2*b0 + c;
00442         }
00443         else
00444         {
00445           s = numer/denom;
00446           t = 1 - s;
00447           sqr_dist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c;
00448         }
00449       }
00450     }
00451   }
00452 
00453   // Account for numerical round-off error.
00454   if(sqr_dist < 0)
00455     sqr_dist = 0;
00456 
00457   if(sqr_dist > radius * radius)
00458   {
00459     *dist = std::sqrt(sqr_dist) - radius;
00460     return true;
00461   }
00462   else
00463   {
00464     *dist = -1;
00465     return false;
00466   }
00467 }
00468 
00469 
00470 
00471 struct ContactPoint
00472 {
00473   Vec3f normal;
00474   Vec3f point;
00475   FCL_REAL depth;
00476   ContactPoint(const Vec3f& n, const Vec3f& p, FCL_REAL d) : normal(n), point(p), depth(d) {}
00477 };
00478 
00479 
00480 static inline void lineClosestApproach(const Vec3f& pa, const Vec3f& ua,
00481                                        const Vec3f& pb, const Vec3f& ub,
00482                                        FCL_REAL* alpha, FCL_REAL* beta)
00483 {
00484   Vec3f p = pb - pa;
00485   FCL_REAL uaub = ua.dot(ub);
00486   FCL_REAL q1 = ua.dot(p);
00487   FCL_REAL q2 = -ub.dot(p);
00488   FCL_REAL d = 1 - uaub * uaub;
00489   if(d <= (FCL_REAL)(0.0001f))
00490   {
00491     *alpha = 0;
00492     *beta = 0;
00493   }
00494   else
00495   {
00496     d = 1 / d;
00497     *alpha = (q1 + uaub * q2) * d;
00498     *beta = (uaub * q1 + q2) * d;
00499   }
00500 }
00501 
00502 // find all the intersection points between the 2D rectangle with vertices
00503 // at (+/-h[0],+/-h[1]) and the 2D quadrilateral with vertices (p[0],p[1]),
00504 // (p[2],p[3]),(p[4],p[5]),(p[6],p[7]).
00505 //
00506 // the intersection points are returned as x,y pairs in the 'ret' array.
00507 // the number of intersection points is returned by the function (this will
00508 // be in the range 0 to 8).
00509 static int intersectRectQuad2(FCL_REAL h[2], FCL_REAL p[8], FCL_REAL ret[16])
00510 {
00511   // q (and r) contain nq (and nr) coordinate points for the current (and
00512   // chopped) polygons
00513   int nq = 4, nr = 0;
00514   FCL_REAL buffer[16];
00515   FCL_REAL* q = p;
00516   FCL_REAL* r = ret;
00517   for(int dir = 0; dir <= 1; ++dir) 
00518   {
00519     // direction notation: xy[0] = x axis, xy[1] = y axis
00520     for(int sign = -1; sign <= 1; sign += 2) 
00521     {
00522       // chop q along the line xy[dir] = sign*h[dir]
00523       FCL_REAL* pq = q;
00524       FCL_REAL* pr = r;
00525       nr = 0;
00526       for(int i = nq; i > 0; --i) 
00527       {
00528         // go through all points in q and all lines between adjacent points
00529         if(sign * pq[dir] < h[dir]) 
00530         {
00531           // this point is inside the chopping line
00532           pr[0] = pq[0];
00533           pr[1] = pq[1];
00534           pr += 2;
00535           nr++;
00536           if(nr & 8) 
00537           {
00538             q = r;
00539             goto done;
00540           }
00541         }
00542         FCL_REAL* nextq = (i > 1) ? pq+2 : q;
00543         if((sign*pq[dir] < h[dir]) ^ (sign*nextq[dir] < h[dir])) 
00544         {
00545           // this line crosses the chopping line
00546           pr[1-dir] = pq[1-dir] + (nextq[1-dir]-pq[1-dir]) /
00547             (nextq[dir]-pq[dir]) * (sign*h[dir]-pq[dir]);
00548           pr[dir] = sign*h[dir];
00549           pr += 2;
00550           nr++;
00551           if(nr & 8) 
00552           {
00553             q = r;
00554             goto done;
00555           }
00556         }
00557         pq += 2;
00558       }
00559       q = r;
00560       r = (q == ret) ? buffer : ret;
00561       nq = nr;
00562     }
00563   }
00564  
00565  done:
00566   if(q != ret) memcpy(ret, q, nr*2*sizeof(FCL_REAL));
00567   return nr;  
00568 }
00569 
00570 // given n points in the plane (array p, of size 2*n), generate m points that
00571 // best represent the whole set. the definition of 'best' here is not
00572 // predetermined - the idea is to select points that give good box-box
00573 // collision detection behavior. the chosen point indexes are returned in the
00574 // array iret (of size m). 'i0' is always the first entry in the array.
00575 // n must be in the range [1..8]. m must be in the range [1..n]. i0 must be
00576 // in the range [0..n-1].
00577 static inline void cullPoints2(int n, FCL_REAL p[], int m, int i0, int iret[])
00578 {
00579   // compute the centroid of the polygon in cx,cy
00580   FCL_REAL a, cx, cy, q;
00581   switch(n)
00582   {
00583   case 1:
00584     cx = p[0];
00585     cy = p[1];
00586     break;
00587   case 2:
00588     cx = 0.5 * (p[0] + p[2]);
00589     cy = 0.5 * (p[1] + p[3]);
00590     break;
00591   default:
00592     a = 0;
00593     cx = 0;
00594     cy = 0;
00595     for(int i = 0; i < n-1; ++i) 
00596     {
00597       q = p[i*2]*p[i*2+3] - p[i*2+2]*p[i*2+1];
00598       a += q;
00599       cx += q*(p[i*2]+p[i*2+2]);
00600       cy += q*(p[i*2+1]+p[i*2+3]);
00601     }
00602     q = p[n*2-2]*p[1] - p[0]*p[n*2-1];
00603     if(std::abs(a+q) > std::numeric_limits<FCL_REAL>::epsilon())
00604       a = 1/(3*(a+q));
00605     else
00606       a= 1e18f;
00607     
00608     cx = a*(cx + q*(p[n*2-2]+p[0]));
00609     cy = a*(cy + q*(p[n*2-1]+p[1]));
00610   }
00611 
00612 
00613   // compute the angle of each point w.r.t. the centroid
00614   FCL_REAL A[8];
00615   for(int i = 0; i < n; ++i) 
00616     A[i] = atan2(p[i*2+1]-cy,p[i*2]-cx);
00617 
00618   // search for points that have angles closest to A[i0] + i*(2*pi/m).
00619   int avail[8];
00620   for(int i = 0; i < n; ++i) avail[i] = 1;
00621   avail[i0] = 0;
00622   iret[0] = i0;
00623   iret++;
00624   const double pi = boost::math::constants::pi<FCL_REAL>();
00625   for(int j = 1; j < m; ++j) 
00626   {
00627     a = j*(2*pi/m) + A[i0];
00628     if (a > pi) a -= 2*pi;
00629     FCL_REAL maxdiff= 1e9, diff;
00630 
00631     *iret = i0; // iret is not allowed to keep this value, but it sometimes does, when diff=#QNAN0
00632     for(int i = 0; i < n; ++i) 
00633     {
00634       if(avail[i]) 
00635       {
00636         diff = std::abs(A[i]-a);
00637         if(diff > pi) diff = 2*pi - diff;
00638         if(diff < maxdiff) 
00639         {
00640           maxdiff = diff;
00641           *iret = i;
00642         }
00643       }
00644     }
00645     avail[*iret] = 0;
00646     iret++;
00647   }
00648 }
00649 
00650 
00651 
00652 int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
00653             const Vec3f& side2, const Matrix3f& R2, const Vec3f& T2,
00654             Vec3f& normal, FCL_REAL* depth, int* return_code,
00655             int maxc, std::vector<ContactPoint>& contacts)
00656 {
00657   const FCL_REAL fudge_factor = FCL_REAL(1.05);
00658   Vec3f normalC;
00659   FCL_REAL s, s2, l;
00660   int invert_normal, code;
00661 
00662   Vec3f p = T2 - T1; // get vector from centers of box 1 to box 2, relative to box 1
00663   Vec3f pp = R1.transposeTimes(p); // get pp = p relative to body 1
00664 
00665   // get side lengths / 2
00666   Vec3f A = side1 * 0.5;
00667   Vec3f B = side2 * 0.5;
00668 
00669   // Rij is R1'*R2, i.e. the relative rotation between R1 and R2
00670   Matrix3f R = R1.transposeTimes(R2);
00671   Matrix3f Q = abs(R);
00672 
00673 
00674   // for all 15 possible separating axes:
00675   //   * see if the axis separates the boxes. if so, return 0.
00676   //   * find the depth of the penetration along the separating axis (s2)
00677   //   * if this is the largest depth so far, record it.
00678   // the normal vector will be set to the separating axis with the smallest
00679   // depth. note: normalR is set to point to a column of R1 or R2 if that is
00680   // the smallest depth normal so far. otherwise normalR is 0 and normalC is
00681   // set to a vector relative to body 1. invert_normal is 1 if the sign of
00682   // the normal should be flipped.
00683 
00684   int best_col_id = -1;
00685   FCL_REAL tmp = 0;
00686 
00687   s = - std::numeric_limits<FCL_REAL>::max();
00688   invert_normal = 0;
00689   code = 0;
00690 
00691   // separating axis = u1, u2, u3
00692   tmp = pp[0];
00693   s2 = std::abs(tmp) - (Q.dotX(B) + A[0]);
00694   if(s2 > 0) { *return_code = 0; return 0; }
00695   if(s2 > s) 
00696   {
00697     s = s2; 
00698     best_col_id = 0;
00699     invert_normal = (tmp < 0);
00700     code = 1;
00701   }
00702 
00703   tmp = pp[1]; 
00704   s2 = std::abs(tmp) - (Q.dotY(B) + A[1]);
00705   if(s2 > 0) { *return_code = 0; return 0; }
00706   if(s2 > s) 
00707   {
00708     s = s2; 
00709     best_col_id = 1;
00710     invert_normal = (tmp < 0);
00711     code = 2;
00712   }
00713 
00714   tmp = pp[2];
00715   s2 = std::abs(tmp) - (Q.dotZ(B) + A[2]);
00716   if(s2 > 0) { *return_code = 0; return 0; }
00717   if(s2 > s)
00718   {
00719     s = s2;
00720     best_col_id = 2;
00721     invert_normal = (tmp < 0);
00722     code = 3;
00723   }
00724 
00725   // separating axis = v1, v2, v3
00726   tmp = R2.transposeDotX(p);
00727   s2 = std::abs(tmp) - (Q.transposeDotX(A) + B[0]);
00728   if(s2 > 0) { *return_code = 0; return 0; }
00729   if(s2 > s)
00730   {
00731     s = s2;
00732     best_col_id = 0;
00733     code = 4;
00734   }
00735 
00736   tmp = R2.transposeDotY(p);
00737   s2 = std::abs(tmp) - (Q.transposeDotY(A) + B[1]);
00738   if(s2 > 0) { *return_code = 0; return 0; }
00739   if(s2 > s)
00740   {
00741     s = s2;
00742     best_col_id = 1;
00743     code = 5;
00744   }
00745 
00746   tmp = R2.transposeDotZ(p);
00747   s2 =  std::abs(tmp) - (Q.transposeDotZ(A) + B[2]);
00748   if(s2 > 0) { *return_code = 0; return 0; }
00749   if(s2 > s)
00750   {
00751     s = s2;
00752     best_col_id = 2;
00753     code = 6;
00754   }
00755   
00756 
00757   FCL_REAL fudge2(1.0e-6);
00758   Q += fudge2;
00759 
00760   Vec3f n;
00761   FCL_REAL eps = std::numeric_limits<FCL_REAL>::epsilon();
00762 
00763   // separating axis = u1 x (v1,v2,v3)
00764   tmp = pp[2] * R(1, 0) - pp[1] * R(2, 0);
00765   s2 = std::abs(tmp) - (A[1] * Q(2, 0) + A[2] * Q(1, 0) + B[1] * Q(0, 2) + B[2] * Q(0, 1));
00766   if(s2 > eps) { *return_code = 0; return 0; }
00767   n = Vec3f(0, -R(2, 0), R(1, 0));
00768   l = n.length();
00769   if(l > eps) 
00770   {
00771     s2 /= l;
00772     if(s2 * fudge_factor > s)
00773     {
00774       s = s2;
00775       best_col_id = 0;
00776       normalC = n / l;
00777       invert_normal = (tmp < 0);
00778       code = 7;
00779     }
00780   }
00781 
00782   tmp = pp[2] * R(1, 1) - pp[1] * R(2, 1);
00783   s2 = std::abs(tmp) - (A[1] * Q(2, 1) + A[2] * Q(1, 1) + B[0] * Q(0, 2) + B[2] * Q(0, 0));
00784   if(s2 > eps) { *return_code = 0; return 0; }
00785   n = Vec3f(0, -R(2, 1), R(1, 1));
00786   l = n.length();
00787   if(l > eps) 
00788   {
00789     s2 /= l;
00790     if(s2 * fudge_factor > s)
00791     {
00792       s = s2;
00793       best_col_id = 0;
00794       normalC = n / l;
00795       invert_normal = (tmp < 0);
00796       code = 8;
00797     }
00798   }
00799   
00800   tmp = pp[2] * R(1, 2) - pp[1] * R(2, 2);
00801   s2 = std::abs(tmp) - (A[1] * Q(2, 2) + A[2] * Q(1, 2) + B[0] * Q(0, 1) + B[1] * Q(0, 0));
00802   if(s2 > eps) { *return_code = 0; return 0; }
00803   n = Vec3f(0, -R(2, 2), R(1, 2));
00804   l = n.length();
00805   if(l > eps) 
00806   {
00807     s2 /= l;
00808     if(s2 * fudge_factor > s)
00809     {
00810       s = s2;
00811       best_col_id = 0;
00812       normalC = n / l;
00813       invert_normal = (tmp < 0);
00814       code = 9;
00815     }
00816   }
00817 
00818   // separating axis = u2 x (v1,v2,v3)
00819   tmp = pp[0] * R(2, 0) - pp[2] * R(0, 0);
00820   s2 = std::abs(tmp) - (A[0] * Q(2, 0) + A[2] * Q(0, 0) + B[1] * Q(1, 2) + B[2] * Q(1, 1));
00821   if(s2 > eps) { *return_code = 0; return 0; }
00822   n = Vec3f(R(2, 0), 0, -R(0, 0));
00823   l = n.length();
00824   if(l > eps) 
00825   {
00826     s2 /= l;
00827     if(s2 * fudge_factor > s)
00828     {
00829       s = s2;
00830       best_col_id = 0;
00831       normalC = n / l;
00832       invert_normal = (tmp < 0);
00833       code = 10;
00834     }
00835   }
00836 
00837   tmp = pp[0] * R(2, 1) - pp[2] * R(0, 1);
00838   s2 = std::abs(tmp) - (A[0] * Q(2, 1) + A[2] * Q(0, 1) + B[0] * Q(1, 2) + B[2] * Q(1, 0));
00839   if(s2 > eps) { *return_code = 0; return 0; }
00840   n = Vec3f(R(2, 1), 0, -R(0, 1));
00841   l = n.length();
00842   if(l > eps) 
00843   {
00844     s2 /= l;
00845     if(s2 * fudge_factor > s)
00846     {
00847       s = s2;
00848       best_col_id = 0;
00849       normalC = n / l;
00850       invert_normal = (tmp < 0);
00851       code = 11;
00852     }
00853   }
00854   
00855   tmp = pp[0] * R(2, 2) - pp[2] * R(0, 2);
00856   s2 = std::abs(tmp) - (A[0] * Q(2, 2) + A[2] * Q(0, 2) + B[0] * Q(1, 1) + B[1] * Q(1, 0));
00857   if(s2 > eps) { *return_code = 0; return 0; }
00858   n = Vec3f(R(2, 2), 0, -R(0, 2));
00859   l = n.length();
00860   if(l > eps) 
00861   {
00862     s2 /= l;
00863     if(s2 * fudge_factor > s)
00864     {
00865       s = s2;
00866       best_col_id = 0;
00867       normalC = n / l;
00868       invert_normal = (tmp < 0);
00869       code = 12;
00870     }
00871   }
00872 
00873   // separating axis = u3 x (v1,v2,v3)
00874   tmp = pp[1] * R(0, 0) - pp[0] * R(1, 0);
00875   s2 = std::abs(tmp) - (A[0] * Q(1, 0) + A[1] * Q(0, 0) + B[1] * Q(2, 2) + B[2] * Q(2, 1));
00876   if(s2 > eps) { *return_code = 0; return 0; }
00877   n = Vec3f(-R(1, 0), R(0, 0), 0);
00878   l = n.length();
00879   if(l > eps) 
00880   {
00881     s2 /= l;
00882     if(s2 * fudge_factor > s)
00883     {
00884       s = s2;
00885       best_col_id = 0;
00886       normalC = n / l;
00887       invert_normal = (tmp < 0);
00888       code = 13;
00889     }
00890   }
00891 
00892   tmp = pp[1] * R(0, 1) - pp[0] * R(1, 1);
00893   s2 = std::abs(tmp) - (A[0] * Q(1, 1) + A[1] * Q(0, 1) + B[0] * Q(2, 2) + B[2] * Q(2, 0));
00894   if(s2 > eps) { *return_code = 0; return 0; }
00895   n = Vec3f(-R(1, 1), R(0, 1), 0);
00896   l = n.length();
00897   if(l > eps) 
00898   {
00899     s2 /= l;
00900     if(s2 * fudge_factor > s)
00901     {
00902       s = s2;
00903       best_col_id = 0;
00904       normalC = n / l;
00905       invert_normal = (tmp < 0);
00906       code = 14;
00907     }
00908   }
00909   
00910   tmp = pp[1] * R(0, 2) - pp[0] * R(1, 2);
00911   s2 = std::abs(tmp) - (A[0] * Q(1, 2) + A[1] * Q(0, 2) + B[0] * Q(2, 1) + B[1] * Q(2, 0));
00912   if(s2 > eps) { *return_code = 0; return 0; }
00913   n = Vec3f(-R(1, 2), R(0, 2), 0);
00914   l = n.length();
00915   if(l > eps) 
00916   {
00917     s2 /= l;
00918     if(s2 * fudge_factor > s)
00919     {
00920       s = s2;
00921       best_col_id = 0;
00922       normalC = n / l;
00923       invert_normal = (tmp < 0);
00924       code = 15;
00925     }
00926   }
00927 
00928 
00929   
00930   if (!code) { *return_code = code; return 0; }
00931 
00932   // if we get to this point, the boxes interpenetrate. compute the normal
00933   // in global coordinates.
00934   if(best_col_id != -1) 
00935     normal = R.getColumn(best_col_id);
00936   else 
00937     normal = R1 * normalC;
00938   
00939   if(invert_normal) 
00940     normal.negate();
00941 
00942   *depth = -s;
00943 
00944   // compute contact point(s)
00945 
00946   if(code > 6) 
00947   {
00948     // an edge from box 1 touches an edge from box 2.
00949     // find a point pa on the intersecting edge of box 1
00950     Vec3f pa(T1);
00951     FCL_REAL sign;
00952   
00953     for(int j = 0; j < 3; ++j)
00954     {
00955       sign = (R1.transposeDot(j, normal) > 0) ? 1 : -1;
00956       pa += R1.getColumn(j) * (A[j] * sign);
00957     }
00958   
00959     // find a point pb on the intersecting edge of box 2
00960     Vec3f pb;
00961     pb = T2;
00962     for(int j = 0; j < 3; ++j)
00963     {
00964       sign = (R2.transposeDot(j, normal) > 0) ? 1 : -1;
00965       pb += R2.getColumn(j) * (B[j] * sign);
00966     }
00967 
00968     FCL_REAL alpha, beta;
00969     Vec3f ua(R1.getColumn((code-7)/3));
00970     Vec3f ub(R2.getColumn((code-7)%3));
00971     
00972     lineClosestApproach(pa, ua, pb, ub, &alpha, &beta);
00973     pa += ua * alpha;
00974     pb += ub * beta;
00975     
00976     
00977     Vec3f pointInWorld((pa + pb) * 0.5);
00978     contacts.push_back(ContactPoint(-normal, pointInWorld, -*depth));
00979     *return_code = code;
00980     
00981     return 1;
00982   }
00983 
00984   // okay, we have a face-something intersection (because the separating
00985   // axis is perpendicular to a face). define face 'a' to be the reference
00986   // face (i.e. the normal vector is perpendicular to this) and face 'b' to be
00987   // the incident face (the closest face of the other box).
00988   
00989   const Matrix3f *Ra, *Rb;
00990   const Vec3f *pa, *pb, *Sa, *Sb;
00991 
00992   if(code <= 3)
00993   {
00994     Ra = &R1;
00995     Rb = &R2;
00996     pa = &T1;
00997     pb = &T2;
00998     Sa = &A;
00999     Sb = &B;
01000   }
01001   else
01002   {
01003     Ra = &R2;
01004     Rb = &R1;
01005     pa = &T2;
01006     pb = &T1;
01007     Sa = &B;
01008     Sb = &A;
01009   }
01010 
01011   // nr = normal vector of reference face dotted with axes of incident box.
01012   // anr = absolute values of nr.
01013   Vec3f normal2, nr, anr;
01014   if(code <= 3) 
01015     normal2 = normal;
01016   else 
01017     normal2 = -normal;
01018 
01019   nr = Rb->transposeTimes(normal2);
01020   anr = abs(anr);
01021 
01022   // find the largest compontent of anr: this corresponds to the normal
01023   // for the indident face. the other axis numbers of the indicent face
01024   // are stored in a1,a2.
01025   int lanr, a1, a2;
01026   if(anr[1] > anr[0]) 
01027   {
01028     if(anr[1] > anr[2]) 
01029     {
01030       a1 = 0;
01031       lanr = 1;
01032       a2 = 2;
01033     }
01034     else 
01035     {
01036       a1 = 0;
01037       a2 = 1;
01038       lanr = 2;
01039     }
01040   }
01041   else 
01042   {
01043     if(anr[0] > anr[2]) 
01044     {
01045       lanr = 0;
01046       a1 = 1;
01047       a2 = 2;
01048     }
01049     else 
01050     {
01051       a1 = 0;
01052       a2 = 1;
01053       lanr = 2;
01054     }
01055   }
01056 
01057   // compute center point of incident face, in reference-face coordinates
01058   Vec3f center;
01059   if(nr[lanr] < 0) 
01060     center = (*pb) - (*pa) + Rb->getColumn(lanr) * ((*Sb)[lanr]);
01061   else
01062     center = (*pb) - (*pa) - Rb->getColumn(lanr) * ((*Sb)[lanr]);
01063 
01064   // find the normal and non-normal axis numbers of the reference box
01065   int codeN, code1, code2;
01066   if(code <= 3) 
01067     codeN = code-1; 
01068   else codeN = code-4;
01069   
01070   if(codeN == 0) 
01071   {
01072     code1 = 1;
01073     code2 = 2;
01074   }
01075   else if(codeN == 1) 
01076   {
01077     code1 = 0;
01078     code2 = 2;
01079   }
01080   else 
01081   {
01082     code1 = 0;
01083     code2 = 1;
01084   }
01085 
01086   // find the four corners of the incident face, in reference-face coordinates
01087   FCL_REAL quad[8]; // 2D coordinate of incident face (x,y pairs)
01088   FCL_REAL c1, c2, m11, m12, m21, m22;
01089   c1 = Ra->transposeDot(code1, center);
01090   c2 = Ra->transposeDot(code2, center);
01091   // optimize this? - we have already computed this data above, but it is not
01092   // stored in an easy-to-index format. for now it's quicker just to recompute
01093   // the four dot products.
01094   Vec3f tempRac = Ra->getColumn(code1);
01095   m11 = Rb->transposeDot(a1, tempRac);
01096   m12 = Rb->transposeDot(a2, tempRac);
01097   tempRac = Ra->getColumn(code2);
01098   m21 = Rb->transposeDot(a1, tempRac);
01099   m22 = Rb->transposeDot(a2, tempRac);
01100  
01101   FCL_REAL k1 = m11 * (*Sb)[a1];
01102   FCL_REAL k2 = m21 * (*Sb)[a1];
01103   FCL_REAL k3 = m12 * (*Sb)[a2];
01104   FCL_REAL k4 = m22 * (*Sb)[a2];
01105   quad[0] = c1 - k1 - k3;
01106   quad[1] = c2 - k2 - k4;
01107   quad[2] = c1 - k1 + k3;
01108   quad[3] = c2 - k2 + k4;
01109   quad[4] = c1 + k1 + k3;
01110   quad[5] = c2 + k2 + k4;
01111   quad[6] = c1 + k1 - k3;
01112   quad[7] = c2 + k2 - k4;
01113 
01114   // find the size of the reference face
01115   FCL_REAL rect[2];
01116   rect[0] = (*Sa)[code1];
01117   rect[1] = (*Sa)[code2];
01118 
01119   // intersect the incident and reference faces
01120   FCL_REAL ret[16];
01121   int n_intersect = intersectRectQuad2(rect, quad, ret);
01122   if(n_intersect < 1) { *return_code = code; return 0; } // this should never happen
01123 
01124   // convert the intersection points into reference-face coordinates,
01125   // and compute the contact position and depth for each point. only keep
01126   // those points that have a positive (penetrating) depth. delete points in
01127   // the 'ret' array as necessary so that 'point' and 'ret' correspond.
01128   Vec3f points[8]; // penetrating contact points
01129   FCL_REAL dep[8]; // depths for those points
01130   FCL_REAL det1 = 1.f/(m11*m22 - m12*m21);
01131   m11 *= det1;
01132   m12 *= det1;
01133   m21 *= det1;
01134   m22 *= det1;
01135   int cnum = 0; // number of penetrating contact points found
01136   for(int j = 0; j < n_intersect; ++j) 
01137   {
01138     FCL_REAL k1 =  m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2);
01139     FCL_REAL k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2);
01140     points[cnum] = center + Rb->getColumn(a1) * k1 + Rb->getColumn(a2) * k2;
01141     dep[cnum] = (*Sa)[codeN] - normal2.dot(points[cnum]);
01142     if(dep[cnum] >= 0) 
01143     {
01144       ret[cnum*2] = ret[j*2];
01145       ret[cnum*2+1] = ret[j*2+1];
01146       cnum++;
01147     }
01148   }
01149   if(cnum < 1) { *return_code = code; return 0; } // this should never happen
01150 
01151   // we can't generate more contacts than we actually have
01152   if(maxc > cnum) maxc = cnum;
01153   if(maxc < 1) maxc = 1;
01154 
01155   if(cnum <= maxc) 
01156   {
01157     if(code<4) 
01158     {
01159       // we have less contacts than we need, so we use them all
01160       for(int j = 0; j < cnum; ++j) 
01161       {
01162         Vec3f pointInWorld = points[j] + (*pa);
01163         contacts.push_back(ContactPoint(-normal, pointInWorld, -dep[j]));
01164       }
01165     } 
01166     else
01167     {
01168       // we have less contacts than we need, so we use them all
01169       for(int j = 0; j < cnum; ++j) 
01170       {
01171         Vec3f pointInWorld = points[j] + (*pa) - normal * dep[j];
01172         contacts.push_back(ContactPoint(-normal, pointInWorld, -dep[j]));
01173       }
01174     }
01175   }
01176   else 
01177   {
01178     // we have more contacts than are wanted, some of them must be culled.
01179     // find the deepest point, it is always the first contact.
01180     int i1 = 0;
01181     FCL_REAL maxdepth = dep[0];
01182     for(int i = 1; i < cnum; ++i) 
01183     {
01184       if(dep[i] > maxdepth) 
01185       {
01186         maxdepth = dep[i];
01187         i1 = i;
01188       }
01189     }
01190 
01191     int iret[8];
01192     cullPoints2(cnum, ret, maxc, i1, iret);
01193 
01194     for(int j = 0; j < maxc; ++j) 
01195     {
01196       Vec3f posInWorld = points[iret[j] * 3] + (*pa);
01197       if(code < 4)
01198         contacts.push_back(ContactPoint(-normal, posInWorld, -dep[iret[j]]));
01199       else
01200         contacts.push_back(ContactPoint(-normal, posInWorld - normal * dep[iret[j]], -dep[iret[j]]));
01201     }
01202     cnum = maxc;
01203   }
01204 
01205   *return_code = code;
01206   return cnum;
01207 }
01208 
01209 
01210 
01211 bool boxBoxIntersect(const Box& s1, const Transform3f& tf1,
01212                      const Box& s2, const Transform3f& tf2,
01213                      Vec3f* contact_points, FCL_REAL* penetration_depth_, Vec3f* normal_)
01214 {
01215   std::vector<ContactPoint> contacts;
01216   int return_code; 
01217   Vec3f normal;
01218   FCL_REAL depth;
01219   int cnum = boxBox2(s1.side, tf1.getRotation(), tf1.getTranslation(),
01220                      s2.side, tf2.getRotation(), tf2.getTranslation(),
01221                      normal, &depth, &return_code,
01222                      4, contacts);
01223 
01224   if(normal_) *normal_ = normal;
01225   if(penetration_depth_) *penetration_depth_ = depth;
01226 
01227   if(contact_points)
01228   {
01229     Vec3f contact_point;
01230     for(size_t i = 0; i < contacts.size(); ++i)
01231     {
01232       contact_point += contacts[i].point;
01233     }
01234 
01235     contact_point = contact_point / (FCL_REAL)contacts.size();
01236 
01237     *contact_points = contact_point;
01238   }
01239 
01240   return return_code != 0;
01241 }
01242 
01243 
01244 
01245 
01246 } // details
01247 
01248 
01249 template<>
01250 bool GJKSolver_libccd::shapeIntersect<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
01251                                                       const Sphere& s2, const Transform3f& tf2,
01252                                                       Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
01253 {
01254   return details::sphereSphereIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
01255 }
01256 
01257 template<> 
01258 bool GJKSolver_libccd::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf,
01259                                               const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
01260 {
01261   return details::sphereTriangleIntersect(s, tf, P1, P2, P3, contact_points, penetration_depth, normal);
01262 }
01263 
01264 template<> 
01265 bool GJKSolver_libccd::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf1,
01266                                               const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
01267 {
01268   return details::sphereTriangleIntersect(s, tf1, tf2.transform(P1), tf2.transform(P2), tf2.transform(P3), contact_points, penetration_depth, normal);
01269 }
01270 
01271 template<>
01272 bool GJKSolver_libccd::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
01273                                                      const Sphere& s2, const Transform3f& tf2,
01274                                                      FCL_REAL* dist) const
01275 {
01276   return details::sphereSphereDistance(s1, tf1, s2, tf2, dist);
01277 }
01278 
01279 template<>
01280 bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf,
01281                                                      const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, 
01282                                                      FCL_REAL* dist) const
01283 {
01284   return details::sphereTriangleDistance(s, tf, P1, P2, P3, dist);
01285 }
01286 
01287 template<> 
01288 bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf1, 
01289                                                      const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
01290                                                      FCL_REAL* dist) const
01291 {
01292   return details::sphereTriangleDistance(s, tf1, tf2.transform(P1), tf2.transform(P2), tf2.transform(P3), dist);
01293 }
01294 
01295 
01296 
01297 
01298 
01299 template<>
01300 bool GJKSolver_indep::shapeIntersect<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
01301                                                       const Sphere& s2, const Transform3f& tf2,
01302                                                       Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
01303 {
01304   return details::sphereSphereIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
01305 }
01306 
01307 template<> 
01308 bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf,
01309                                              const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
01310 {
01311   return details::sphereTriangleIntersect(s, tf, P1, P2, P3, contact_points, penetration_depth, normal);
01312 }
01313 
01314 template<> 
01315 bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf1,
01316                                              const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
01317 {
01318   return details::sphereTriangleIntersect(s, tf1, tf2.transform(P1), tf2.transform(P2), tf2.transform(P3), contact_points, penetration_depth, normal);
01319 }
01320 
01321 
01322 template<>
01323 bool GJKSolver_indep::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
01324                                                     const Sphere& s2, const Transform3f& tf2,
01325                                                     FCL_REAL* dist) const
01326 {
01327   return details::sphereSphereDistance(s1, tf1, s2, tf2, dist);
01328 }
01329 
01330 
01331 template<>
01332 bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf,
01333                                                     const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, 
01334                                                     FCL_REAL* dist) const
01335 {
01336   return details::sphereTriangleDistance(s, tf, P1, P2, P3, dist);
01337 }
01338 
01339 template<> 
01340 bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf1, 
01341                                                     const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
01342                                                     FCL_REAL* dist) const
01343 {
01344   return details::sphereTriangleDistance(s, tf1, tf2.transform(P1), tf2.transform(P2), tf2.transform(P3), dist);
01345 }
01346 
01347 
01348 template<>
01349 bool GJKSolver_libccd::shapeIntersect<Box, Box>(const Box& s1, const Transform3f& tf1,
01350                                                 const Box& s2, const Transform3f& tf2,
01351                                                 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
01352 {
01353   return details::boxBoxIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
01354 }
01355 
01356 template<>
01357 bool GJKSolver_indep::shapeIntersect<Box, Box>(const Box& s1, const Transform3f& tf1,
01358                                                 const Box& s2, const Transform3f& tf2,
01359                                                 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
01360 {
01361   return details::boxBoxIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
01362 }
01363 
01364 
01365 
01366 } // fcl