GJK_nD
 All Classes Files Functions Typedefs Friends Pages
GJK_nD.hpp
1 /*
2  * Copyright (c) 2012 Laurent Provot <provot.research@gmail.com>,
3  * Yan Gerard <yan.gerard@free.fr> and Fabien Feschet <research@feschet.fr>
4  * All rights reserved.
5  *
6  * This is free software: you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation, either version 3 of the License, or
9  * (at your option) any later version.
10  *
11  * This program is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with this program. If not, see <http://www.gnu.org/licenses/>.
18  */
19 
20 #ifndef GJK_ND_HPP
21 #define GJK_ND_HPP
22 
23 #include <vector>
24 #include <stdexcept>
25 #include "Eigen/Dense"
26 
31 class SingularSystem : public std::runtime_error
32 {
33 public:
35  : std::runtime_error("The system's matrix is not invertible.")
36  {}
37 };
38 
39 
46 class SuspiciousLoop : public std::runtime_error
47 {
48 public:
50  : std::runtime_error("The convergence loop for this instance seems "
51  "suspicious (too many iterations).")
52  {}
53 };
54 
55 
95 class GJKnD
96 {
97 public:
99  typedef Eigen::VectorXd Vector;
101  typedef Vector Point;
102 
103 private:
105  typedef Eigen::MatrixXd Matrix;
106 
107 public:
124  static bool isDigitalHyperplane(const std::vector<Point> & setIn,
125  const std::vector<Point> & setAbove,
126  const std::vector<Point> & setBelow,
127  Vector & normal,
128  double & h, double & H);
129 
130 private:
131  static Vector barycentricCoordinatesOrigin(const std::vector<Point> & simplex);
132 
133  static void closestSimplexToOrigin(const Point & newPoint,
134  const std::vector<Point> & previousSimplex,
135  std::vector<Point> & closestSimplex,
136  Point & towardOrigin);
137 
138  static void findSupportPoint(const Vector & searchDirection,
139  const std::vector<Point> & setIn,
140  const std::vector<Point> & setAbove,
141  const std::vector<Point> & setBelow,
142  Point & result,
143  double & distance);
144 };
145 
146 #endif // GJK_ND_HPP