DART  6.10.1
CollisionShapes.hpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2011-2021, The DART development contributors
3  * All rights reserved.
4  *
5  * The list of contributors can be found at:
6  * https://github.com/dartsim/dart/blob/master/LICENSE
7  *
8  * This file is provided under the following "BSD-style" License:
9  * Redistribution and use in source and binary forms, with or
10  * without modification, are permitted provided that the following
11  * conditions are met:
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
19  * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
20  * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21  * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
23  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
24  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
25  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
26  * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  */
32 
33 #ifndef DART_COLLISION_FCL_MESH_COLLISIONSHAPES_HPP_
34 #define DART_COLLISION_FCL_MESH_COLLISIONSHAPES_HPP_
35 
36 #include <cmath>
37 #include <assimp/scene.h>
38 
39 #include "dart/collision/fcl/BackwardCompatibility.hpp"
40 #include "dart/math/Constants.hpp"
41 
42 namespace dart {
43 namespace collision {
44 
45 template <class BV>
46 ::fcl::BVHModel<BV>* createMesh(
47  float _scaleX,
48  float _scaleY,
49  float _scaleZ,
50  const aiScene* _mesh,
51  const dart::collision::fcl::Transform3& _transform)
52 {
53  assert(_mesh);
54  ::fcl::BVHModel<BV>* model = new ::fcl::BVHModel<BV>;
55  model->beginModel();
56 
57  for (unsigned int i = 0; i < _mesh->mNumMeshes; i++)
58  {
59  for (unsigned int j = 0; j < _mesh->mMeshes[i]->mNumFaces; j++)
60  {
61  dart::collision::fcl::Vector3 vertices[3];
62  for (unsigned int k = 0; k < 3; k++)
63  {
64  const aiVector3D& vertex
65  = _mesh->mMeshes[i]
66  ->mVertices[_mesh->mMeshes[i]->mFaces[j].mIndices[k]];
67  vertices[k] = dart::collision::fcl::Vector3(
68  vertex.x * _scaleX, vertex.y * _scaleY, vertex.z * _scaleZ);
69  vertices[k] = dart::collision::fcl::transform(_transform, vertices[k]);
70  }
71  model->addTriangle(vertices[0], vertices[1], vertices[2]);
72  }
73  }
74 
75  model->endModel();
76  return model;
77 }
78 
79 template <class BV>
80 ::fcl::BVHModel<BV>* createEllipsoid(
81  float _sizeX,
82  float _sizeY,
83  float _sizeZ,
84  const dart::collision::fcl::Transform3& _transform)
85 {
86  float v[59][3] = {{0, 0, 0},
87  {0.135299, -0.461940, -0.135299},
88  {0.000000, -0.461940, -0.191342},
89  {-0.135299, -0.461940, -0.135299},
90  {-0.191342, -0.461940, 0.000000},
91  {-0.135299, -0.461940, 0.135299},
92  {0.000000, -0.461940, 0.191342},
93  {0.135299, -0.461940, 0.135299},
94  {0.191342, -0.461940, 0.000000},
95  {0.250000, -0.353553, -0.250000},
96  {0.000000, -0.353553, -0.353553},
97  {-0.250000, -0.353553, -0.250000},
98  {-0.353553, -0.353553, 0.000000},
99  {-0.250000, -0.353553, 0.250000},
100  {0.000000, -0.353553, 0.353553},
101  {0.250000, -0.353553, 0.250000},
102  {0.353553, -0.353553, 0.000000},
103  {0.326641, -0.191342, -0.326641},
104  {0.000000, -0.191342, -0.461940},
105  {-0.326641, -0.191342, -0.326641},
106  {-0.461940, -0.191342, 0.000000},
107  {-0.326641, -0.191342, 0.326641},
108  {0.000000, -0.191342, 0.461940},
109  {0.326641, -0.191342, 0.326641},
110  {0.461940, -0.191342, 0.000000},
111  {0.353553, 0.000000, -0.353553},
112  {0.000000, 0.000000, -0.500000},
113  {-0.353553, 0.000000, -0.353553},
114  {-0.500000, 0.000000, 0.000000},
115  {-0.353553, 0.000000, 0.353553},
116  {0.000000, 0.000000, 0.500000},
117  {0.353553, 0.000000, 0.353553},
118  {0.500000, 0.000000, 0.000000},
119  {0.326641, 0.191342, -0.326641},
120  {0.000000, 0.191342, -0.461940},
121  {-0.326641, 0.191342, -0.326641},
122  {-0.461940, 0.191342, 0.000000},
123  {-0.326641, 0.191342, 0.326641},
124  {0.000000, 0.191342, 0.461940},
125  {0.326641, 0.191342, 0.326641},
126  {0.461940, 0.191342, 0.000000},
127  {0.250000, 0.353553, -0.250000},
128  {0.000000, 0.353553, -0.353553},
129  {-0.250000, 0.353553, -0.250000},
130  {-0.353553, 0.353553, 0.000000},
131  {-0.250000, 0.353553, 0.250000},
132  {0.000000, 0.353553, 0.353553},
133  {0.250000, 0.353553, 0.250000},
134  {0.353553, 0.353553, 0.000000},
135  {0.135299, 0.461940, -0.135299},
136  {0.000000, 0.461940, -0.191342},
137  {-0.135299, 0.461940, -0.135299},
138  {-0.191342, 0.461940, 0.000000},
139  {-0.135299, 0.461940, 0.135299},
140  {0.000000, 0.461940, 0.191342},
141  {0.135299, 0.461940, 0.135299},
142  {0.191342, 0.461940, 0.000000},
143  {0.000000, -0.500000, 0.000000},
144  {0.000000, 0.500000, 0.000000}};
145  int f[112][3]
146  = {{1, 2, 9}, {9, 2, 10}, {2, 3, 10}, {10, 3, 11}, {3, 4, 11},
147  {11, 4, 12}, {4, 5, 12}, {12, 5, 13}, {5, 6, 13}, {13, 6, 14},
148  {6, 7, 14}, {14, 7, 15}, {7, 8, 15}, {15, 8, 16}, {8, 1, 16},
149  {16, 1, 9}, {9, 10, 17}, {17, 10, 18}, {10, 11, 18}, {18, 11, 19},
150  {11, 12, 19}, {19, 12, 20}, {12, 13, 20}, {20, 13, 21}, {13, 14, 21},
151  {21, 14, 22}, {14, 15, 22}, {22, 15, 23}, {15, 16, 23}, {23, 16, 24},
152  {16, 9, 24}, {24, 9, 17}, {17, 18, 25}, {25, 18, 26}, {18, 19, 26},
153  {26, 19, 27}, {19, 20, 27}, {27, 20, 28}, {20, 21, 28}, {28, 21, 29},
154  {21, 22, 29}, {29, 22, 30}, {22, 23, 30}, {30, 23, 31}, {23, 24, 31},
155  {31, 24, 32}, {24, 17, 32}, {32, 17, 25}, {25, 26, 33}, {33, 26, 34},
156  {26, 27, 34}, {34, 27, 35}, {27, 28, 35}, {35, 28, 36}, {28, 29, 36},
157  {36, 29, 37}, {29, 30, 37}, {37, 30, 38}, {30, 31, 38}, {38, 31, 39},
158  {31, 32, 39}, {39, 32, 40}, {32, 25, 40}, {40, 25, 33}, {33, 34, 41},
159  {41, 34, 42}, {34, 35, 42}, {42, 35, 43}, {35, 36, 43}, {43, 36, 44},
160  {36, 37, 44}, {44, 37, 45}, {37, 38, 45}, {45, 38, 46}, {38, 39, 46},
161  {46, 39, 47}, {39, 40, 47}, {47, 40, 48}, {40, 33, 48}, {48, 33, 41},
162  {41, 42, 49}, {49, 42, 50}, {42, 43, 50}, {50, 43, 51}, {43, 44, 51},
163  {51, 44, 52}, {44, 45, 52}, {52, 45, 53}, {45, 46, 53}, {53, 46, 54},
164  {46, 47, 54}, {54, 47, 55}, {47, 48, 55}, {55, 48, 56}, {48, 41, 56},
165  {56, 41, 49}, {2, 1, 57}, {3, 2, 57}, {4, 3, 57}, {5, 4, 57},
166  {6, 5, 57}, {7, 6, 57}, {8, 7, 57}, {1, 8, 57}, {49, 50, 58},
167  {50, 51, 58}, {51, 52, 58}, {52, 53, 58}, {53, 54, 58}, {54, 55, 58},
168  {55, 56, 58}, {56, 49, 58}};
169 
170  ::fcl::BVHModel<BV>* model = new ::fcl::BVHModel<BV>;
171  dart::collision::fcl::Vector3 p1, p2, p3;
172  model->beginModel();
173 
174  for (int i = 0; i < 112; i++)
175  {
176  p1 = dart::collision::fcl::Vector3(
177  v[f[i][0]][0] * _sizeX, v[f[i][0]][1] * _sizeY, v[f[i][0]][2] * _sizeZ);
178  p2 = dart::collision::fcl::Vector3(
179  v[f[i][1]][0] * _sizeX, v[f[i][1]][1] * _sizeY, v[f[i][1]][2] * _sizeZ);
180  p3 = dart::collision::fcl::Vector3(
181  v[f[i][2]][0] * _sizeX, v[f[i][2]][1] * _sizeY, v[f[i][2]][2] * _sizeZ);
182  p1 = dart::collision::fcl::transform(_transform, p1);
183  p2 = dart::collision::fcl::transform(_transform, p2);
184  p3 = dart::collision::fcl::transform(_transform, p3);
185  model->addTriangle(p1, p2, p3);
186  }
187  model->endModel();
188  return model;
189 }
190 
191 // Create a cube mesh for collision detection
192 template <class BV>
193 ::fcl::BVHModel<BV>* createCube(
194  float _sizeX,
195  float _sizeY,
196  float _sizeZ,
197  const dart::collision::fcl::Transform3& _transform)
198 {
199  // float n[6][3] = {
200  // {-1.0, 0.0, 0.0},
201  // {0.0, 1.0, 0.0},
202  // {1.0, 0.0, 0.0},
203  // {0.0, -1.0, 0.0},
204  // {0.0, 0.0, 1.0},
205  // {0.0, 0.0, -1.0}
206  // };
207  int faces[6][4] = {{0, 1, 2, 3},
208  {3, 2, 6, 7},
209  {7, 6, 5, 4},
210  {4, 5, 1, 0},
211  {5, 6, 2, 1},
212  {7, 4, 0, 3}};
213  float v[8][3];
214 
215  v[0][0] = v[1][0] = v[2][0] = v[3][0] = -_sizeX / 2;
216  v[4][0] = v[5][0] = v[6][0] = v[7][0] = _sizeX / 2;
217  v[0][1] = v[1][1] = v[4][1] = v[5][1] = -_sizeY / 2;
218  v[2][1] = v[3][1] = v[6][1] = v[7][1] = _sizeY / 2;
219  v[0][2] = v[3][2] = v[4][2] = v[7][2] = -_sizeZ / 2;
220  v[1][2] = v[2][2] = v[5][2] = v[6][2] = _sizeZ / 2;
221 
222  ::fcl::BVHModel<BV>* model = new ::fcl::BVHModel<BV>;
223  dart::collision::fcl::Vector3 p1, p2, p3;
224  model->beginModel();
225 
226  for (int i = 0; i < 6; i++)
227  {
228  p1 = dart::collision::fcl::Vector3(
229  v[faces[i][0]][0], v[faces[i][0]][1], v[faces[i][0]][2]);
230  p2 = dart::collision::fcl::Vector3(
231  v[faces[i][1]][0], v[faces[i][1]][1], v[faces[i][1]][2]);
232  p3 = dart::collision::fcl::Vector3(
233  v[faces[i][2]][0], v[faces[i][2]][1], v[faces[i][2]][2]);
234  p1 = dart::collision::fcl::transform(_transform, p1);
235  p2 = dart::collision::fcl::transform(_transform, p2);
236  p3 = dart::collision::fcl::transform(_transform, p3);
237  model->addTriangle(p1, p2, p3);
238  p1 = dart::collision::fcl::Vector3(
239  v[faces[i][0]][0], v[faces[i][0]][1], v[faces[i][0]][2]);
240  p2 = dart::collision::fcl::Vector3(
241  v[faces[i][2]][0], v[faces[i][2]][1], v[faces[i][2]][2]);
242  p3 = dart::collision::fcl::Vector3(
243  v[faces[i][3]][0], v[faces[i][3]][1], v[faces[i][3]][2]);
244  p1 = dart::collision::fcl::transform(_transform, p1);
245  p2 = dart::collision::fcl::transform(_transform, p2);
246  p3 = dart::collision::fcl::transform(_transform, p3);
247  model->addTriangle(p1, p2, p3);
248  }
249  model->endModel();
250  return model;
251 }
252 
253 template <class BV>
254 ::fcl::BVHModel<BV>* createCylinder(
255  double _baseRadius,
256  double _topRadius,
257  double _height,
258  int _slices,
259  int _stacks,
260  const dart::collision::fcl::Transform3& _transform)
261 {
262  const int CACHE_SIZE = 240;
263 
264  int i, j;
265  float sinCache[CACHE_SIZE];
266  float cosCache[CACHE_SIZE];
267  float angle;
268  float zBase;
269  float zLow, zHigh;
270  float sintemp, costemp;
271  float deltaRadius;
272  float radiusLow, radiusHigh;
273 
274  if (_slices >= CACHE_SIZE)
275  _slices = CACHE_SIZE - 1;
276 
277  if (_slices < 2 || _stacks < 1 || _baseRadius < 0.0 || _topRadius < 0.0
278  || _height < 0.0)
279  {
280  return nullptr;
281  }
282 
283  /* Center at CoM */
284  zBase = -_height / 2;
285 
286  /* Compute delta */
287  deltaRadius = _baseRadius - _topRadius;
288 
289  /* Cache is the vertex locations cache */
290  for (i = 0; i < _slices; i++)
291  {
292  angle = 2 * math::constantsd::pi() * i / _slices;
293  sinCache[i] = sin(angle);
294  cosCache[i] = cos(angle);
295  }
296 
297  sinCache[_slices] = sinCache[0];
298  cosCache[_slices] = cosCache[0];
299 
300  ::fcl::BVHModel<BV>* model = new ::fcl::BVHModel<BV>;
301  dart::collision::fcl::Vector3 p1, p2, p3, p4;
302 
303  model->beginModel();
304 
305  /* Base of cylinder */
306  sintemp = sinCache[0];
307  costemp = cosCache[0];
308  radiusLow = _baseRadius;
309  zLow = zBase;
310  p1 = dart::collision::fcl::Vector3(
311  radiusLow * sintemp, radiusLow * costemp, zLow);
312  p1 = dart::collision::fcl::transform(_transform, p1);
313  for (i = 1; i < _slices; i++)
314  {
315  p2 = dart::collision::fcl::Vector3(
316  radiusLow * sinCache[i], radiusLow * cosCache[i], zLow);
317  p3 = dart::collision::fcl::Vector3(
318  radiusLow * sinCache[i + 1], radiusLow * cosCache[i + 1], zLow);
319  p2 = dart::collision::fcl::transform(_transform, p2);
320  p3 = dart::collision::fcl::transform(_transform, p3);
321  model->addTriangle(p1, p2, p3);
322 
323  Eigen::Vector3d v(radiusLow * sinCache[i], radiusLow * cosCache[i], zLow);
324  }
325 
326  /* Body of cylinder */
327  for (i = 0; i < _slices; i++)
328  {
329  for (j = 0; j < _stacks; j++)
330  {
331  zLow = j * _height / _stacks + zBase;
332  zHigh = (j + 1) * _height / _stacks + zBase;
333  radiusLow = _baseRadius - deltaRadius * (static_cast<float>(j) / _stacks);
334  radiusHigh
335  = _baseRadius - deltaRadius * (static_cast<float>(j + 1) / _stacks);
336 
337  p1 = dart::collision::fcl::Vector3(
338  radiusLow * sinCache[i], radiusLow * cosCache[i], zLow);
339  p2 = dart::collision::fcl::Vector3(
340  radiusLow * sinCache[i + 1], radiusLow * cosCache[i + 1], zLow);
341  p3 = dart::collision::fcl::Vector3(
342  radiusHigh * sinCache[i], radiusHigh * cosCache[i], zHigh);
343  p4 = dart::collision::fcl::Vector3(
344  radiusHigh * sinCache[i + 1], radiusHigh * cosCache[i + 1], zHigh);
345  p1 = dart::collision::fcl::transform(_transform, p1);
346  p2 = dart::collision::fcl::transform(_transform, p2);
347  p3 = dart::collision::fcl::transform(_transform, p3);
348  p4 = dart::collision::fcl::transform(_transform, p4);
349 
350  model->addTriangle(p1, p2, p3);
351  model->addTriangle(p2, p3, p4);
352  }
353  }
354 
355  /* Top of cylinder */
356  sintemp = sinCache[0];
357  costemp = cosCache[0];
358  radiusLow = _topRadius;
359  zLow = zBase + _height;
360  p1 = dart::collision::fcl::Vector3(
361  radiusLow * sintemp, radiusLow * costemp, zLow);
362  p1 = dart::collision::fcl::transform(_transform, p1);
363  for (i = 1; i < _slices; i++)
364  {
365  p2 = dart::collision::fcl::Vector3(
366  radiusLow * sinCache[i], radiusLow * cosCache[i], zLow);
367  p3 = dart::collision::fcl::Vector3(
368  radiusLow * sinCache[i + 1], radiusLow * cosCache[i + 1], zLow);
369  p2 = dart::collision::fcl::transform(_transform, p2);
370  p3 = dart::collision::fcl::transform(_transform, p3);
371  model->addTriangle(p1, p2, p3);
372  }
373 
374  model->endModel();
375  return model;
376 }
377 
378 } // namespace collision
379 } // namespace dart
380 
381 #endif // DART_COLLISION_FCL_MESH_COLLISIONSHAPES_HPP_
dart::collision::fcl::Vector3
::fcl::Vec3f Vector3
Definition: BackwardCompatibility.hpp:148
dart::collision::fcl::transform
dart::collision::fcl::Vector3 transform(const dart::collision::fcl::Transform3 &t, const dart::collision::fcl::Vector3 &v)
Transforms a 3-dim vector by a transform and returns the result.
Definition: BackwardCompatibility.cpp:133
dart::collision::fcl::Transform3
::fcl::Transform3f Transform3
Definition: BackwardCompatibility.hpp:150
dart::collision::createMesh
::fcl::BVHModel< BV > * createMesh(float _scaleX, float _scaleY, float _scaleZ, const aiScene *_mesh, const dart::collision::fcl::Transform3 &_transform)
Definition: CollisionShapes.hpp:46
dart::collision::createCylinder
::fcl::BVHModel< BV > * createCylinder(double _baseRadius, double _topRadius, double _height, int _slices, int _stacks, const dart::collision::fcl::Transform3 &_transform)
Definition: CollisionShapes.hpp:254
dart::collision::createCube
::fcl::BVHModel< BV > * createCube(float _sizeX, float _sizeY, float _sizeZ, const dart::collision::fcl::Transform3 &_transform)
Definition: CollisionShapes.hpp:193
dart::collision::createEllipsoid
::fcl::BVHModel< BV > * createEllipsoid(float _sizeX, float _sizeY, float _sizeZ, const dart::collision::fcl::Transform3 &_transform)
Definition: CollisionShapes.hpp:80
dart
Definition: BulletCollisionDetector.cpp:65
dart::math::constants::pi
static constexpr T pi()
Definition: Constants.hpp:45