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