DART 6.7.3
Loading...
Searching...
No Matches
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
41namespace dart {
42namespace collision {
43
44template<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
72template<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
275template<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
327template<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::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::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
Definition BulletCollisionDetector.cpp:63