# GJK.h

Go to the documentation of this file.
```00001 #include "Planners/PlannerObstacles.h"
00002 #include "Shared/plistSpecialty.h"
00003 #include <vector>
00004
00005 namespace GJK {
00006
00007   //! Returns (@a a x @a b) x @a c = @a b * (@a a • @a c) - @a a * (@a b • @a c)
00008   /*! Commutative only if @a a = @a c: @a a x (@a b x @a a) == (@a a x @a b) x @a a
00009    *  Otherwise, to get @a a x (@a b x @a c), call tripleProduct(@a b, @a c, -@a a) */
00010   template<size_t N>
00011   fmat::Column<N> tripleProduct(const fmat::Column<N>& a, const fmat::Column<N>& b, const fmat::Column<N>& c) {
00012     return b * fmat::dotProduct(a, c) - a * fmat::dotProduct(b, c);
00013   }
00014
00015   template<size_t N>
00016   bool processLine(std::vector<fmat::Column<N> >& simplex, fmat::Column<N>& direction) {
00017     fmat::Column<N>& a = simplex[1], b = simplex[0];
00018     fmat::Column<N> ab = b - a;
00019
00020     if (fmat::dotProduct(ab, -a) > 0)
00021       direction = tripleProduct(ab, -a, ab);
00022     else {
00023       simplex.erase(simplex.begin());
00024       direction = -a;
00025     }
00026     return false;
00027   }
00028
00029   template<size_t N>
00030   bool processTriangle(std::vector<fmat::Column<N> >& simplex, fmat::Column<N>& direction) {
00031     fmat::Column<N>& a = simplex[2], b = simplex[1], c = simplex[0];
00032
00033     fmat::Column<N> ab = b - a;
00034     fmat::Column<N> ac = c - a;
00035
00036     // ababc = ab x (ab x ac) = (ab x ac) x -ab
00037     fmat::Column<N> ababc = tripleProduct(ab, ac, -ab);
00038     // abcac = (ab x ac) x ac
00039     fmat::Column<N> abcac = tripleProduct(ab, ac, ac);
00040
00041     if (fmat::dotProduct(abcac, -a) > 0) {
00042       // ac simplex
00043       if (fmat::dotProduct(ac, -a) > 0) {
00044         // erase b
00045         simplex.erase(simplex.begin()+1);
00046         direction = tripleProduct(ac, -a, ac);
00047       }
00048       else {
00049         // erase c
00050         simplex.erase(simplex.begin());
00051         return processLine(simplex, direction);
00052       }
00053     }
00054     else {
00055       if (fmat::dotProduct(ababc, -a) > 0) {
00056         // erase c
00057         simplex.erase(simplex.begin());
00058         return processLine(simplex, direction);
00059       }
00060       /* for 2D case, we collided if we got to here.
00061        but for 3D case, we test if the point is above or below the point */
00062       else {
00063         if (N==2)
00064           return true;
00065         else if (N==3) {
00066           fmat::Column<N> abc = fmat::Column<N>(fmat::crossProduct(ab, ac));
00067           if (fmat::dotProduct(abc, -a) > 0) {
00068             direction = abc;
00069           }
00070           else {
00071             // swap b and c
00072             std::swap(b, c);
00073             direction = -abc;
00074           }
00075         }
00076       }
00077     }
00078     return false;
00079   }
00080
00081   // the 2D case will never get here anyway, but it has to be templated for consistency.
00082   // TODO: fix so that this is not templated, and is only defined for N=3
00083   template<size_t N>
00084   bool processTetrahedron(std::vector<fmat::Column<N> >& simplex, fmat::Column<N>& direction) {
00085     fmat::Column<N>& a = simplex[3], b = simplex[2], c = simplex[1], d = simplex[0];
00086
00087     fmat::Column<N> ab = b - a;
00088     fmat::Column<N> ac = c - a;
00089     fmat::Column<N> ad = d - a;
00090     // casting needed because of templating
00091     fmat::Column<N> abc = fmat::Column<N>(fmat::crossProduct(ab, ac));
00092     fmat::Column<N> acd = fmat::Column<N>(fmat::crossProduct(ac, ad));
00094
00095     if (fmat::dotProduct(abc, -a) > 0) {
00096       if (fmat::dotProduct(acd, -a) > 0) {
00097         if (fmat::dotProduct(adb, -a) > 0) {
00098           // erase b, c, d
00099           simplex.erase(simplex.begin(), simplex.begin()+3);
00100           direction = -simplex[0];
00101           return false;
00102         }
00103         else {
00104           // erase b, d
00105           simplex.erase(simplex.begin()+2);
00106           simplex.erase(simplex.begin());
00107           return processLine(simplex, direction);
00108         }
00109       }
00110       else {
00111         if (fmat::dotProduct(adb, -a) > 0) {
00112           // erase c, d
00113           simplex.erase(simplex.begin(), simplex.begin()+2);
00114           return processLine(simplex, direction);
00115         }
00116         else {
00117           // erase d
00118           simplex.erase(simplex.begin());
00119           return processTriangle(simplex, direction);
00120         }
00121       }
00122     }
00123     else {
00124       if (fmat::dotProduct(acd, -a) > 0) {
00125         if (fmat::dotProduct(adb, -a) > 0) {
00126           // erase b, c
00127           simplex.erase(simplex.begin()+1, simplex.begin()+3);
00128           return processLine(simplex, direction);
00129         }
00130         else {
00131           // erase b
00132           simplex.erase(simplex.begin()+2);
00133           return processTriangle(simplex, direction);
00134         }
00135       }
00136       else {
00137         if (fmat::dotProduct(adb, -a) > 0) {
00138           // erase c
00139           simplex.erase(simplex.begin()+1);
00140           return processTriangle(simplex, direction);
00141         }
00142         else
00143           return true;
00144       }
00145     }
00146   }
00147
00148   template<size_t N>
00149   bool processSimplex(std::vector<fmat::Column<N> >& simplex, fmat::Column<N>& direction) {
00150     switch (simplex.size()) {
00151       case 2:
00152         return processLine(simplex, direction);
00153       case 3:
00154         return processTriangle(simplex, direction);
00155       case 4:
00156         // only for 3D case, so we cast just to avoid error
00157         return processTetrahedron(simplex, direction);
00158       default: // should never happen
00159         return false;
00160     }
00161   }
00162
00163   template<size_t N>
00164   fmat::Column<N> getSupport(const PlannerObstacle<N>* obs1, const PlannerObstacle<N>* obs2, const fmat::Column<N>& direction) {
00165     return obs1->getSupport(direction) - obs2->getSupport(-direction);
00166   }
00167
00168   template<size_t N>
00169   bool collides(const PlannerObstacle<N>* obs1, const PlannerObstacle<N>* obs2) {
00170     // choose initial direction
00171     fmat::Column<N> direction;
00172     direction[0] = 1;
00173
00174     // initialize simplex
00175     std::vector<fmat::Column<N> > simplex;
00176
00177     // for the 2 case, we needs 3 pts, for the 3 case we need 4 pts.
00178     simplex.reserve(N+1);
00179
00180     // get first point
00181     simplex.push_back(getSupport(obs1, obs2, direction));
00182
00183     // direction should point towards the origin
00184     direction = -simplex[0];
00185
00186     int maxIterations = 20;
00187
00188     while (maxIterations--) {
00190       simplex.push_back(getSupport(obs1, obs2, direction));
00191
00192       // see if the new point was on the correct side of the origin
00193       if (fmat::dotProduct(simplex.back(), direction) < 0)
00194         return false;
00195
00196       // process the simplex
00197       if (processSimplex(simplex, direction))
00198         return true;
00199     }
00200
00201     // too many iterations... algorithm should converge in very few. assuming collision
00202     // std::cout << "WARNING: GJK: too many iterations, assuming collision." << std::endl;
00203     return true;
00204   }
00205
00206 }
```

 Tekkotsu v5.1CVS Generated Mon May 9 04:58:41 2016 by Doxygen 1.6.3