1 |
|
|
// Copyright (c) 2017, Joseph Mirabel |
2 |
|
|
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr) |
3 |
|
|
// |
4 |
|
|
// This file is part of hpp-fcl. |
5 |
|
|
// hpp-fcl is free software: you can redistribute it |
6 |
|
|
// and/or modify it under the terms of the GNU Lesser General Public |
7 |
|
|
// License as published by the Free Software Foundation, either version |
8 |
|
|
// 3 of the License, or (at your option) any later version. |
9 |
|
|
// |
10 |
|
|
// hpp-fcl is distributed in the hope that it will be |
11 |
|
|
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty |
12 |
|
|
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU |
13 |
|
|
// General Lesser Public License for more details. You should have |
14 |
|
|
// received a copy of the GNU Lesser General Public License along with |
15 |
|
|
// hpp-fcl. If not, see <http://www.gnu.org/licenses/>. |
16 |
|
|
|
17 |
|
|
#include <hpp/fcl/collision_utility.h> |
18 |
|
|
|
19 |
|
|
#include <hpp/fcl/BVH/BVH_utility.h> |
20 |
|
|
|
21 |
|
|
namespace hpp { |
22 |
|
|
namespace fcl { |
23 |
|
|
namespace details { |
24 |
|
|
|
25 |
|
|
template <typename NT> |
26 |
|
|
inline CollisionGeometry* extractBVHtpl(const CollisionGeometry* model, |
27 |
|
|
const Transform3f& pose, |
28 |
|
|
const AABB& aabb) { |
29 |
|
|
// Ensure AABB is already computed |
30 |
|
|
if (model->aabb_radius < 0) |
31 |
|
|
HPP_FCL_THROW_PRETTY("Collision geometry AABB should be computed first.", |
32 |
|
|
std::invalid_argument); |
33 |
|
|
AABB objAabb = rotate(translate(model->aabb_local, pose.getTranslation()), |
34 |
|
|
pose.getRotation()); |
35 |
|
|
if (!objAabb.overlap(aabb)) { |
36 |
|
|
// No intersection. |
37 |
|
|
return nullptr; |
38 |
|
|
} |
39 |
|
|
const BVHModel<NT>* m = static_cast<const BVHModel<NT>*>(model); |
40 |
|
|
return BVHExtract(*m, pose, aabb); |
41 |
|
|
} |
42 |
|
|
|
43 |
|
|
CollisionGeometry* extractBVH(const CollisionGeometry* model, |
44 |
|
|
const Transform3f& pose, const AABB& aabb) { |
45 |
|
|
switch (model->getNodeType()) { |
46 |
|
|
case BV_AABB: |
47 |
|
|
return extractBVHtpl<AABB>(model, pose, aabb); |
48 |
|
|
case BV_OBB: |
49 |
|
|
return extractBVHtpl<OBB>(model, pose, aabb); |
50 |
|
|
case BV_RSS: |
51 |
|
|
return extractBVHtpl<RSS>(model, pose, aabb); |
52 |
|
|
case BV_kIOS: |
53 |
|
|
return extractBVHtpl<kIOS>(model, pose, aabb); |
54 |
|
|
case BV_OBBRSS: |
55 |
|
|
return extractBVHtpl<OBBRSS>(model, pose, aabb); |
56 |
|
|
case BV_KDOP16: |
57 |
|
|
return extractBVHtpl<KDOP<16> >(model, pose, aabb); |
58 |
|
|
case BV_KDOP18: |
59 |
|
|
return extractBVHtpl<KDOP<18> >(model, pose, aabb); |
60 |
|
|
case BV_KDOP24: |
61 |
|
|
return extractBVHtpl<KDOP<24> >(model, pose, aabb); |
62 |
|
|
default: |
63 |
|
|
throw std::runtime_error("Unknown type of bounding volume"); |
64 |
|
|
} |
65 |
|
|
} |
66 |
|
|
} // namespace details |
67 |
|
|
|
68 |
|
|
CollisionGeometry* extract(const CollisionGeometry* model, |
69 |
|
|
const Transform3f& pose, const AABB& aabb) { |
70 |
|
|
switch (model->getObjectType()) { |
71 |
|
|
case OT_BVH: |
72 |
|
|
return details::extractBVH(model, pose, aabb); |
73 |
|
|
// case OT_GEOM: return model; |
74 |
|
|
default: |
75 |
|
|
throw std::runtime_error( |
76 |
|
|
"Extraction is not implemented for this type of object"); |
77 |
|
|
} |
78 |
|
|
} |
79 |
|
|
} // namespace fcl |
80 |
|
|
|
81 |
|
|
} // namespace hpp |