convex_hull.cpp
Go to the documentation of this file.
1 
7 #include "convex_hull.h"
8 
9 #include <algorithm>
10 
11 namespace argos {
12 
13  /****************************************/
14  /****************************************/
15 
16  void Insert(std::vector<std::vector<CConvexHull::TEdge> >& vec_edges,
17  const std::array<UInt32, 3>& arr_vertices) {
18  for(UInt32 un_idx_i = 0; un_idx_i < arr_vertices.size(); un_idx_i++) {
19  for(UInt32 un_idx_j = 0; un_idx_j < arr_vertices.size(); un_idx_j++) {
20  if(un_idx_i != un_idx_j) {
21  CConvexHull::TEdge& tEdge =
22  vec_edges[arr_vertices[un_idx_i]][arr_vertices[un_idx_j]];
23  for(UInt32 un_idx_k = 0; un_idx_k < arr_vertices.size(); un_idx_k++) {
24  if(un_idx_k != un_idx_i && un_idx_k != un_idx_j) {
25  tEdge.push_back(arr_vertices[un_idx_k]);
26  }
27  }
28  }
29  }
30  }
31  }
32 
33  /****************************************/
34  /****************************************/
35 
36  void Erase(std::vector<std::vector<CConvexHull::TEdge> >& vec_edges,
37  const std::array<UInt32, 3>& arr_vertices) {
38  for(UInt32 un_idx_i = 0; un_idx_i < arr_vertices.size(); un_idx_i++) {
39  for(UInt32 un_idx_j = 0; un_idx_j < arr_vertices.size(); un_idx_j++) {
40  if(un_idx_i != un_idx_j) {
41  CConvexHull::TEdge& tEdge =
42  vec_edges[arr_vertices[un_idx_i]][arr_vertices[un_idx_j]];
43  for(UInt32 un_idx_k = 0; un_idx_k < arr_vertices.size(); un_idx_k++) {
44  if(un_idx_k != un_idx_i && un_idx_k != un_idx_j) {
45  auto itErase =
46  std::find(std::begin(tEdge), std::end(tEdge), arr_vertices[un_idx_k]);
47  if(itErase != std::end(tEdge)) {
48  tEdge.erase(itErase);
49  }
50  }
51  }
52  }
53  }
54  }
55  }
56 
57  /****************************************/
58  /****************************************/
59 
60  /*
61  * The algorithm used to building the convex hull is based on:
62  * 1. http://instructor3.algorithmdesign.net/ppt/pdf1/IncrementalHull.pdf
63  * 2. https://gist.github.com/msg555/4963794
64  */
65  CConvexHull::CConvexHull(const std::vector<CVector3>& vec_points) :
66  m_vecPoints(vec_points) {
67  /* prepare the edge table */
68  std::vector<std::vector<TEdge> > m_vecEdge;
69  m_vecEdge.resize(m_vecPoints.size());
70  for(std::vector<TEdge>& vec_edge : m_vecEdge) {
71  vec_edge.resize(m_vecPoints.size());
72  }
73  /* some of the points may form a plane or a line so find four points
74  that make a solid 3D entity as a starting point */
75  std::array<UInt32, 4> arrInitialPoints;
76  bool bFoundInitialPoints = false;
77  for (UInt32 unI = 0; unI < m_vecPoints.size() && !bFoundInitialPoints; unI++) {
78  for (UInt32 unJ = unI + 1; unJ < m_vecPoints.size() && !bFoundInitialPoints; unJ++) {
79  for (UInt32 unK = unJ + 1; unK < m_vecPoints.size() && !bFoundInitialPoints; unK++) {
80  for (UInt32 unL = unK + 1; unL < m_vecPoints.size(); unL++) {
81  /* calculate normal vector with the first 3 points */
82  CVector3 cNormal =
83  (m_vecPoints[unI] - m_vecPoints[unJ]).CrossProduct(m_vecPoints[unI] - m_vecPoints[unK]);
84  /* normal is 0 means this 3 points are in a line */
85  /* compare Real cNormal.Length() with 0 (m_fZero)
86  * */
87  if (cNormal.Length() < 0.000001)
88  continue;
89  /* whether the 4th point is in the same plane */
90  if ((cNormal.DotProduct(m_vecPoints[unL] - m_vecPoints[unI]) < 0.000001) &&
91  (cNormal.DotProduct(m_vecPoints[unL] - m_vecPoints[unI]) > -0.000001) )
92  continue;
93  arrInitialPoints = {unI, unJ, unK, unL};
94  bFoundInitialPoints = true;
95  break;
96  }
97  }
98  }
99  }
100  /* create 4 faces out of these 4 points*/
101  for (UInt32 un_idx_i = 0; un_idx_i < 4; un_idx_i++) {
102  for (UInt32 un_idx_j = un_idx_i + 1; un_idx_j < 4; un_idx_j++) {
103  for (UInt32 un_idx_k = un_idx_j + 1; un_idx_k < 4; un_idx_k++) {
104  m_vecFaces.emplace_back(m_vecPoints,
105  arrInitialPoints[un_idx_i],
106  arrInitialPoints[un_idx_j],
107  arrInitialPoints[un_idx_k],
108  arrInitialPoints[6 - un_idx_i - un_idx_j - un_idx_k]);
109  const SFace& sFace = m_vecFaces.back();
110  Insert(m_vecEdge, sFace.VertexIndices);
111  }
112  }
113  }
114  /* create faces with the remaining points */
115  for (UInt32 un_idx_point = 0; un_idx_point < m_vecPoints.size(); un_idx_point++) {
116  const CVector3& cPoint = m_vecPoints[un_idx_point];
117 
118  /* check if this point was already used */
119  auto itPoint =
120  std::find(std::begin(arrInitialPoints), std::end(arrInitialPoints), un_idx_point);
121  if(itPoint != std::end(arrInitialPoints)) {
122  continue;
123  }
124  /* find all the faces with the focal point at its outside, and remove them */
125  for (const SFace& s_face : m_vecFaces) {
126  if (s_face.Normal.DotProduct(cPoint) > s_face.Direction + 0.000001) {
127  Erase(m_vecEdge, s_face.VertexIndices);
128  }
129  }
130  auto itEraseFrom =
131  std::remove_if(std::begin(m_vecFaces), std::end(m_vecFaces), [cPoint] (const SFace& s_face) {
132  return (s_face.Normal.DotProduct(cPoint) > s_face.Direction + 0.000001);
133  });
134  m_vecFaces.erase(itEraseFrom, std::end(m_vecFaces));
135  /* add new faces */
136  std::vector<SFace> vecFacesToAdd;
137  for (const SFace& s_face : m_vecFaces) {
138  for (UInt32 unIdxA = 0; unIdxA < 3; unIdxA++) {
139  for (UInt32 unIdxB = unIdxA + 1; unIdxB < 3; unIdxB++) {
140  if (m_vecEdge[s_face.VertexIndices[unIdxA]][s_face.VertexIndices[unIdxB]].size() == 2)
141  continue;
142  UInt32 unIdxC = 3 - unIdxA - unIdxB;
143  vecFacesToAdd.emplace_back(m_vecPoints,
144  s_face.VertexIndices[unIdxA],
145  s_face.VertexIndices[unIdxB],
146  un_idx_point,
147  s_face.VertexIndices[unIdxC]);
148  SFace& sFace = vecFacesToAdd.back();
149  /* verify direction, the direction should be consistent with the focal face (s_face)
150  * the right direction is told by the order of unIdxA and unIdxB
151  * for example, if unIdxA and unIdxB are 0 and 1, then in the new face, the
152  * order of point s_face.VertexIndices[unIdxA] and s_face.VertexIndices[unIdxB] should be reversed
153  * */
154  if ((((unIdxB - unIdxA + 3) % 3 == 1) && (sFace.VertexIndices[2] != s_face.VertexIndices[unIdxB])) ||
155  (((unIdxA - unIdxB + 3) % 3 == 1) && (sFace.VertexIndices[1] != s_face.VertexIndices[unIdxB]))) {
156  sFace.Flip();
157  }
158  Insert(m_vecEdge, sFace.VertexIndices);
159  }
160  }
161  }
162  std::move(std::begin(vecFacesToAdd),
163  std::end(vecFacesToAdd),
164  std::back_inserter(m_vecFaces));
165  vecFacesToAdd.clear();
166  }
167  }
168 
169  /****************************************/
170  /****************************************/
171 
172  CConvexHull::SFace::SFace(const std::vector<CVector3>& vec_points,
173  UInt32 un_A,
174  UInt32 un_B,
175  UInt32 un_C,
176  UInt32 un_inside_point) {
177  VertexIndices = {un_A, un_B, un_C};
178  Normal = (vec_points[un_B] - vec_points[un_A]).CrossProduct(vec_points[un_C] - vec_points[un_A]);
179  Direction = Normal.DotProduct(vec_points[un_A]);
180  /* flip face outwards if required */
181  if (Normal.DotProduct(vec_points[un_inside_point]) > Direction) {
182  Flip();
183  }
184  }
185 
186  /****************************************/
187  /****************************************/
188 
189  void CConvexHull::SFace::Flip() {
190  Normal = -Normal;
191  Direction = -Direction;
192  std::swap(VertexIndices[1], VertexIndices[2]);
193  }
194 
195  /****************************************/
196  /****************************************/
197 
198 }
UInt32
unsigned int UInt32
32-bit unsigned integer.
Definition: datatypes.h:97
argos
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
argos::Erase
void Erase(std::vector< std::vector< CConvexHull::TEdge > > &vec_edges, const std::array< UInt32, 3 > &arr_vertices)
Definition: convex_hull.cpp:36
argos::Insert
void Insert(std::vector< std::vector< CConvexHull::TEdge > > &vec_edges, const std::array< UInt32, 3 > &arr_vertices)
Definition: convex_hull.cpp:16
argos::CConvexHull::TEdge
std::vector< UInt32 > TEdge
Definition: convex_hull.h:23
argos::CConvexHull::CConvexHull
CConvexHull(const std::vector< CVector3 > &vec_points)
Definition: convex_hull.cpp:65
argos::CConvexHull::SFace::VertexIndices
std::array< UInt32, 3 > VertexIndices
Definition: convex_hull.h:38
argos::CConvexHull::SFace::SFace
SFace(const std::vector< CVector3 > &vec_points, UInt32 un_A, UInt32 un_B, UInt32 un_C, UInt32 un_inside_point)
Definition: convex_hull.cpp:172
argos::CVector3
A 3D vector class.
Definition: vector3.h:31
argos::CVector3::Length
Real Length() const
Returns the length of this vector.
Definition: vector3.h:227
argos::CVector3::DotProduct
Real DotProduct(const CVector3 &c_vector3) const
Returns the dot product between this vector and the passed one.
Definition: vector3.h:369