gjk.cpp
Go to the documentation of this file.
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/gjk.h"
00038 
00039 namespace fcl
00040 {
00041 
00042 namespace details
00043 {
00044 
00045 Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir)
00046 {
00047   switch(shape->getNodeType())
00048   {
00049   case GEOM_TRIANGLE:
00050     {
00051       const Triangle2* triangle = static_cast<const Triangle2*>(shape);
00052       FCL_REAL dota = dir.dot(triangle->a);
00053       FCL_REAL dotb = dir.dot(triangle->b);
00054       FCL_REAL dotc = dir.dot(triangle->c);
00055       if(dota > dotb)
00056       {
00057         if(dotc > dota)
00058           return triangle->c;
00059         else
00060           return triangle->a;
00061       }
00062       else
00063       {
00064         if(dotc > dotb)
00065           return triangle->c;
00066         else
00067           return triangle->b;
00068       }
00069     }
00070     break;
00071   case GEOM_BOX:
00072     {
00073       const Box* box = static_cast<const Box*>(shape);
00074       return Vec3f((dir[0]>0)?(box->side[0]/2):(-box->side[0]/2),
00075                    (dir[1]>0)?(box->side[1]/2):(-box->side[1]/2),
00076                    (dir[2]>0)?(box->side[2]/2):(-box->side[2]/2));
00077     }
00078     break;
00079   case GEOM_SPHERE:
00080     {
00081       const Sphere* sphere = static_cast<const Sphere*>(shape);
00082       return dir * sphere->radius;
00083     }
00084     break;
00085   case GEOM_CAPSULE:
00086     {
00087       const Capsule* capsule = static_cast<const Capsule*>(shape);
00088       FCL_REAL half_h = capsule->lz * 0.5;
00089       Vec3f pos1(0, 0, half_h);
00090       Vec3f pos2(0, 0, -half_h);
00091       Vec3f v = dir * capsule->radius;
00092       pos1 += v;
00093       pos2 += v;
00094       if(dir.dot(pos1) > dir.dot(pos2))
00095         return pos1;
00096       else return pos2;
00097     }
00098     break;
00099   case GEOM_CONE:
00100     {
00101       const Cone* cone = static_cast<const Cone*>(shape);
00102       FCL_REAL zdist = dir[0] * dir[0] + dir[1] * dir[1];
00103       FCL_REAL len = zdist + dir[2] * dir[2];
00104       zdist = std::sqrt(zdist);
00105       len = std::sqrt(len);
00106       FCL_REAL half_h = cone->lz * 0.5;
00107       FCL_REAL radius = cone->radius;
00108 
00109       FCL_REAL sin_a = radius / std::sqrt(radius * radius + 4 * half_h * half_h);
00110 
00111       if(dir[2] > len * sin_a)
00112         return Vec3f(0, 0, half_h);
00113       else if(zdist > 0)
00114       {
00115         FCL_REAL rad = radius / zdist;
00116         return Vec3f(rad * dir[0], rad * dir[1], -half_h);
00117       }
00118       else
00119         return Vec3f(0, 0, -half_h);
00120     }
00121     break;
00122   case GEOM_CYLINDER:
00123     {
00124       const Cylinder* cylinder = static_cast<const Cylinder*>(shape);
00125       FCL_REAL zdist = std::sqrt(dir[0] * dir[0] + dir[1] * dir[1]);
00126       FCL_REAL half_h = cylinder->lz * 0.5;
00127       if(zdist == 0.0)
00128       {
00129         return Vec3f(0, 0, (dir[2]>0)? half_h:-half_h);
00130       }
00131       else
00132       {
00133         FCL_REAL d = cylinder->radius / zdist;
00134         return Vec3f(d * dir[0], d * dir[1], (dir[2]>0)?half_h:-half_h);
00135       }
00136     }
00137     break;
00138   case GEOM_CONVEX:
00139     {
00140       const Convex* convex = static_cast<const Convex*>(shape);
00141       FCL_REAL maxdot = - std::numeric_limits<FCL_REAL>::max();
00142       Vec3f* curp = convex->points;
00143       Vec3f bestv;
00144       for(size_t i = 0; i < convex->num_points; ++i, curp+=1)
00145       {
00146         FCL_REAL dot = dir.dot(*curp);
00147         if(dot > maxdot)
00148         {
00149           bestv = *curp;
00150           maxdot = dot;
00151         }
00152       }
00153       return bestv;
00154     }
00155     break;
00156   case GEOM_PLANE:
00157     {
00158       return Vec3f(0, 0, 0);
00159     }
00160     break;
00161   default:
00162     ; // nothing
00163   }
00164 
00165   return Vec3f(0, 0, 0);
00166 }
00167 
00168 
00169 FCL_REAL projectOrigin(const Vec3f& a, const Vec3f& b, FCL_REAL* w, size_t& m)
00170 {
00171   const Vec3f d = b - a;
00172   const FCL_REAL l = d.sqrLength();
00173   if(l > 0)
00174   {
00175     const FCL_REAL t(l > 0 ? - a.dot(d) / l : 0);
00176     if(t >= 1) { w[0] = 0; w[1] = 1; m = 2; return b.sqrLength(); } // m = 0x10 
00177     else if(t <= 0) { w[0] = 1; w[1] = 0; m = 1; return a.sqrLength(); } // m = 0x01
00178     else { w[0] = 1 - (w[1] = t); m = 3; return (a + d * t).sqrLength(); } // m = 0x11
00179   }
00180 
00181   return -1;
00182 }
00183 
00184 FCL_REAL projectOrigin(const Vec3f& a, const Vec3f& b, const Vec3f& c, FCL_REAL* w, size_t& m)
00185 {
00186   static const size_t nexti[3] = {1, 2, 0};
00187   const Vec3f* vt[] = {&a, &b, &c};
00188   const Vec3f dl[] = {a - b, b - c, c - a};
00189   const Vec3f& n = dl[0].cross(dl[1]);
00190   const FCL_REAL l = n.sqrLength();
00191 
00192   if(l > 0)
00193   {
00194     FCL_REAL mindist = -1;
00195     FCL_REAL subw[2] = {0, 0};
00196     size_t subm = 0;
00197     for(size_t i = 0; i < 3; ++i)
00198     {
00199       if(vt[i]->dot(dl[i].cross(n)) > 0) // origin is to the outside part of the triangle edge, then the optimal can only be on the edge
00200       {
00201         size_t j = nexti[i];
00202         FCL_REAL subd = projectOrigin(*vt[i], *vt[j], subw, subm);
00203         if(mindist < 0 || subd < mindist)
00204         {
00205           mindist = subd;
00206           m = static_cast<size_t>(((subm&1)?1<<i:0) + ((subm&2)?1<<j:0));
00207           w[i] = subw[0];
00208           w[j] = subw[1];
00209           w[nexti[j]] = 0;
00210         }
00211       }
00212     }
00213     
00214     if(mindist < 0) // the origin project is within the triangle
00215     {
00216       FCL_REAL d = a.dot(n);
00217       FCL_REAL s = sqrt(l);
00218       Vec3f p = n * (d / l);
00219       mindist = p.sqrLength();
00220       m = 7; // m = 0x111
00221       w[0] = dl[1].cross(b-p).length() / s;
00222       w[1] = dl[2].cross(c-p).length() / s;
00223       w[2] = 1 - (w[0] + w[1]);
00224     }
00225 
00226     return mindist;
00227   }
00228   return -1;
00229 }
00230 
00231 FCL_REAL projectOrigin(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& d, FCL_REAL* w, size_t& m)
00232 {
00233   static const size_t nexti[] = {1, 2, 0};
00234   const Vec3f* vt[] = {&a, &b, &c, &d};
00235   const Vec3f dl[3] = {a-d, b-d, c-d};
00236   FCL_REAL vl = triple(dl[0], dl[1], dl[2]); 
00237   bool ng = (vl * a.dot((b-c).cross(a-b))) <= 0;
00238   if(ng && std::abs(vl) > 0) // abs(vl) == 0, the tetrahedron is degenerated; if ng is false, then the last vertex in the tetrahedron does not grow toward the origin (in fact origin is on the other side of the abc face)
00239   {
00240     FCL_REAL mindist = -1;
00241     FCL_REAL subw[3] = {0, 0, 0};
00242     size_t subm = 0;
00243     for(size_t i = 0; i < 3; ++i)
00244     {
00245       size_t j = nexti[i];
00246       FCL_REAL s = vl * d.dot(dl[i].cross(dl[j]));
00247       if(s > 0) // the origin is to the outside part of a triangle face, then the optimal can only be on the triangle face
00248       {
00249         FCL_REAL subd = projectOrigin(*vt[i], *vt[j], d, subw, subm);
00250         if(mindist < 0 || subd < mindist)
00251         {
00252           mindist = subd;
00253           m = static_cast<size_t>( (subm&1?1<<i:0) + (subm&2?1<<j:0) + (subm&4?8:0) );
00254           w[i] = subw[0];
00255           w[j] = subw[1];
00256           w[nexti[j]] = 0;
00257           w[3] = subw[2];
00258         }
00259       }
00260     }
00261 
00262     if(mindist < 0)
00263     {
00264       mindist = 0;
00265       m = 15;
00266       w[0] = triple(c, b, d) / vl;
00267       w[1] = triple(a, c, d) / vl;
00268       w[2] = triple(b, a, d) / vl;
00269       w[3] = 1 - (w[0] + w[1] + w[2]);
00270     }
00271     
00272     return mindist;
00273   }
00274   return -1;
00275 }
00276 
00277 
00278 void GJK::initialize()
00279 {
00280   ray = Vec3f();
00281   nfree = 0;
00282   status = Failed;
00283   current = 0;
00284   distance = 0.0;
00285 }
00286 
00287 GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
00288 {
00289   size_t iterations = 0;
00290   FCL_REAL sqdist = 0;
00291   FCL_REAL alpha = 0;
00292   Vec3f lastw[4];
00293   size_t clastw = 0;
00294     
00295   free_v[0] = &store_v[0];
00296   free_v[1] = &store_v[1];
00297   free_v[2] = &store_v[2];
00298   free_v[3] = &store_v[3];
00299     
00300   nfree = 4;
00301   current = 0;
00302   status = Valid;
00303   shape = shape_;
00304   distance = 0.0;
00305   simplices[0].rank = 0;
00306   ray = guess;
00307     
00308   FCL_REAL sqrl = ray.sqrLength();
00309   appendVertex(simplices[0], (sqrl>0) ? -ray : Vec3f(1, 0, 0));
00310   simplices[0].p[0] = 1;
00311   ray = simplices[0].c[0]->w;
00312   sqdist = sqrl;
00313   lastw[0] = lastw[1] = lastw[2] = lastw[3] = ray;
00314 
00315   do
00316   {
00317     size_t next = 1 - current;
00318     Simplex& curr_simplex = simplices[current];
00319     Simplex& next_simplex = simplices[next];
00320       
00321     FCL_REAL rl = ray.sqrLength();
00322     if(rl < tolerance) // means origin is near the face of original simplex, return touch
00323     {
00324       status = Inside;
00325       break;
00326     }
00327 
00328     appendVertex(curr_simplex, -ray); // see below, ray points away from origin
00329       
00330     Vec3f& w = curr_simplex.c[curr_simplex.rank - 1]->w;
00331     bool found = false;
00332     for(size_t i = 0; i < 4; ++i)
00333     {
00334       if((w - lastw[i]).sqrLength() < tolerance)
00335       {
00336         found = true; break;
00337       }
00338     }
00339 
00340     if(found)
00341     {
00342       removeVertex(simplices[current]);
00343       break; 
00344     }
00345     else
00346     {
00347       lastw[clastw = (clastw+1)&3] = w;
00348     }
00349 
00350     // check for termination, from bullet
00351     FCL_REAL omega = ray.dot(w) / rl;
00352     alpha = std::max(alpha, omega);
00353     if((rl - alpha) - tolerance * rl <= 0)
00354     {
00355       removeVertex(simplices[current]);
00356       break;
00357     }
00358 
00359     // reduce simplex and decide the extend direction
00360     FCL_REAL weights[4];
00361     size_t mask = 0; // decide the simplex vertices that compose the minimal distance
00362     switch(curr_simplex.rank)
00363     {
00364     case 2:
00365       sqdist = projectOrigin(curr_simplex.c[0]->w, curr_simplex.c[1]->w, weights, mask); break;
00366     case 3:
00367       sqdist = projectOrigin(curr_simplex.c[0]->w, curr_simplex.c[1]->w, curr_simplex.c[2]->w, weights, mask); break;
00368     case 4:
00369       sqdist = projectOrigin(curr_simplex.c[0]->w, curr_simplex.c[1]->w, curr_simplex.c[2]->w, curr_simplex.c[3]->w, weights, mask); break;
00370     }
00371       
00372     if(sqdist >= 0)
00373     {
00374       next_simplex.rank = 0;
00375       ray = Vec3f();
00376       current = next;
00377       for(size_t i = 0; i < curr_simplex.rank; ++i)
00378       {
00379         if(mask & (1 << i))
00380         {
00381           next_simplex.c[next_simplex.rank] = curr_simplex.c[i];
00382           next_simplex.p[next_simplex.rank++] = weights[i];
00383           ray += curr_simplex.c[i]->w * weights[i];
00384         }
00385         else
00386           free_v[nfree++] = curr_simplex.c[i];
00387       }
00388       if(mask == 15) status = Inside; // the origin is within the 4-simplex, collision
00389     }
00390     else
00391     {
00392       removeVertex(simplices[current]);
00393       break;
00394     }
00395 
00396     status = ((++iterations) < max_iterations) ? status : Failed;
00397       
00398   } while(status == Valid);
00399 
00400   simplex = &simplices[current];
00401   switch(status)
00402   {
00403   case Valid: distance = ray.length(); break;
00404   case Inside: distance = 0; break;
00405   }
00406   return status;
00407 }
00408 
00409 void GJK::getSupport(const Vec3f& d, SimplexV& sv) const
00410 {
00411   sv.d = normalize(d);
00412   sv.w = shape.support(sv.d);
00413 }
00414 
00415 void GJK::removeVertex(Simplex& simplex)
00416 {
00417   free_v[nfree++] = simplex.c[--simplex.rank];
00418 }
00419 
00420 void GJK::appendVertex(Simplex& simplex, const Vec3f& v)
00421 {
00422   simplex.p[simplex.rank] = 0; // initial weight 0
00423   simplex.c[simplex.rank] = free_v[--nfree]; // set the memory
00424   getSupport(v, *simplex.c[simplex.rank++]);
00425 }
00426 
00427 bool GJK::encloseOrigin()
00428 {
00429   switch(simplex->rank)
00430   {
00431   case 1:
00432     {
00433       for(size_t i = 0; i < 3; ++i)
00434       {
00435         Vec3f axis;
00436         axis[i] = 1;
00437         appendVertex(*simplex, axis);
00438         if(encloseOrigin()) return true;
00439         removeVertex(*simplex);
00440         appendVertex(*simplex, -axis);
00441         if(encloseOrigin()) return true;
00442         removeVertex(*simplex);
00443       }
00444     }
00445     break;
00446   case 2:
00447     {
00448       Vec3f d = simplex->c[1]->w - simplex->c[0]->w;
00449       for(size_t i = 0; i < 3; ++i)
00450       {
00451         Vec3f axis;
00452         axis[i] = 1;
00453         Vec3f p = d.cross(axis);
00454         if(p.sqrLength() > 0)
00455         {
00456           appendVertex(*simplex, p);
00457           if(encloseOrigin()) return true;
00458           removeVertex(*simplex);
00459           appendVertex(*simplex, -p);
00460           if(encloseOrigin()) return true;
00461           removeVertex(*simplex);
00462         }
00463       }
00464     }
00465     break;
00466   case 3:
00467     {
00468       Vec3f n = (simplex->c[1]->w - simplex->c[0]->w).cross(simplex->c[2]->w - simplex->c[0]->w);
00469       if(n.sqrLength() > 0)
00470       {
00471         appendVertex(*simplex, n);
00472         if(encloseOrigin()) return true;
00473         removeVertex(*simplex);
00474         appendVertex(*simplex, -n);
00475         if(encloseOrigin()) return true;
00476         removeVertex(*simplex);
00477       }
00478     }
00479     break;
00480   case 4:
00481     {
00482       if(std::abs(triple(simplex->c[0]->w - simplex->c[3]->w, simplex->c[1]->w - simplex->c[3]->w, simplex->c[2]->w - simplex->c[3]->w)) > 0)
00483         return true;
00484     }
00485     break;
00486   }
00487 
00488   return false;
00489 }
00490 
00491 
00492 void EPA::initialize()
00493 {
00494   sv_store = new SimplexV[max_vertex_num];
00495   fc_store = new SimplexF[max_face_num];
00496   status = Failed;
00497   normal = Vec3f(0, 0, 0);
00498   depth = 0;
00499   nextsv = 0;
00500   for(size_t i = 0; i < max_face_num; ++i)
00501     stock.append(&fc_store[max_face_num-i-1]);
00502 }
00503 
00504 bool EPA::getEdgeDist(SimplexF* face, SimplexV* a, SimplexV* b, FCL_REAL& dist)
00505 {
00506   Vec3f ba = b->w - a->w;
00507   Vec3f n_ab = ba.cross(face->n);
00508   FCL_REAL a_dot_nab = a->w.dot(n_ab);
00509 
00510   if(a_dot_nab < 0) // the origin is on the outside part of ab
00511   {
00512     FCL_REAL ba_l2 = ba.sqrLength();
00513 
00514     // following is similar to projectOrigin for two points
00515     // however, as we dont need to compute the parameterization, dont need to compute 0 or 1
00516     FCL_REAL a_dot_ba = a->w.dot(ba); 
00517     FCL_REAL b_dot_ba = b->w.dot(ba);
00518 
00519     if(a_dot_ba > 0) 
00520       dist = a->w.length();
00521     else if(b_dot_ba < 0)
00522       dist = b->w.length();
00523     else
00524     {
00525       FCL_REAL a_dot_b = a->w.dot(b->w);
00526       dist = std::sqrt(std::max(a->w.sqrLength() * b->w.sqrLength() - a_dot_b * a_dot_b, (FCL_REAL)0));
00527     }
00528 
00529     return true;
00530   }
00531 
00532   return false;
00533 }
00534 
00535 EPA::SimplexF* EPA::newFace(SimplexV* a, SimplexV* b, SimplexV* c, bool forced)
00536 {
00537   if(stock.root)
00538   {
00539     SimplexF* face = stock.root;
00540     stock.remove(face);
00541     hull.append(face);
00542     face->pass = 0;
00543     face->c[0] = a;
00544     face->c[1] = b;
00545     face->c[2] = c;
00546     face->n = (b->w - a->w).cross(c->w - a->w);
00547     FCL_REAL l = face->n.length();
00548       
00549     if(l > tolerance)
00550     {
00551       if(!(getEdgeDist(face, a, b, face->d) ||
00552            getEdgeDist(face, b, c, face->d) ||
00553            getEdgeDist(face, c, a, face->d)))
00554       {
00555         face->d = a->w.dot(face->n) / l;
00556       }
00557 
00558       face->n /= l;
00559       if(forced || face->d >= -tolerance)
00560         return face;
00561       else
00562         status = NonConvex;
00563     }
00564     else
00565       status = Degenerated;
00566 
00567     hull.remove(face);
00568     stock.append(face);
00569     return NULL;
00570   }
00571     
00572   status = stock.root ? OutOfVertices : OutOfFaces;
00573   return NULL;
00574 }
00575 
00577 EPA::SimplexF* EPA::findBest()
00578 {
00579   SimplexF* minf = hull.root;
00580   FCL_REAL mind = minf->d * minf->d;
00581   for(SimplexF* f = minf->l[1]; f; f = f->l[1])
00582   {
00583     FCL_REAL sqd = f->d * f->d;
00584     if(sqd < mind)
00585     {
00586       minf = f;
00587       mind = sqd;
00588     }
00589   }
00590   return minf;
00591 }
00592 
00593 EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess)
00594 {
00595   GJK::Simplex& simplex = *gjk.getSimplex();
00596   if((simplex.rank > 1) && gjk.encloseOrigin())
00597   {
00598     while(hull.root)
00599     {
00600       SimplexF* f = hull.root;
00601       hull.remove(f);
00602       stock.append(f);
00603     }
00604 
00605     status = Valid;
00606     nextsv = 0;
00607 
00608     if((simplex.c[0]->w - simplex.c[3]->w).dot((simplex.c[1]->w - simplex.c[3]->w).cross(simplex.c[2]->w - simplex.c[3]->w)) < 0)
00609     {
00610       SimplexV* tmp = simplex.c[0];
00611       simplex.c[0] = simplex.c[1];
00612       simplex.c[1] = tmp;
00613 
00614       FCL_REAL tmpv = simplex.p[0];
00615       simplex.p[0] = simplex.p[1];
00616       simplex.p[1] = tmpv;
00617     }
00618 
00619     SimplexF* tetrahedron[] = {newFace(simplex.c[0], simplex.c[1], simplex.c[2], true),
00620                                newFace(simplex.c[1], simplex.c[0], simplex.c[3], true),
00621                                newFace(simplex.c[2], simplex.c[1], simplex.c[3], true),
00622                                newFace(simplex.c[0], simplex.c[2], simplex.c[3], true) };
00623 
00624     if(hull.count == 4)
00625     {
00626       SimplexF* best = findBest(); // find the best face (the face with the minimum distance to origin) to split
00627       SimplexF outer = *best;
00628       size_t pass = 0;
00629       size_t iterations = 0;
00630         
00631       // set the face connectivity
00632       bind(tetrahedron[0], 0, tetrahedron[1], 0);
00633       bind(tetrahedron[0], 1, tetrahedron[2], 0);
00634       bind(tetrahedron[0], 2, tetrahedron[3], 0);
00635       bind(tetrahedron[1], 1, tetrahedron[3], 2);
00636       bind(tetrahedron[1], 2, tetrahedron[2], 1);
00637       bind(tetrahedron[2], 2, tetrahedron[3], 1);
00638 
00639       status = Valid;
00640       for(; iterations < max_iterations; ++iterations)
00641       {
00642         if(nextsv < max_vertex_num)
00643         {
00644           SimplexHorizon horizon;
00645           SimplexV* w = &sv_store[nextsv++];
00646           bool valid = true;
00647           best->pass = ++pass;
00648           gjk.getSupport(best->n, *w);
00649           FCL_REAL wdist = best->n.dot(w->w) - best->d;
00650           if(wdist > tolerance)
00651           {
00652             for(size_t j = 0; (j < 3) && valid; ++j)
00653             {
00654               valid &= expand(pass, w, best->f[j], best->e[j], horizon);
00655             }
00656 
00657               
00658             if(valid && horizon.nf >= 3)
00659             {
00660               // need to add the edge connectivity between first and last faces
00661               bind(horizon.ff, 2, horizon.cf, 1);
00662               hull.remove(best);
00663               stock.append(best);
00664               best = findBest();
00665               outer = *best;
00666             }
00667             else
00668             {
00669               status = InvalidHull; break;
00670             }
00671           }
00672           else
00673           {
00674             status = AccuracyReached; break;
00675           }
00676         }
00677         else
00678         {
00679           status = OutOfVertices; break;
00680         }
00681       }
00682 
00683       Vec3f projection = outer.n * outer.d;
00684       normal = outer.n;
00685       depth = outer.d;
00686       result.rank = 3;
00687       result.c[0] = outer.c[0];
00688       result.c[1] = outer.c[1];
00689       result.c[2] = outer.c[2];
00690       result.p[0] = ((outer.c[1]->w - projection).cross(outer.c[2]->w - projection)).length();
00691       result.p[1] = ((outer.c[2]->w - projection).cross(outer.c[0]->w - projection)).length();
00692       result.p[2] = ((outer.c[0]->w - projection).cross(outer.c[1]->w - projection)).length();
00693 
00694       FCL_REAL sum = result.p[0] + result.p[1] + result.p[2];
00695       result.p[0] /= sum;
00696       result.p[1] /= sum;
00697       result.p[2] /= sum;
00698       return status;
00699     }
00700   }
00701 
00702   status = FallBack;
00703   normal = -guess;
00704   FCL_REAL nl = normal.length();
00705   if(nl > 0) normal /= nl;
00706   else normal = Vec3f(1, 0, 0);
00707   depth = 0;
00708   result.rank = 1;
00709   result.c[0] = simplex.c[0];
00710   result.p[0] = 1;
00711   return status;
00712 }
00713 
00714 
00716 bool EPA::expand(size_t pass, SimplexV* w, SimplexF* f, size_t e, SimplexHorizon& horizon)
00717 {
00718   static const size_t nexti[] = {1, 2, 0};
00719   static const size_t previ[] = {2, 0, 1};
00720 
00721   if(f->pass != pass)
00722   {
00723     const size_t e1 = nexti[e];
00724       
00725     // case 1: the new face is not degenerated, i.e., the new face is not coplanar with the old face f.
00726     if(f->n.dot(w->w) - f->d < -tolerance)
00727     {
00728       SimplexF* nf = newFace(f->c[e1], f->c[e], w, false);
00729       if(nf)
00730       {
00731         // add face-face connectivity
00732         bind(nf, 0, f, e);
00733           
00734         // if there is last face in the horizon, then need to add another connectivity, i.e. the edge connecting the current new add edge and the last new add edge. 
00735         // This does not finish all the connectivities because the final face need to connect with the first face, this will be handled in the evaluate function.
00736         // Notice the face is anti-clockwise, so the edges are 0 (bottom), 1 (right), 2 (left)
00737         if(horizon.cf)  
00738           bind(nf, 2, horizon.cf, 1);
00739         else
00740           horizon.ff = nf; 
00741           
00742         horizon.cf = nf;
00743         ++horizon.nf;
00744         return true;
00745       }
00746     }
00747     else // case 2: the new face is coplanar with the old face f. We need to add two faces and delete the old face
00748     {
00749       const size_t e2 = previ[e];
00750       f->pass = pass;
00751       if(expand(pass, w, f->f[e1], f->e[e1], horizon) && expand(pass, w, f->f[e2], f->e[e2], horizon))
00752       {
00753         hull.remove(f);
00754         stock.append(f);
00755         return true;
00756       }
00757     }
00758   }
00759 
00760   return false;
00761 }
00762 
00763 } // details
00764 
00765 } // fcl
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines


fcl
Author(s): Jia Pan
autogenerated on Tue Jan 15 2013 16:05:30