DART  6.7.3
CollisionShapes.hpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2011-2019, 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 
41 namespace dart {
42 namespace collision {
43 
44 template<class BV>
45 ::fcl::BVHModel<BV>* createMesh(float _scaleX, float _scaleY, float _scaleZ,
46  const aiScene* _mesh,
47  const dart::collision::fcl::Transform3& _transform) {
48  assert(_mesh);
49  ::fcl::BVHModel<BV>* model = new ::fcl::BVHModel<BV>;
50  model->beginModel();
51 
52  for (unsigned int i = 0; i < _mesh->mNumMeshes; i++) {
53  for (unsigned int j = 0; j < _mesh->mMeshes[i]->mNumFaces; j++) {
54  dart::collision::fcl::Vector3 vertices[3];
55  for (unsigned int k = 0; k < 3; k++) {
56  const aiVector3D& vertex
57  = _mesh->mMeshes[i]->mVertices[
58  _mesh->mMeshes[i]->mFaces[j].mIndices[k]];
59  vertices[k] = dart::collision::fcl::Vector3(vertex.x * _scaleX,
60  vertex.y * _scaleY,
61  vertex.z * _scaleZ);
62  vertices[k] = dart::collision::fcl::transform(_transform, vertices[k]);
63  }
64  model->addTriangle(vertices[0], vertices[1], vertices[2]);
65  }
66  }
67 
68  model->endModel();
69  return model;
70 }
71 
72 template<class BV>
73 ::fcl::BVHModel<BV>* createEllipsoid(float _sizeX, float _sizeY, float _sizeZ,
74  const dart::collision::fcl::Transform3& _transform) {
75  float v[59][3] = {
76  {0, 0, 0},
77  {0.135299, -0.461940, -0.135299},
78  {0.000000, -0.461940, -0.191342},
79  {-0.135299, -0.461940, -0.135299},
80  {-0.191342, -0.461940, 0.000000},
81  {-0.135299, -0.461940, 0.135299},
82  {0.000000, -0.461940, 0.191342},
83  {0.135299, -0.461940, 0.135299},
84  {0.191342, -0.461940, 0.000000},
85  {0.250000, -0.353553, -0.250000},
86  {0.000000, -0.353553, -0.353553},
87  {-0.250000, -0.353553, -0.250000},
88  {-0.353553, -0.353553, 0.000000},
89  {-0.250000, -0.353553, 0.250000},
90  {0.000000, -0.353553, 0.353553},
91  {0.250000, -0.353553, 0.250000},
92  {0.353553, -0.353553, 0.000000},
93  {0.326641, -0.191342, -0.326641},
94  {0.000000, -0.191342, -0.461940},
95  {-0.326641, -0.191342, -0.326641},
96  {-0.461940, -0.191342, 0.000000},
97  {-0.326641, -0.191342, 0.326641},
98  {0.000000, -0.191342, 0.461940},
99  {0.326641, -0.191342, 0.326641},
100  {0.461940, -0.191342, 0.000000},
101  {0.353553, 0.000000, -0.353553},
102  {0.000000, 0.000000, -0.500000},
103  {-0.353553, 0.000000, -0.353553},
104  {-0.500000, 0.000000, 0.000000},
105  {-0.353553, 0.000000, 0.353553},
106  {0.000000, 0.000000, 0.500000},
107  {0.353553, 0.000000, 0.353553},
108  {0.500000, 0.000000, 0.000000},
109  {0.326641, 0.191342, -0.326641},
110  {0.000000, 0.191342, -0.461940},
111  {-0.326641, 0.191342, -0.326641},
112  {-0.461940, 0.191342, 0.000000},
113  {-0.326641, 0.191342, 0.326641},
114  {0.000000, 0.191342, 0.461940},
115  {0.326641, 0.191342, 0.326641},
116  {0.461940, 0.191342, 0.000000},
117  {0.250000, 0.353553, -0.250000},
118  {0.000000, 0.353553, -0.353553},
119  {-0.250000, 0.353553, -0.250000},
120  {-0.353553, 0.353553, 0.000000},
121  {-0.250000, 0.353553, 0.250000},
122  {0.000000, 0.353553, 0.353553},
123  {0.250000, 0.353553, 0.250000},
124  {0.353553, 0.353553, 0.000000},
125  {0.135299, 0.461940, -0.135299},
126  {0.000000, 0.461940, -0.191342},
127  {-0.135299, 0.461940, -0.135299},
128  {-0.191342, 0.461940, 0.000000},
129  {-0.135299, 0.461940, 0.135299},
130  {0.000000, 0.461940, 0.191342},
131  {0.135299, 0.461940, 0.135299},
132  {0.191342, 0.461940, 0.000000},
133  {0.000000, -0.500000, 0.000000},
134  {0.000000, 0.500000, 0.000000}
135  };
136  int f[112][3] = {
137  {1, 2, 9},
138  {9, 2, 10},
139  {2, 3, 10},
140  {10, 3, 11},
141  {3, 4, 11},
142  {11, 4, 12},
143  {4, 5, 12},
144  {12, 5, 13},
145  {5, 6, 13},
146  {13, 6, 14},
147  {6, 7, 14},
148  {14, 7, 15},
149  {7, 8, 15},
150  {15, 8, 16},
151  {8, 1, 16},
152  {16, 1, 9},
153  {9, 10, 17},
154  {17, 10, 18},
155  {10, 11, 18},
156  {18, 11, 19},
157  {11, 12, 19},
158  {19, 12, 20},
159  {12, 13, 20},
160  {20, 13, 21},
161  {13, 14, 21},
162  {21, 14, 22},
163  {14, 15, 22},
164  {22, 15, 23},
165  {15, 16, 23},
166  {23, 16, 24},
167  {16, 9, 24},
168  {24, 9, 17},
169  {17, 18, 25},
170  {25, 18, 26},
171  {18, 19, 26},
172  {26, 19, 27},
173  {19, 20, 27},
174  {27, 20, 28},
175  {20, 21, 28},
176  {28, 21, 29},
177  {21, 22, 29},
178  {29, 22, 30},
179  {22, 23, 30},
180  {30, 23, 31},
181  {23, 24, 31},
182  {31, 24, 32},
183  {24, 17, 32},
184  {32, 17, 25},
185  {25, 26, 33},
186  {33, 26, 34},
187  {26, 27, 34},
188  {34, 27, 35},
189  {27, 28, 35},
190  {35, 28, 36},
191  {28, 29, 36},
192  {36, 29, 37},
193  {29, 30, 37},
194  {37, 30, 38},
195  {30, 31, 38},
196  {38, 31, 39},
197  {31, 32, 39},
198  {39, 32, 40},
199  {32, 25, 40},
200  {40, 25, 33},
201  {33, 34, 41},
202  {41, 34, 42},
203  {34, 35, 42},
204  {42, 35, 43},
205  {35, 36, 43},
206  {43, 36, 44},
207  {36, 37, 44},
208  {44, 37, 45},
209  {37, 38, 45},
210  {45, 38, 46},
211  {38, 39, 46},
212  {46, 39, 47},
213  {39, 40, 47},
214  {47, 40, 48},
215  {40, 33, 48},
216  {48, 33, 41},
217  {41, 42, 49},
218  {49, 42, 50},
219  {42, 43, 50},
220  {50, 43, 51},
221  {43, 44, 51},
222  {51, 44, 52},
223  {44, 45, 52},
224  {52, 45, 53},
225  {45, 46, 53},
226  {53, 46, 54},
227  {46, 47, 54},
228  {54, 47, 55},
229  {47, 48, 55},
230  {55, 48, 56},
231  {48, 41, 56},
232  {56, 41, 49},
233  {2, 1, 57},
234  {3, 2, 57},
235  {4, 3, 57},
236  {5, 4, 57},
237  {6, 5, 57},
238  {7, 6, 57},
239  {8, 7, 57},
240  {1, 8, 57},
241  {49, 50, 58},
242  {50, 51, 58},
243  {51, 52, 58},
244  {52, 53, 58},
245  {53, 54, 58},
246  {54, 55, 58},
247  {55, 56, 58},
248  {56, 49, 58}
249  };
250 
251  ::fcl::BVHModel<BV>* model = new ::fcl::BVHModel<BV>;
252  dart::collision::fcl::Vector3 p1, p2, p3;
253  model->beginModel();
254 
255  for (int i = 0; i < 112; i++) {
256  p1 = dart::collision::fcl::Vector3(v[f[i][0]][0] * _sizeX,
257  v[f[i][0]][1] * _sizeY,
258  v[f[i][0]][2] * _sizeZ);
259  p2 = dart::collision::fcl::Vector3(v[f[i][1]][0] * _sizeX,
260  v[f[i][1]][1] * _sizeY,
261  v[f[i][1]][2] * _sizeZ);
262  p3 = dart::collision::fcl::Vector3(v[f[i][2]][0] * _sizeX,
263  v[f[i][2]][1] * _sizeY,
264  v[f[i][2]][2] * _sizeZ);
265  p1 = dart::collision::fcl::transform(_transform, p1);
266  p2 = dart::collision::fcl::transform(_transform, p2);
267  p3 = dart::collision::fcl::transform(_transform, p3);
268  model->addTriangle(p1, p2, p3);
269  }
270  model->endModel();
271  return model;
272 }
273 
274 // Create a cube mesh for collision detection
275 template<class BV>
276 ::fcl::BVHModel<BV>* createCube(float _sizeX, float _sizeY, float _sizeZ,
277  const dart::collision::fcl::Transform3& _transform) {
278 // float n[6][3] = {
279 // {-1.0, 0.0, 0.0},
280 // {0.0, 1.0, 0.0},
281 // {1.0, 0.0, 0.0},
282 // {0.0, -1.0, 0.0},
283 // {0.0, 0.0, 1.0},
284 // {0.0, 0.0, -1.0}
285 // };
286  int faces[6][4] = {
287  {0, 1, 2, 3},
288  {3, 2, 6, 7},
289  {7, 6, 5, 4},
290  {4, 5, 1, 0},
291  {5, 6, 2, 1},
292  {7, 4, 0, 3}
293  };
294  float v[8][3];
295 
296  v[0][0] = v[1][0] = v[2][0] = v[3][0] = -_sizeX / 2;
297  v[4][0] = v[5][0] = v[6][0] = v[7][0] = _sizeX / 2;
298  v[0][1] = v[1][1] = v[4][1] = v[5][1] = -_sizeY / 2;
299  v[2][1] = v[3][1] = v[6][1] = v[7][1] = _sizeY / 2;
300  v[0][2] = v[3][2] = v[4][2] = v[7][2] = -_sizeZ / 2;
301  v[1][2] = v[2][2] = v[5][2] = v[6][2] = _sizeZ / 2;
302 
303  ::fcl::BVHModel<BV>* model = new ::fcl::BVHModel<BV>;
304  dart::collision::fcl::Vector3 p1, p2, p3;
305  model->beginModel();
306 
307  for (int i = 0; i < 6; i++) {
308  p1 = dart::collision::fcl::Vector3(v[faces[i][0]][0], v[faces[i][0]][1], v[faces[i][0]][2]);
309  p2 = dart::collision::fcl::Vector3(v[faces[i][1]][0], v[faces[i][1]][1], v[faces[i][1]][2]);
310  p3 = dart::collision::fcl::Vector3(v[faces[i][2]][0], v[faces[i][2]][1], v[faces[i][2]][2]);
311  p1 = dart::collision::fcl::transform(_transform, p1);
312  p2 = dart::collision::fcl::transform(_transform, p2);
313  p3 = dart::collision::fcl::transform(_transform, p3);
314  model->addTriangle(p1, p2, p3);
315  p1 = dart::collision::fcl::Vector3(v[faces[i][0]][0], v[faces[i][0]][1], v[faces[i][0]][2]);
316  p2 = dart::collision::fcl::Vector3(v[faces[i][2]][0], v[faces[i][2]][1], v[faces[i][2]][2]);
317  p3 = dart::collision::fcl::Vector3(v[faces[i][3]][0], v[faces[i][3]][1], v[faces[i][3]][2]);
318  p1 = dart::collision::fcl::transform(_transform, p1);
319  p2 = dart::collision::fcl::transform(_transform, p2);
320  p3 = dart::collision::fcl::transform(_transform, p3);
321  model->addTriangle(p1, p2, p3);
322  }
323  model->endModel();
324  return model;
325 }
326 
327 template<class BV>
328 ::fcl::BVHModel<BV>* createCylinder(double _baseRadius, double _topRadius,
329  double _height, int _slices, int _stacks,
330  const dart::collision::fcl::Transform3& _transform) {
331  const int CACHE_SIZE = 240;
332 
333  int i, j;
334  float sinCache[CACHE_SIZE];
335  float cosCache[CACHE_SIZE];
336  float angle;
337  float zBase;
338  float zLow, zHigh;
339  float sintemp, costemp;
340  float deltaRadius;
341  float radiusLow, radiusHigh;
342 
343  if (_slices >= CACHE_SIZE) _slices = CACHE_SIZE-1;
344 
345  if (_slices < 2 || _stacks < 1 || _baseRadius < 0.0 || _topRadius < 0.0 ||
346  _height < 0.0) {
347  return nullptr;
348  }
349 
350  /* Center at CoM */
351  zBase = -_height/2;
352 
353  /* Compute delta */
354  deltaRadius = _baseRadius - _topRadius;
355 
356  /* Cache is the vertex locations cache */
357  for (i = 0; i < _slices; i++) {
358  angle = 2 * M_PI * i / _slices;
359  sinCache[i] = sin(angle);
360  cosCache[i] = cos(angle);
361  }
362 
363  sinCache[_slices] = sinCache[0];
364  cosCache[_slices] = cosCache[0];
365 
366  ::fcl::BVHModel<BV>* model = new ::fcl::BVHModel<BV>;
367  dart::collision::fcl::Vector3 p1, p2, p3, p4;
368 
369  model->beginModel();
370 
371  /* Base of cylinder */
372  sintemp = sinCache[0];
373  costemp = cosCache[0];
374  radiusLow = _baseRadius;
375  zLow = zBase;
376  p1 = dart::collision::fcl::Vector3(radiusLow * sintemp, radiusLow * costemp, zLow);
377  p1 = dart::collision::fcl::transform(_transform, p1);
378  for (i = 1; i < _slices; i++) {
379  p2 = dart::collision::fcl::Vector3(radiusLow * sinCache[i], radiusLow * cosCache[i], zLow);
380  p3 = dart::collision::fcl::Vector3(radiusLow * sinCache[i+1], radiusLow * cosCache[i+1], zLow);
381  p2 = dart::collision::fcl::transform(_transform, p2);
382  p3 = dart::collision::fcl::transform(_transform, p3);
383  model->addTriangle(p1, p2, p3);
384 
385  Eigen::Vector3d v(radiusLow * sinCache[i], radiusLow * cosCache[i], zLow);
386  }
387 
388  /* Body of cylinder */
389  for (i = 0; i < _slices; i++) {
390  for (j = 0; j < _stacks; j++) {
391  zLow = j * _height / _stacks + zBase;
392  zHigh = (j + 1) * _height / _stacks + zBase;
393  radiusLow = _baseRadius
394  - deltaRadius * (static_cast<float>(j) / _stacks);
395  radiusHigh = _baseRadius
396  - deltaRadius * (static_cast<float>(j + 1) / _stacks);
397 
398  p1 = dart::collision::fcl::Vector3(radiusLow * sinCache[i], radiusLow * cosCache[i],
399  zLow);
400  p2 = dart::collision::fcl::Vector3(radiusLow * sinCache[i+1], radiusLow * cosCache[i+1],
401  zLow);
402  p3 = dart::collision::fcl::Vector3(radiusHigh * sinCache[i], radiusHigh * cosCache[i],
403  zHigh);
404  p4 = dart::collision::fcl::Vector3(radiusHigh * sinCache[i+1], radiusHigh * cosCache[i+1],
405  zHigh);
406  p1 = dart::collision::fcl::transform(_transform, p1);
407  p2 = dart::collision::fcl::transform(_transform, p2);
408  p3 = dart::collision::fcl::transform(_transform, p3);
409  p4 = dart::collision::fcl::transform(_transform, p4);
410 
411  model->addTriangle(p1, p2, p3);
412  model->addTriangle(p2, p3, p4);
413  }
414  }
415 
416  /* Top of cylinder */
417  sintemp = sinCache[0];
418  costemp = cosCache[0];
419  radiusLow = _topRadius;
420  zLow = zBase + _height;
421  p1 = dart::collision::fcl::Vector3(radiusLow * sintemp, radiusLow * costemp, zLow);
422  p1 = dart::collision::fcl::transform(_transform, p1);
423  for (i = 1; i < _slices; i++) {
424  p2 = dart::collision::fcl::Vector3(radiusLow * sinCache[i], radiusLow * cosCache[i], zLow);
425  p3 = dart::collision::fcl::Vector3(radiusLow * sinCache[i+1], radiusLow * cosCache[i+1], zLow);
426  p2 = dart::collision::fcl::transform(_transform, p2);
427  p3 = dart::collision::fcl::transform(_transform, p3);
428  model->addTriangle(p1, p2, p3);
429  }
430 
431  model->endModel();
432  return model;
433 }
434 
435 } // namespace collision
436 } // namespace dart
437 
438 #endif // DART_COLLISION_FCL_MESH_COLLISIONSHAPES_HPP_
dart::collision::fcl::Vector3
::fcl::Vec3f Vector3
Definition: BackwardCompatibility.hpp:140
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:131
dart::collision::fcl::Transform3
::fcl::Transform3f Transform3
Definition: BackwardCompatibility.hpp:142
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:45
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:328
dart::collision::createCube
::fcl::BVHModel< BV > * createCube(float _sizeX, float _sizeY, float _sizeZ, const dart::collision::fcl::Transform3 &_transform)
Definition: CollisionShapes.hpp:276
dart::collision::createEllipsoid
::fcl::BVHModel< BV > * createEllipsoid(float _sizeX, float _sizeY, float _sizeZ, const dart::collision::fcl::Transform3 &_transform)
Definition: CollisionShapes.hpp:73
dart
Definition: BulletCollisionDetector.cpp:63