| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // Copyright (c) 2017, Joseph Mirabel | ||
| 2 | // Authors: Joseph Mirabel (joseph.mirabel@laas.fr) | ||
| 3 | // | ||
| 4 | // This file is part of Coal. | ||
| 5 | // Coal 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 | // Coal 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 | // Coal. If not, see <http://www.gnu.org/licenses/>. | ||
| 16 | |||
| 17 | #include "coal/collision_utility.h" | ||
| 18 | #include "coal/BVH/BVH_utility.h" | ||
| 19 | |||
| 20 | namespace coal { | ||
| 21 | namespace details { | ||
| 22 | |||
| 23 | template <typename NT> | ||
| 24 | ✗ | inline CollisionGeometry* extractBVHtpl(const CollisionGeometry* model, | |
| 25 | const Transform3s& pose, | ||
| 26 | const AABB& aabb) { | ||
| 27 | // Ensure AABB is already computed | ||
| 28 | ✗ | if (model->aabb_radius < 0) | |
| 29 | ✗ | COAL_THROW_PRETTY("Collision geometry AABB should be computed first.", | |
| 30 | std::invalid_argument); | ||
| 31 | ✗ | AABB objAabb = rotate(translate(model->aabb_local, pose.getTranslation()), | |
| 32 | pose.getRotation()); | ||
| 33 | ✗ | if (!objAabb.overlap(aabb)) { | |
| 34 | // No intersection. | ||
| 35 | ✗ | return nullptr; | |
| 36 | } | ||
| 37 | ✗ | const BVHModel<NT>* m = static_cast<const BVHModel<NT>*>(model); | |
| 38 | ✗ | return BVHExtract(*m, pose, aabb); | |
| 39 | } | ||
| 40 | |||
| 41 | ✗ | CollisionGeometry* extractBVH(const CollisionGeometry* model, | |
| 42 | const Transform3s& pose, const AABB& aabb) { | ||
| 43 | ✗ | switch (model->getNodeType()) { | |
| 44 | ✗ | case BV_AABB: | |
| 45 | ✗ | return extractBVHtpl<AABB>(model, pose, aabb); | |
| 46 | ✗ | case BV_OBB: | |
| 47 | ✗ | return extractBVHtpl<OBB>(model, pose, aabb); | |
| 48 | ✗ | case BV_RSS: | |
| 49 | ✗ | return extractBVHtpl<RSS>(model, pose, aabb); | |
| 50 | ✗ | case BV_kIOS: | |
| 51 | ✗ | return extractBVHtpl<kIOS>(model, pose, aabb); | |
| 52 | ✗ | case BV_OBBRSS: | |
| 53 | ✗ | return extractBVHtpl<OBBRSS>(model, pose, aabb); | |
| 54 | ✗ | case BV_KDOP16: | |
| 55 | ✗ | return extractBVHtpl<KDOP<16> >(model, pose, aabb); | |
| 56 | ✗ | case BV_KDOP18: | |
| 57 | ✗ | return extractBVHtpl<KDOP<18> >(model, pose, aabb); | |
| 58 | ✗ | case BV_KDOP24: | |
| 59 | ✗ | return extractBVHtpl<KDOP<24> >(model, pose, aabb); | |
| 60 | ✗ | default: | |
| 61 | ✗ | COAL_THROW_PRETTY("Unknown type of bounding volume", std::runtime_error); | |
| 62 | } | ||
| 63 | } | ||
| 64 | } // namespace details | ||
| 65 | |||
| 66 | ✗ | CollisionGeometry* extract(const CollisionGeometry* model, | |
| 67 | const Transform3s& pose, const AABB& aabb) { | ||
| 68 | ✗ | switch (model->getObjectType()) { | |
| 69 | ✗ | case OT_BVH: | |
| 70 | ✗ | return details::extractBVH(model, pose, aabb); | |
| 71 | // case OT_GEOM: return model; | ||
| 72 | ✗ | default: | |
| 73 | ✗ | COAL_THROW_PRETTY("Extraction is not implemented for this type of object", | |
| 74 | std::runtime_error); | ||
| 75 | } | ||
| 76 | } | ||
| 77 | } // namespace coal | ||
| 78 |