All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends

gjk_libccd.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 
00038 #include "fcl/narrowphase/gjk_libccd.h"
00039 #include <ccd/simplex.h>
00040 #include <ccd/vec3.h>
00041 
00042 namespace fcl
00043 {
00044 
00045 namespace details
00046 {
00047 
00048 struct ccd_obj_t
00049 {
00050   ccd_vec3_t pos;
00051   ccd_quat_t rot, rot_inv;
00052 };
00053 
00054 struct ccd_box_t : public ccd_obj_t
00055 {
00056   ccd_real_t dim[3];
00057 };
00058 
00059 struct ccd_cap_t : public ccd_obj_t
00060 {
00061   ccd_real_t radius, height;
00062 };
00063 
00064 struct ccd_cyl_t : public ccd_obj_t
00065 {
00066   ccd_real_t radius, height;
00067 };
00068 
00069 struct ccd_cone_t : public ccd_obj_t
00070 {
00071   ccd_real_t radius, height;
00072 };
00073 
00074 struct ccd_sphere_t : public ccd_obj_t
00075 {
00076   ccd_real_t radius;
00077 };
00078 
00079 struct ccd_convex_t : public ccd_obj_t
00080 {
00081   const Convex* convex;
00082 };
00083 
00084 struct ccd_triangle_t : public ccd_obj_t
00085 {
00086   ccd_vec3_t p[3];
00087   ccd_vec3_t c;
00088 };
00089 
00090 static void tripleCross(const ccd_vec3_t* a, const ccd_vec3_t* b, 
00091                         const ccd_vec3_t* c, ccd_vec3_t* d)
00092 {
00093   ccd_vec3_t e;
00094   ccdVec3Cross(&e, a, b);
00095   ccdVec3Cross(d, &e, c);
00096 }
00097 
00098 static int doSimplex2Dist(ccd_simplex_t *simplex, ccd_vec3_t *dir, ccd_real_t* dist)
00099 {
00100   const ccd_support_t *A, *B;
00101   ccd_vec3_t AB, AO, tmp;
00102   ccd_real_t dot;
00103 
00104   // get last added as A
00105   A = ccdSimplexLast(simplex);
00106   // get the other point
00107   B = ccdSimplexPoint(simplex, 0);
00108   // compute AB oriented segment
00109   ccdVec3Sub2(&AB, &B->v, &A->v);
00110   // compute AO vector
00111   ccdVec3Copy(&AO, &A->v);
00112   ccdVec3Scale(&AO, -CCD_ONE);
00113 
00114   // dot product AB . -AO
00115   dot = ccdVec3Dot(&AB, &AO);
00116 
00117   // check if origin doesn't lie on AB segment
00118   ccdVec3Cross(&tmp, &AB, &AO);
00119   if(ccdIsZero(ccdVec3Len2(&tmp)) && dot > CCD_ZERO)
00120   {
00121     return 1;
00122   }
00123 
00124   *dist = ccdVec3PointSegmentDist2(ccd_vec3_origin, &A->v, &B->v, NULL);
00125 
00126   // check if origin is in area where AB segment is
00127   if(ccdIsZero(dot) || dot < CCD_ZERO)
00128   {
00129     // origin is in outside are of A
00130     ccdSimplexSet(simplex, 0, A);
00131     ccdSimplexSetSize(simplex, 1);
00132     ccdVec3Copy(dir, &AO);
00133   }
00134   else
00135   {
00136     // origin is in area where AB segment is
00137 
00138     // keep simplex untouched and set direction to
00139     // AB x AO x AB
00140     tripleCross(&AB, &AO, &AB, dir);
00141   }
00142 
00143   return 0;
00144 }
00145 
00146 static int doSimplex3Dist(ccd_simplex_t *simplex, ccd_vec3_t *dir, ccd_real_t* dist)
00147 {
00148   const ccd_support_t *A, *B, *C;
00149   ccd_vec3_t AO, AB, AC, ABC, tmp;
00150   ccd_real_t dot;
00151 
00152   // get last added as A
00153   A = ccdSimplexLast(simplex);
00154   // get the other points
00155   B = ccdSimplexPoint(simplex, 1);
00156   C = ccdSimplexPoint(simplex, 0);
00157 
00158   // check touching contact
00159   *dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &C->v, NULL);
00160   if(ccdIsZero(*dist))
00161   {
00162     return 1;
00163   }
00164 
00165   // check if triangle is really triangle (has area > 0)
00166   // if not simplex can't be expanded and thus no itersection is found
00167   if(ccdVec3Eq(&A->v, &B->v) || ccdVec3Eq(&A->v, &C->v))
00168   {
00169     return -1;
00170   }
00171 
00172   // compute AO vector
00173   ccdVec3Copy(&AO, &A->v);
00174   ccdVec3Scale(&AO, -CCD_ONE);
00175 
00176   // compute AB and AC segments and ABC vector (perpendircular to triangle)
00177   ccdVec3Sub2(&AB, &B->v, &A->v);
00178   ccdVec3Sub2(&AC, &C->v, &A->v);
00179   ccdVec3Cross(&ABC, &AB, &AC);
00180 
00181   ccdVec3Cross(&tmp, &ABC, &AC);
00182   dot = ccdVec3Dot(&tmp, &AO);
00183   if(ccdIsZero(dot) || dot > CCD_ZERO)
00184   {
00185     dot = ccdVec3Dot(&AC, &AO);
00186     if(ccdIsZero(dot) || dot > CCD_ZERO)
00187     {
00188       // C is already in place
00189       ccdSimplexSet(simplex, 1, A);
00190       ccdSimplexSetSize(simplex, 2);
00191       tripleCross(&AC, &AO, &AC, dir);
00192     }
00193     else
00194     {
00195     ccd_do_simplex3_45:
00196       dot = ccdVec3Dot(&AB, &AO);
00197       if(ccdIsZero(dot) || dot > CCD_ZERO)
00198       {
00199         ccdSimplexSet(simplex, 0, B);
00200         ccdSimplexSet(simplex, 1, A);
00201         ccdSimplexSetSize(simplex, 2);
00202         tripleCross(&AB, &AO, &AB, dir);
00203       }
00204       else
00205       {
00206         ccdSimplexSet(simplex, 0, A);
00207         ccdSimplexSetSize(simplex, 1);
00208         ccdVec3Copy(dir, &AO);
00209       }
00210     }
00211   }
00212   else
00213   {
00214     ccdVec3Cross(&tmp, &AB, &ABC);
00215     dot = ccdVec3Dot(&tmp, &AO);
00216     if(ccdIsZero(dot) || dot > CCD_ZERO)
00217     {
00218       goto ccd_do_simplex3_45;
00219     }
00220     else
00221     {
00222       dot = ccdVec3Dot(&ABC, &AO);
00223       if (ccdIsZero(dot) || dot > CCD_ZERO)
00224       {
00225         ccdVec3Copy(dir, &ABC);
00226       }
00227       else
00228       {
00229         ccd_support_t Ctmp;
00230         ccdSupportCopy(&Ctmp, C);
00231         ccdSimplexSet(simplex, 0, B);
00232         ccdSimplexSet(simplex, 1, &Ctmp);
00233 
00234         ccdVec3Copy(dir, &ABC);
00235         ccdVec3Scale(dir, -CCD_ONE);
00236       }
00237     }
00238   }
00239 
00240 
00241   return 0;
00242 }
00243 
00244 static int doSimplex4Dist(ccd_simplex_t *simplex, ccd_vec3_t *dir, ccd_real_t* dist)
00245 {
00246   const ccd_support_t *A, *B, *C, *D;
00247   ccd_vec3_t AO, AB, AC, AD, ABC, ACD, ADB;
00248   int B_on_ACD, C_on_ADB, D_on_ABC;
00249   int AB_O, AC_O, AD_O;
00250 
00251   // get last added as A
00252   A = ccdSimplexLast(simplex);
00253   // get the other points
00254   B = ccdSimplexPoint(simplex, 2);
00255   C = ccdSimplexPoint(simplex, 1);
00256   D = ccdSimplexPoint(simplex, 0);
00257 
00258   // check if tetrahedron is really tetrahedron (has volume > 0)
00259   // if it is not simplex can't be expanded and thus no intersection is
00260   // found
00261   ccd_real_t volume = ccdVec3PointTriDist2(&A->v, &B->v, &C->v, &D->v, NULL);
00262   if(ccdIsZero(volume))
00263   {
00264     return -1;
00265   }
00266 
00267   // check if origin lies on some of tetrahedron's face - if so objects
00268   // intersect
00269   volume = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &C->v, NULL);
00270   if(ccdIsZero(volume))
00271     return 1;
00272   volume = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &C->v, &D->v, NULL);
00273   if(ccdIsZero(volume))
00274     return 1;
00275   volume = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &D->v, NULL);
00276   if(ccdIsZero(volume))
00277     return 1;
00278   volume = ccdVec3PointTriDist2(ccd_vec3_origin, &B->v, &C->v, &D->v, NULL);
00279   if(ccdIsZero(volume))
00280     return 1;
00281 
00282   // compute AO, AB, AC, AD segments and ABC, ACD, ADB normal vectors
00283   ccdVec3Copy(&AO, &A->v);
00284   ccdVec3Scale(&AO, -CCD_ONE);
00285   ccdVec3Sub2(&AB, &B->v, &A->v);
00286   ccdVec3Sub2(&AC, &C->v, &A->v);
00287   ccdVec3Sub2(&AD, &D->v, &A->v);
00288   ccdVec3Cross(&ABC, &AB, &AC);
00289   ccdVec3Cross(&ACD, &AC, &AD);
00290   ccdVec3Cross(&ADB, &AD, &AB);
00291 
00292   // side (positive or negative) of B, C, D relative to planes ACD, ADB
00293   // and ABC respectively
00294   B_on_ACD = ccdSign(ccdVec3Dot(&ACD, &AB));
00295   C_on_ADB = ccdSign(ccdVec3Dot(&ADB, &AC));
00296   D_on_ABC = ccdSign(ccdVec3Dot(&ABC, &AD));
00297 
00298   // whether origin is on same side of ACD, ADB, ABC as B, C, D
00299   // respectively
00300   AB_O = ccdSign(ccdVec3Dot(&ACD, &AO)) == B_on_ACD;
00301   AC_O = ccdSign(ccdVec3Dot(&ADB, &AO)) == C_on_ADB;
00302   AD_O = ccdSign(ccdVec3Dot(&ABC, &AO)) == D_on_ABC;
00303 
00304   if(AB_O && AC_O && AD_O)
00305   {
00306     // origin is in tetrahedron
00307     return 1;
00308 
00309     // rearrange simplex to triangle and call doSimplex3Dist()
00310   }
00311   else if(!AB_O)
00312   {
00313     // B is farthest from the origin among all of the tetrahedron's
00314     // points, so remove it from the list and go on with the triangle
00315     // case
00316 
00317     // D and C are in place
00318     ccdSimplexSet(simplex, 2, A);
00319     ccdSimplexSetSize(simplex, 3);
00320   }
00321   else if(!AC_O)
00322   {
00323     // C is farthest
00324     ccdSimplexSet(simplex, 1, D);
00325     ccdSimplexSet(simplex, 0, B);
00326     ccdSimplexSet(simplex, 2, A);
00327     ccdSimplexSetSize(simplex, 3);
00328   }
00329   else
00330   { 
00331     // (!AD_O)
00332     ccdSimplexSet(simplex, 0, C);
00333     ccdSimplexSet(simplex, 1, B);
00334     ccdSimplexSet(simplex, 2, A);
00335     ccdSimplexSetSize(simplex, 3);
00336   }
00337 
00338   return doSimplex3Dist(simplex, dir, dist);
00339 }
00340 
00341 static int doSimplexDist(ccd_simplex_t *simplex, ccd_vec3_t *dir, ccd_real_t* dist)
00342 {
00343   if(ccdSimplexSize(simplex) == 2)
00344   {
00345     // simplex contains segment only one segment
00346     return doSimplex2Dist(simplex, dir, dist);
00347   }
00348   else if(ccdSimplexSize(simplex) == 3)
00349   {
00350     // simplex contains triangle
00351     return doSimplex3Dist(simplex, dir, dist);
00352   }
00353   else
00354   { 
00355     // ccdSimplexSize(simplex) == 4
00356     // tetrahedron - this is the only shape which can encapsule origin
00357     // so doSimplex4() also contains test on it
00358     return doSimplex4Dist(simplex, dir, dist);
00359   }
00360 }
00361 
00362 
00363 static ccd_real_t __ccdGJKDist(const void *obj1, const void *obj2,
00364                                const ccd_t *ccd, ccd_simplex_t *simplex,
00365                                ccd_real_t tolerance)
00366 {
00367   unsigned long iterations;
00368   ccd_vec3_t dir; // direction vector
00369   ccd_support_t last; // last support point
00370   int do_simplex_res;
00371   ccd_real_t min_dist = -1;
00372 
00373   // initialize simplex struct
00374   ccdSimplexInit(simplex);
00375 
00376   // get first direction
00377   ccd->first_dir(obj1, obj2, &dir);
00378   // get first support point
00379   __ccdSupport(obj1, obj2, &dir, ccd, &last);
00380   // and add this point to simplex as last one
00381   ccdSimplexAdd(simplex, &last);
00382 
00383   // set up direction vector to as (O - last) which is exactly -last
00384   ccdVec3Copy(&dir, &last.v);
00385   ccdVec3Scale(&dir, -CCD_ONE);
00386 
00387   bool collision_free = false;
00388 
00389   // start iterations
00390   for(iterations = 0UL; iterations < ccd->max_iterations; ++iterations)
00391   {
00392     // obtain support point
00393     __ccdSupport(obj1, obj2, &dir, ccd, &last);
00394 
00395     // check if farthest point in Minkowski difference in direction dir
00396     // isn't somewhere before origin (the test on negative dot product)
00397     // - because if it is, objects are not intersecting at all.
00398     if(ccdVec3Dot(&last.v, &dir) < CCD_ZERO)
00399     {
00400       collision_free = true;
00401       ccd_real_t dist = ccdVec3Len2(&last.v);
00402       if(min_dist < 0) min_dist = dist;
00403       else min_dist = std::min(min_dist, dist);
00404     }
00405 
00406     // add last support vector to simplex
00407     ccdSimplexAdd(simplex, &last);
00408 
00409     // if doSimplex returns 1 if objects intersect, -1 if objects don't
00410     // intersect and 0 if algorithm should continue
00411     ccd_real_t dist;
00412     do_simplex_res = doSimplexDist(simplex, &dir, &dist);
00413 
00414     if((do_simplex_res == 1) && (!collision_free))
00415     {
00416       return -1; // intersection found
00417     }
00418     else if(do_simplex_res == -1) collision_free = true;
00419 
00420     if(ccdIsZero(ccdVec3Len2(&dir)))
00421       collision_free = true;
00422     
00423 
00424     if(min_dist > 0)
00425     {
00426       ccd_real_t old_min_dist = min_dist;
00427       min_dist = std::min(min_dist, dist);
00428 
00429       if((fabs(min_dist - old_min_dist) < tolerance) && (iterations > 0))
00430         break;
00431     }
00432     else min_dist = dist;
00433   }
00434 
00435   // intersection wasn't found
00436   return min_dist;
00437 }
00438 
00439 
00441 static void shapeToGJK(const ShapeBase& s, const Transform3f& tf, ccd_obj_t* o)
00442 {
00443   const Quaternion3f& q = tf.getQuatRotation();
00444   const Vec3f& T = tf.getTranslation();
00445   ccdVec3Set(&o->pos, T[0], T[1], T[2]);
00446   ccdQuatSet(&o->rot, q.getX(), q.getY(), q.getZ(), q.getW());
00447   ccdQuatInvert2(&o->rot_inv, &o->rot);
00448 }
00449 
00450 static void boxToGJK(const Box& s, const Transform3f& tf, ccd_box_t* box)
00451 {
00452   shapeToGJK(s, tf, box);
00453   box->dim[0] = s.side[0] / 2.0;
00454   box->dim[1] = s.side[1] / 2.0;
00455   box->dim[2] = s.side[2] / 2.0;
00456 }
00457 
00458 static void capToGJK(const Capsule& s, const Transform3f& tf, ccd_cap_t* cap)
00459 {
00460   shapeToGJK(s, tf, cap);
00461   cap->radius = s.radius;
00462   cap->height = s.lz / 2;
00463 }
00464 
00465 static void cylToGJK(const Cylinder& s, const Transform3f& tf, ccd_cyl_t* cyl)
00466 {
00467   shapeToGJK(s, tf, cyl);
00468   cyl->radius = s.radius;
00469   cyl->height = s.lz / 2;
00470 }
00471 
00472 static void coneToGJK(const Cone& s, const Transform3f& tf, ccd_cone_t* cone)
00473 {
00474   shapeToGJK(s, tf, cone);
00475   cone->radius = s.radius;
00476   cone->height = s.lz / 2;
00477 }
00478 
00479 static void sphereToGJK(const Sphere& s, const Transform3f& tf, ccd_sphere_t* sph)
00480 {
00481   shapeToGJK(s, tf, sph);
00482   sph->radius = s.radius;
00483 }
00484 
00485 static void convexToGJK(const Convex& s, const Transform3f& tf, ccd_convex_t* conv)
00486 {
00487   shapeToGJK(s, tf, conv);
00488   conv->convex = &s;
00489 }
00490 
00492 static void supportBox(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v)
00493 {
00494   const ccd_box_t* o = static_cast<const ccd_box_t*>(obj);
00495   ccd_vec3_t dir;
00496   ccdVec3Copy(&dir, dir_);
00497   ccdQuatRotVec(&dir, &o->rot_inv);
00498   ccdVec3Set(v, ccdSign(ccdVec3X(&dir)) * o->dim[0],
00499                 ccdSign(ccdVec3Y(&dir)) * o->dim[1],
00500                 ccdSign(ccdVec3Z(&dir)) * o->dim[2]);
00501   ccdQuatRotVec(v, &o->rot);
00502   ccdVec3Add(v, &o->pos);
00503 }
00504 
00505 static void supportCap(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v)
00506 {
00507   const ccd_cap_t* o = static_cast<const ccd_cap_t*>(obj);
00508   ccd_vec3_t dir, pos1, pos2;
00509 
00510   ccdVec3Copy(&dir, dir_);
00511   ccdQuatRotVec(&dir, &o->rot_inv);
00512 
00513   ccdVec3Set(&pos1, CCD_ZERO, CCD_ZERO, o->height);
00514   ccdVec3Set(&pos2, CCD_ZERO, CCD_ZERO, -o->height);
00515 
00516   ccdVec3Copy(v, &dir);
00517   ccdVec3Scale(v, o->radius);
00518   ccdVec3Add(&pos1, v);
00519   ccdVec3Add(&pos2, v);
00520 
00521   if(ccdVec3Dot(&dir, &pos1) > ccdVec3Dot(&dir, &pos2))
00522     ccdVec3Copy(v, &pos1);
00523   else
00524     ccdVec3Copy(v, &pos2);
00525 
00526   // transform support vertex
00527   ccdQuatRotVec(v, &o->rot);
00528   ccdVec3Add(v, &o->pos);
00529 }
00530 
00531 static void supportCyl(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v)
00532 {
00533   const ccd_cyl_t* cyl = static_cast<const ccd_cyl_t*>(obj);
00534   ccd_vec3_t dir;
00535   double zdist, rad;
00536 
00537   ccdVec3Copy(&dir, dir_);
00538   ccdQuatRotVec(&dir, &cyl->rot_inv);
00539 
00540   zdist = dir.v[0] * dir.v[0] + dir.v[1] * dir.v[1];
00541   zdist = sqrt(zdist);
00542   if(ccdIsZero(zdist))
00543     ccdVec3Set(v, 0., 0., ccdSign(ccdVec3Z(&dir)) * cyl->height);
00544   else
00545   {
00546     rad = cyl->radius / zdist;
00547 
00548     ccdVec3Set(v, rad * ccdVec3X(&dir),
00549                   rad * ccdVec3Y(&dir),
00550                   ccdSign(ccdVec3Z(&dir)) * cyl->height);
00551   }
00552 
00553   // transform support vertex
00554   ccdQuatRotVec(v, &cyl->rot);
00555   ccdVec3Add(v, &cyl->pos);
00556 }
00557 
00558 static void supportCone(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v)
00559 {
00560   const ccd_cone_t* cone = static_cast<const ccd_cone_t*>(obj);
00561   ccd_vec3_t dir;
00562 
00563   ccdVec3Copy(&dir, dir_);
00564   ccdQuatRotVec(&dir, &cone->rot_inv);
00565 
00566   double zdist, len, rad;
00567   zdist = dir.v[0] * dir.v[0] + dir.v[1] * dir.v[1];
00568   len = zdist + dir.v[2] * dir.v[2];
00569   zdist = sqrt(zdist);
00570   len = sqrt(len);
00571 
00572   double sin_a = cone->radius / sqrt(cone->radius * cone->radius + 4 * cone->height * cone->height);
00573 
00574   if(dir.v[2] > len * sin_a)
00575     ccdVec3Set(v, 0., 0., cone->height);
00576   else if(zdist > 0)
00577   {
00578     rad = cone->radius / zdist;
00579     ccdVec3Set(v, rad * ccdVec3X(&dir), rad * ccdVec3Y(&dir), -cone->height);
00580   }
00581   else
00582     ccdVec3Set(v, 0, 0, -cone->height);
00583 
00584   // transform support vertex
00585   ccdQuatRotVec(v, &cone->rot);
00586   ccdVec3Add(v, &cone->pos);
00587 }
00588 
00589 static void supportSphere(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v)
00590 {
00591   const ccd_sphere_t* s = static_cast<const ccd_sphere_t*>(obj);
00592   ccd_vec3_t dir;
00593 
00594   ccdVec3Copy(&dir, dir_);
00595   ccdQuatRotVec(&dir, &s->rot_inv);
00596 
00597   ccdVec3Copy(v, &dir);
00598   ccdVec3Scale(v, s->radius);
00599   ccdVec3Scale(v, CCD_ONE / CCD_SQRT(ccdVec3Len2(&dir)));
00600 
00601   // transform support vertex
00602   ccdQuatRotVec(v, &s->rot);
00603   ccdVec3Add(v, &s->pos);
00604 }
00605 
00606 static void supportConvex(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v)
00607 {
00608   const ccd_convex_t* c = (const ccd_convex_t*)obj;
00609   ccd_vec3_t dir, p;
00610   ccd_real_t maxdot, dot;
00611   int i;
00612   Vec3f* curp;
00613   const Vec3f& center = c->convex->center;
00614 
00615   ccdVec3Copy(&dir, dir_);
00616   ccdQuatRotVec(&dir, &c->rot_inv);
00617 
00618   maxdot = -CCD_REAL_MAX;
00619   curp = c->convex->points;
00620 
00621   for(i = 0; i < c->convex->num_points; ++i, curp += 1)
00622   {
00623     ccdVec3Set(&p, (*curp)[0] - center[0], (*curp)[1] - center[1], (*curp)[2] - center[2]);
00624     dot = ccdVec3Dot(&dir, &p);
00625     if(dot > maxdot)
00626     {
00627       ccdVec3Set(v, (*curp)[0], (*curp)[1], (*curp)[2]);
00628       maxdot = dot;
00629     }
00630   }
00631 
00632   // transform support vertex
00633   ccdQuatRotVec(v, &c->rot);
00634   ccdVec3Add(v, &c->pos);
00635 }
00636 
00637 static void supportTriangle(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v)
00638 {
00639   const ccd_triangle_t* tri = static_cast<const ccd_triangle_t*>(obj);
00640   ccd_vec3_t dir, p;
00641   ccd_real_t maxdot, dot;
00642   int i;
00643 
00644   ccdVec3Copy(&dir, dir_);
00645   ccdQuatRotVec(&dir, &tri->rot_inv);
00646 
00647   maxdot = -CCD_REAL_MAX;
00648 
00649   for(i = 0; i < 3; ++i)
00650   {
00651     ccdVec3Set(&p, tri->p[i].v[0] - tri->c.v[0], tri->p[i].v[1] - tri->c.v[1], tri->p[i].v[2] - tri->c.v[2]);
00652     dot = ccdVec3Dot(&dir, &p);
00653     if(dot > maxdot)
00654     {
00655       ccdVec3Copy(v, &tri->p[i]);
00656       maxdot = dot;
00657     }
00658   }
00659 
00660   // transform support vertex
00661   ccdQuatRotVec(v, &tri->rot);
00662   ccdVec3Add(v, &tri->pos);
00663 }
00664 
00665 static void centerShape(const void* obj, ccd_vec3_t* c)
00666 {
00667   const ccd_obj_t *o = static_cast<const ccd_obj_t*>(obj);
00668     ccdVec3Copy(c, &o->pos);
00669 }
00670 
00671 static void centerConvex(const void* obj, ccd_vec3_t* c)
00672 {
00673   const ccd_convex_t *o = static_cast<const ccd_convex_t*>(obj);
00674   ccdVec3Set(c, o->convex->center[0], o->convex->center[1], o->convex->center[2]);
00675   ccdQuatRotVec(c, &o->rot);
00676   ccdVec3Add(c, &o->pos);
00677 }
00678 
00679 static void centerTriangle(const void* obj, ccd_vec3_t* c)
00680 {
00681   const ccd_triangle_t *o = static_cast<const ccd_triangle_t*>(obj);
00682     ccdVec3Copy(c, &o->c);
00683     ccdQuatRotVec(c, &o->rot);
00684     ccdVec3Add(c, &o->pos);
00685 }
00686 
00687 bool GJKCollide(void* obj1, ccd_support_fn supp1, ccd_center_fn cen1,
00688                 void* obj2, ccd_support_fn supp2, ccd_center_fn cen2,
00689                 unsigned int max_iterations, FCL_REAL tolerance,
00690                 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal)
00691 {
00692   ccd_t ccd;
00693   int res;
00694   ccd_real_t depth;
00695   ccd_vec3_t dir, pos;
00696 
00697 
00698   CCD_INIT(&ccd);
00699   ccd.support1 = supp1;
00700   ccd.support2 = supp2;
00701   ccd.center1 = cen1;
00702   ccd.center2 = cen2;
00703   ccd.max_iterations = max_iterations;
00704   ccd.mpr_tolerance = tolerance;
00705 
00706   if(!contact_points)
00707   {
00708     return ccdMPRIntersect(obj1, obj2, &ccd);
00709   }
00710 
00711 
00712   res = ccdMPRPenetration(obj1, obj2, &ccd, &depth, &dir, &pos);
00713   if(res == 0)
00714   {
00715     contact_points->setValue(ccdVec3X(&pos), ccdVec3Y(&pos), ccdVec3Z(&pos));
00716     *penetration_depth = depth;
00717     normal->setValue(-ccdVec3X(&dir), -ccdVec3Y(&dir), -ccdVec3Z(&dir));
00718 
00719     return true;
00720   }
00721 
00722   return false;
00723 }
00724 
00725 /*
00726 bool GJKDistance(void* obj1, ccd_support_fn supp1,
00727                  void* obj2, ccd_support_fn supp2,
00728                  unsigned int max_iterations, FCL_REAL tolerance_,
00729                  FCL_REAL* res)
00730 {
00731   ccd_t ccd;
00732   ccd_real_t dist;
00733   CCD_INIT(&ccd);
00734   ccd.support1 = supp1;
00735   ccd.support2 = supp2;
00736   
00737   ccd.max_iterations = max_iterations;
00738   ccd_real_t tolerance = tolerance_;
00739   
00740 
00741   ccd_simplex_t simplex;
00742   dist = __ccdGJKDist(obj1, obj2, &ccd, &simplex, tolerance);
00743   
00744   if(dist > 0)
00745   {
00746     *res = std::sqrt(dist);
00747     return true;
00748   }
00749   else
00750     return false;
00751 }
00752 */
00753 
00754 bool GJKDistance(void* obj1, ccd_support_fn supp1,
00755                  void* obj2, ccd_support_fn supp2,
00756                  unsigned int max_iterations, FCL_REAL tolerance,
00757                  FCL_REAL* res)
00758 {
00759   ccd_t ccd;
00760   ccd_real_t dist;
00761   CCD_INIT(&ccd);
00762   ccd.support1 = supp1;
00763   ccd.support2 = supp2;
00764   
00765   ccd.max_iterations = max_iterations;
00766   ccd.dist_tolerance = tolerance;
00767 
00768   dist = ccdGJKDist(obj1, obj2, &ccd);
00769   *res = dist;
00770   if(dist < 0) return false;
00771   else return true;
00772 }
00773 
00774 GJKSupportFunction GJKInitializer<Cylinder>::getSupportFunction()
00775 {
00776   return &supportCyl;
00777 }
00778 
00779 
00780 GJKCenterFunction GJKInitializer<Cylinder>::getCenterFunction()
00781 {
00782   return &centerShape;
00783 }
00784 
00785 
00786 void* GJKInitializer<Cylinder>::createGJKObject(const Cylinder& s, const Transform3f& tf)
00787 {
00788   ccd_cyl_t* o = new ccd_cyl_t;
00789   cylToGJK(s, tf, o);
00790   return o;
00791 }
00792 
00793 
00794 void GJKInitializer<Cylinder>::deleteGJKObject(void* o_)
00795 {
00796   ccd_cyl_t* o = static_cast<ccd_cyl_t*>(o_);
00797   delete o;
00798 }
00799 
00800 
00801 GJKSupportFunction GJKInitializer<Sphere>::getSupportFunction()
00802 {
00803   return &supportSphere;
00804 }
00805 
00806 
00807 GJKCenterFunction GJKInitializer<Sphere>::getCenterFunction()
00808 {
00809   return &centerShape;
00810 }
00811 
00812 
00813 void* GJKInitializer<Sphere>::createGJKObject(const Sphere& s, const Transform3f& tf)
00814 {
00815   ccd_sphere_t* o = new ccd_sphere_t;
00816   sphereToGJK(s, tf, o);
00817   return o;
00818 }
00819 
00820 void GJKInitializer<Sphere>::deleteGJKObject(void* o_)
00821 {
00822   ccd_sphere_t* o = static_cast<ccd_sphere_t*>(o_);
00823   delete o;
00824 }
00825 
00826 GJKSupportFunction GJKInitializer<Box>::getSupportFunction()
00827 {
00828   return &supportBox;
00829 }
00830 
00831 
00832 GJKCenterFunction GJKInitializer<Box>::getCenterFunction()
00833 {
00834   return &centerShape;
00835 }
00836 
00837 
00838 void* GJKInitializer<Box>::createGJKObject(const Box& s, const Transform3f& tf)
00839 {
00840   ccd_box_t* o = new ccd_box_t;
00841   boxToGJK(s, tf, o);
00842   return o;
00843 }
00844 
00845 
00846 void GJKInitializer<Box>::deleteGJKObject(void* o_)
00847 {
00848   ccd_box_t* o = static_cast<ccd_box_t*>(o_);
00849   delete o;
00850 }
00851 
00852 
00853 GJKSupportFunction GJKInitializer<Capsule>::getSupportFunction()
00854 {
00855   return &supportCap;
00856 }
00857 
00858 
00859 GJKCenterFunction GJKInitializer<Capsule>::getCenterFunction()
00860 {
00861   return &centerShape;
00862 }
00863 
00864 
00865 void* GJKInitializer<Capsule>::createGJKObject(const Capsule& s, const Transform3f& tf)
00866 {
00867   ccd_cap_t* o = new ccd_cap_t;
00868   capToGJK(s, tf, o);
00869   return o;
00870 }
00871 
00872 
00873 void GJKInitializer<Capsule>::deleteGJKObject(void* o_)
00874 {
00875   ccd_cap_t* o = static_cast<ccd_cap_t*>(o_);
00876   delete o;
00877 }
00878 
00879 
00880 GJKSupportFunction GJKInitializer<Cone>::getSupportFunction()
00881 {
00882   return &supportCone;
00883 }
00884 
00885 
00886 GJKCenterFunction GJKInitializer<Cone>::getCenterFunction()
00887 {
00888   return &centerShape;
00889 }
00890 
00891 
00892 void* GJKInitializer<Cone>::createGJKObject(const Cone& s, const Transform3f& tf)
00893 {
00894   ccd_cone_t* o = new ccd_cone_t;
00895   coneToGJK(s, tf, o);
00896   return o;
00897 }
00898 
00899 
00900 void GJKInitializer<Cone>::deleteGJKObject(void* o_)
00901 {
00902   ccd_cone_t* o = static_cast<ccd_cone_t*>(o_);
00903   delete o;
00904 }
00905 
00906 
00907 GJKSupportFunction GJKInitializer<Convex>::getSupportFunction()
00908 {
00909   return &supportConvex;
00910 }
00911 
00912 
00913 GJKCenterFunction GJKInitializer<Convex>::getCenterFunction()
00914 {
00915   return &centerConvex;
00916 }
00917 
00918 
00919 void* GJKInitializer<Convex>::createGJKObject(const Convex& s, const Transform3f& tf)
00920 {
00921   ccd_convex_t* o = new ccd_convex_t;
00922   convexToGJK(s, tf, o);
00923   return o;
00924 }
00925 
00926 
00927 void GJKInitializer<Convex>::deleteGJKObject(void* o_)
00928 {
00929   ccd_convex_t* o = static_cast<ccd_convex_t*>(o_);
00930   delete o;
00931 }
00932 
00933 
00934 GJKSupportFunction triGetSupportFunction()
00935 {
00936   return &supportTriangle;
00937 }
00938 
00939 
00940 GJKCenterFunction triGetCenterFunction()
00941 {
00942   return &centerTriangle;
00943 }
00944 
00945 
00946 void* triCreateGJKObject(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3)
00947 {
00948   ccd_triangle_t* o = new ccd_triangle_t;
00949   Vec3f center((P1[0] + P2[0] + P3[0]) / 3, (P1[1] + P2[1] + P3[1]) / 3, (P1[2] + P2[2] + P3[2]) / 3);
00950 
00951   ccdVec3Set(&o->p[0], P1[0], P1[1], P1[2]);
00952   ccdVec3Set(&o->p[1], P2[0], P2[1], P2[2]);
00953   ccdVec3Set(&o->p[2], P3[0], P3[1], P3[2]);
00954   ccdVec3Set(&o->c, center[0], center[1], center[2]);
00955   ccdVec3Set(&o->pos, 0., 0., 0.);
00956   ccdQuatSet(&o->rot, 0., 0., 0., 1.);
00957   ccdQuatInvert2(&o->rot_inv, &o->rot);
00958 
00959   return o;
00960 }
00961 
00962 void* triCreateGJKObject(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf)
00963 {
00964   ccd_triangle_t* o = new ccd_triangle_t;
00965   Vec3f center((P1[0] + P2[0] + P3[0]) / 3, (P1[1] + P2[1] + P3[1]) / 3, (P1[2] + P2[2] + P3[2]) / 3);
00966 
00967   ccdVec3Set(&o->p[0], P1[0], P1[1], P1[2]);
00968   ccdVec3Set(&o->p[1], P2[0], P2[1], P2[2]);
00969   ccdVec3Set(&o->p[2], P3[0], P3[1], P3[2]);
00970   ccdVec3Set(&o->c, center[0], center[1], center[2]);
00971   const Quaternion3f& q = tf.getQuatRotation();
00972   const Vec3f& T = tf.getTranslation();
00973   ccdVec3Set(&o->pos, T[0], T[1], T[2]);
00974   ccdQuatSet(&o->rot, q.getX(), q.getY(), q.getZ(), q.getW());
00975   ccdQuatInvert2(&o->rot_inv, &o->rot);
00976 
00977   return o;
00978 }
00979 
00980 void triDeleteGJKObject(void* o_)
00981 {
00982   ccd_triangle_t* o = static_cast<ccd_triangle_t*>(o_);
00983   delete o;
00984 }
00985 
00986 } // details
00987 
00988 } // fcl