| Directory: | ./ |
|---|---|
| File: | include/coal/BVH/BVH_model.h |
| Date: | 2025-04-13 14:25:19 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 26 | 113 | 23.0% |
| Branches: | 20 | 178 | 11.2% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | /* | ||
| 2 | * Software License Agreement (BSD License) | ||
| 3 | * | ||
| 4 | * Copyright (c) 2011-2014, Willow Garage, Inc. | ||
| 5 | * Copyright (c) 2014-2015, Open Source Robotics Foundation | ||
| 6 | * Copyright (c) 2020-2022, INRIA | ||
| 7 | * All rights reserved. | ||
| 8 | * | ||
| 9 | * Redistribution and use in source and binary forms, with or without | ||
| 10 | * modification, are permitted provided that the following conditions | ||
| 11 | * are met: | ||
| 12 | * | ||
| 13 | * * Redistributions of source code must retain the above copyright | ||
| 14 | * notice, this list of conditions and the following disclaimer. | ||
| 15 | * * Redistributions in binary form must reproduce the above | ||
| 16 | * copyright notice, this list of conditions and the following | ||
| 17 | * disclaimer in the documentation and/or other materials provided | ||
| 18 | * with the distribution. | ||
| 19 | * * Neither the name of Open Source Robotics Foundation nor the names of its | ||
| 20 | * contributors may be used to endorse or promote products derived | ||
| 21 | * from this software without specific prior written permission. | ||
| 22 | * | ||
| 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
| 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT | ||
| 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS | ||
| 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE | ||
| 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, | ||
| 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, | ||
| 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; | ||
| 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER | ||
| 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT | ||
| 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN | ||
| 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE | ||
| 34 | * POSSIBILITY OF SUCH DAMAGE. | ||
| 35 | */ | ||
| 36 | |||
| 37 | /** \author Jia Pan */ | ||
| 38 | |||
| 39 | #ifndef COAL_BVH_MODEL_H | ||
| 40 | #define COAL_BVH_MODEL_H | ||
| 41 | |||
| 42 | #include "coal/fwd.hh" | ||
| 43 | #include "coal/collision_object.h" | ||
| 44 | #include "coal/BVH/BVH_internal.h" | ||
| 45 | #include "coal/BV/BV_node.h" | ||
| 46 | |||
| 47 | #include <vector> | ||
| 48 | #include <memory> | ||
| 49 | #include <iostream> | ||
| 50 | |||
| 51 | namespace coal { | ||
| 52 | |||
| 53 | /// @addtogroup Construction_Of_BVH | ||
| 54 | /// @{ | ||
| 55 | |||
| 56 | class ConvexBase; | ||
| 57 | |||
| 58 | template <typename BV> | ||
| 59 | class BVFitter; | ||
| 60 | template <typename BV> | ||
| 61 | class BVSplitter; | ||
| 62 | |||
| 63 | /// @brief A base class describing the bounding hierarchy of a mesh model or a | ||
| 64 | /// point cloud model (which is viewed as a degraded version of mesh) | ||
| 65 | class COAL_DLLAPI BVHModelBase : public CollisionGeometry { | ||
| 66 | public: | ||
| 67 | /// @brief Geometry point data | ||
| 68 | std::shared_ptr<std::vector<Vec3s>> vertices; | ||
| 69 | |||
| 70 | /// @brief Geometry triangle index data, will be NULL for point clouds | ||
| 71 | std::shared_ptr<std::vector<Triangle>> tri_indices; | ||
| 72 | |||
| 73 | /// @brief Geometry point data in previous frame | ||
| 74 | std::shared_ptr<std::vector<Vec3s>> prev_vertices; | ||
| 75 | |||
| 76 | /// @brief Number of triangles | ||
| 77 | unsigned int num_tris; | ||
| 78 | |||
| 79 | /// @brief Number of points | ||
| 80 | unsigned int num_vertices; | ||
| 81 | |||
| 82 | /// @brief The state of BVH building process | ||
| 83 | BVHBuildState build_state; | ||
| 84 | |||
| 85 | /// @brief Convex<Triangle> representation of this object | ||
| 86 | shared_ptr<ConvexBase> convex; | ||
| 87 | |||
| 88 | /// @brief Model type described by the instance | ||
| 89 | 2347550 | BVHModelType getModelType() const { | |
| 90 |
3/4✓ Branch 0 taken 2347406 times.
✓ Branch 1 taken 144 times.
✓ Branch 2 taken 2347406 times.
✗ Branch 3 not taken.
|
2347550 | if (num_tris && num_vertices) |
| 91 | 2347406 | return BVH_MODEL_TRIANGLES; | |
| 92 |
1/2✓ Branch 0 taken 144 times.
✗ Branch 1 not taken.
|
144 | else if (num_vertices) |
| 93 | 144 | return BVH_MODEL_POINTCLOUD; | |
| 94 | else | ||
| 95 | ✗ | return BVH_MODEL_UNKNOWN; | |
| 96 | } | ||
| 97 | |||
| 98 | /// @brief Constructing an empty BVH | ||
| 99 | BVHModelBase(); | ||
| 100 | |||
| 101 | /// @brief copy from another BVH | ||
| 102 | BVHModelBase(const BVHModelBase& other); | ||
| 103 | |||
| 104 | /// @brief deconstruction, delete mesh data related. | ||
| 105 | 6176 | virtual ~BVHModelBase() {} | |
| 106 | |||
| 107 | /// @brief Get the object type: it is a BVH | ||
| 108 | 35014 | OBJECT_TYPE getObjectType() const { return OT_BVH; } | |
| 109 | |||
| 110 | /// @brief Compute the AABB for the BVH, used for broad-phase collision | ||
| 111 | void computeLocalAABB(); | ||
| 112 | |||
| 113 | /// @brief Begin a new BVH model | ||
| 114 | int beginModel(unsigned int num_tris = 0, unsigned int num_vertices = 0); | ||
| 115 | |||
| 116 | /// @brief Add one point in the new BVH model | ||
| 117 | int addVertex(const Vec3s& p); | ||
| 118 | |||
| 119 | /// @brief Add points in the new BVH model | ||
| 120 | int addVertices(const MatrixX3s& points); | ||
| 121 | |||
| 122 | /// @brief Add triangles in the new BVH model | ||
| 123 | int addTriangles(const Matrixx3i& triangles); | ||
| 124 | |||
| 125 | /// @brief Add one triangle in the new BVH model | ||
| 126 | int addTriangle(const Vec3s& p1, const Vec3s& p2, const Vec3s& p3); | ||
| 127 | |||
| 128 | /// @brief Add a set of triangles in the new BVH model | ||
| 129 | int addSubModel(const std::vector<Vec3s>& ps, | ||
| 130 | const std::vector<Triangle>& ts); | ||
| 131 | |||
| 132 | /// @brief Add a set of points in the new BVH model | ||
| 133 | int addSubModel(const std::vector<Vec3s>& ps); | ||
| 134 | |||
| 135 | /// @brief End BVH model construction, will build the bounding volume | ||
| 136 | /// hierarchy | ||
| 137 | int endModel(); | ||
| 138 | |||
| 139 | /// @brief Replace the geometry information of current frame (i.e. should have | ||
| 140 | /// the same mesh topology with the previous frame) | ||
| 141 | int beginReplaceModel(); | ||
| 142 | |||
| 143 | /// @brief Replace one point in the old BVH model | ||
| 144 | int replaceVertex(const Vec3s& p); | ||
| 145 | |||
| 146 | /// @brief Replace one triangle in the old BVH model | ||
| 147 | int replaceTriangle(const Vec3s& p1, const Vec3s& p2, const Vec3s& p3); | ||
| 148 | |||
| 149 | /// @brief Replace a set of points in the old BVH model | ||
| 150 | int replaceSubModel(const std::vector<Vec3s>& ps); | ||
| 151 | |||
| 152 | /// @brief End BVH model replacement, will also refit or rebuild the bounding | ||
| 153 | /// volume hierarchy | ||
| 154 | int endReplaceModel(bool refit = true, bool bottomup = true); | ||
| 155 | |||
| 156 | /// @brief Replace the geometry information of current frame (i.e. should have | ||
| 157 | /// the same mesh topology with the previous frame). The current frame will be | ||
| 158 | /// saved as the previous frame in prev_vertices. | ||
| 159 | int beginUpdateModel(); | ||
| 160 | |||
| 161 | /// @brief Update one point in the old BVH model | ||
| 162 | int updateVertex(const Vec3s& p); | ||
| 163 | |||
| 164 | /// @brief Update one triangle in the old BVH model | ||
| 165 | int updateTriangle(const Vec3s& p1, const Vec3s& p2, const Vec3s& p3); | ||
| 166 | |||
| 167 | /// @brief Update a set of points in the old BVH model | ||
| 168 | int updateSubModel(const std::vector<Vec3s>& ps); | ||
| 169 | |||
| 170 | /// @brief End BVH model update, will also refit or rebuild the bounding | ||
| 171 | /// volume hierarchy | ||
| 172 | int endUpdateModel(bool refit = true, bool bottomup = true); | ||
| 173 | |||
| 174 | /// @brief Build this \ref Convex "Convex<Triangle>" representation of this | ||
| 175 | /// model. The result is stored in attribute \ref convex. \note this only | ||
| 176 | /// takes the points of this model. It does not check that the | ||
| 177 | /// object is convex. It does not compute a convex hull. | ||
| 178 | void buildConvexRepresentation(bool share_memory); | ||
| 179 | |||
| 180 | /// @brief Build a convex hull | ||
| 181 | /// and store it in attribute \ref convex. | ||
| 182 | /// \param keepTriangle whether the convex should be triangulated. | ||
| 183 | /// \param qhullCommand see \ref ConvexBase::convexHull. | ||
| 184 | /// \return \c true if this object is convex, hence the convex hull represents | ||
| 185 | /// the same object. | ||
| 186 | /// \sa ConvexBase::convexHull | ||
| 187 | /// \warning At the moment, the return value only checks whether there are as | ||
| 188 | /// many points in the convex hull as in the original object. This is | ||
| 189 | /// neither necessary (duplicated vertices get merged) nor sufficient | ||
| 190 | /// (think of a U with 4 vertices and 3 edges). | ||
| 191 | bool buildConvexHull(bool keepTriangle, const char* qhullCommand = NULL); | ||
| 192 | |||
| 193 | virtual int memUsage(const bool msg = false) const = 0; | ||
| 194 | |||
| 195 | /// @brief This is a special acceleration: BVH_model default stores the BV's | ||
| 196 | /// transform in world coordinate. However, we can also store each BV's | ||
| 197 | /// transform related to its parent BV node. When traversing the BVH, this can | ||
| 198 | /// save one matrix transformation. | ||
| 199 | virtual void makeParentRelative() = 0; | ||
| 200 | |||
| 201 | ✗ | Vec3s computeCOM() const { | |
| 202 | ✗ | Scalar vol = 0; | |
| 203 | ✗ | Vec3s com(0, 0, 0); | |
| 204 | ✗ | if (!(vertices.get())) { | |
| 205 | std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain " | ||
| 206 | ✗ | "vertices." | |
| 207 | ✗ | << std::endl; | |
| 208 | ✗ | return com; | |
| 209 | } | ||
| 210 | ✗ | const std::vector<Vec3s>& vertices_ = *vertices; | |
| 211 | ✗ | if (!(tri_indices.get())) { | |
| 212 | std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain " | ||
| 213 | ✗ | "triangles." | |
| 214 | ✗ | << std::endl; | |
| 215 | ✗ | return com; | |
| 216 | } | ||
| 217 | ✗ | const std::vector<Triangle>& tri_indices_ = *tri_indices; | |
| 218 | |||
| 219 | ✗ | for (unsigned int i = 0; i < num_tris; ++i) { | |
| 220 | ✗ | const Triangle& tri = tri_indices_[i]; | |
| 221 | Scalar d_six_vol = | ||
| 222 | ✗ | (vertices_[tri[0]].cross(vertices_[tri[1]])).dot(vertices_[tri[2]]); | |
| 223 | ✗ | vol += d_six_vol; | |
| 224 | ✗ | com += (vertices_[tri[0]] + vertices_[tri[1]] + vertices_[tri[2]]) * | |
| 225 | ✗ | d_six_vol; | |
| 226 | } | ||
| 227 | |||
| 228 | ✗ | return com / (vol * 4); | |
| 229 | } | ||
| 230 | |||
| 231 | ✗ | Scalar computeVolume() const { | |
| 232 | ✗ | Scalar vol = 0; | |
| 233 | ✗ | if (!(vertices.get())) { | |
| 234 | std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain " | ||
| 235 | ✗ | "vertices." | |
| 236 | ✗ | << std::endl; | |
| 237 | ✗ | return vol; | |
| 238 | } | ||
| 239 | ✗ | const std::vector<Vec3s>& vertices_ = *vertices; | |
| 240 | ✗ | if (!(tri_indices.get())) { | |
| 241 | std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain " | ||
| 242 | ✗ | "triangles." | |
| 243 | ✗ | << std::endl; | |
| 244 | ✗ | return vol; | |
| 245 | } | ||
| 246 | ✗ | const std::vector<Triangle>& tri_indices_ = *tri_indices; | |
| 247 | ✗ | for (unsigned int i = 0; i < num_tris; ++i) { | |
| 248 | ✗ | const Triangle& tri = tri_indices_[i]; | |
| 249 | Scalar d_six_vol = | ||
| 250 | ✗ | (vertices_[tri[0]].cross(vertices_[tri[1]])).dot(vertices_[tri[2]]); | |
| 251 | ✗ | vol += d_six_vol; | |
| 252 | } | ||
| 253 | |||
| 254 | ✗ | return vol / 6; | |
| 255 | } | ||
| 256 | |||
| 257 | ✗ | Matrix3s computeMomentofInertia() const { | |
| 258 | ✗ | Matrix3s C = Matrix3s::Zero(); | |
| 259 | |||
| 260 | ✗ | Matrix3s C_canonical; | |
| 261 | ✗ | C_canonical << Scalar(1 / 60.0), // | |
| 262 | ✗ | Scalar(1 / 120.0), // | |
| 263 | ✗ | Scalar(1 / 120.0), // | |
| 264 | ✗ | Scalar(1 / 120.0), // | |
| 265 | ✗ | Scalar(1 / 60.0), // | |
| 266 | ✗ | Scalar(1 / 120.0), // | |
| 267 | ✗ | Scalar(1 / 120.0), // | |
| 268 | ✗ | Scalar(1 / 120.0), // | |
| 269 | ✗ | Scalar(1 / 60.0); | |
| 270 | |||
| 271 | ✗ | if (!(vertices.get())) { | |
| 272 | std::cerr << "BVH Error in `computeMomentofInertia`! The BVHModel does " | ||
| 273 | ✗ | "not contain vertices." | |
| 274 | ✗ | << std::endl; | |
| 275 | ✗ | return C; | |
| 276 | } | ||
| 277 | ✗ | const std::vector<Vec3s>& vertices_ = *vertices; | |
| 278 | ✗ | if (!(vertices.get())) { | |
| 279 | std::cerr << "BVH Error in `computeMomentofInertia`! The BVHModel does " | ||
| 280 | ✗ | "not contain vertices." | |
| 281 | ✗ | << std::endl; | |
| 282 | ✗ | return C; | |
| 283 | } | ||
| 284 | ✗ | const std::vector<Triangle>& tri_indices_ = *tri_indices; | |
| 285 | ✗ | for (unsigned int i = 0; i < num_tris; ++i) { | |
| 286 | ✗ | const Triangle& tri = tri_indices_[i]; | |
| 287 | ✗ | const Vec3s& v1 = vertices_[tri[0]]; | |
| 288 | ✗ | const Vec3s& v2 = vertices_[tri[1]]; | |
| 289 | ✗ | const Vec3s& v3 = vertices_[tri[2]]; | |
| 290 | ✗ | Matrix3s A; | |
| 291 | ✗ | A << v1.transpose(), v2.transpose(), v3.transpose(); | |
| 292 | ✗ | C += A.derived().transpose() * C_canonical * A * (v1.cross(v2)).dot(v3); | |
| 293 | } | ||
| 294 | |||
| 295 | ✗ | return C.trace() * Matrix3s::Identity() - C; | |
| 296 | } | ||
| 297 | |||
| 298 | protected: | ||
| 299 | virtual void deleteBVs() = 0; | ||
| 300 | virtual bool allocateBVs() = 0; | ||
| 301 | |||
| 302 | /// @brief Build the bounding volume hierarchy | ||
| 303 | virtual int buildTree() = 0; | ||
| 304 | |||
| 305 | /// @brief Refit the bounding volume hierarchy | ||
| 306 | virtual int refitTree(bool bottomup) = 0; | ||
| 307 | |||
| 308 | unsigned int num_tris_allocated; | ||
| 309 | unsigned int num_vertices_allocated; | ||
| 310 | unsigned int num_vertex_updated; /// for ccd vertex update | ||
| 311 | |||
| 312 | protected: | ||
| 313 | /// \brief Comparison operators | ||
| 314 | virtual bool isEqual(const CollisionGeometry& other) const; | ||
| 315 | }; | ||
| 316 | |||
| 317 | /// @brief A class describing the bounding hierarchy of a mesh model or a point | ||
| 318 | /// cloud model (which is viewed as a degraded version of mesh) \tparam BV one | ||
| 319 | /// of the bounding volume class in \ref Bounding_Volume. | ||
| 320 | template <typename BV> | ||
| 321 | class COAL_DLLAPI BVHModel : public BVHModelBase { | ||
| 322 | typedef BVHModelBase Base; | ||
| 323 | |||
| 324 | public: | ||
| 325 | using bv_node_vector_t = | ||
| 326 | std::vector<BVNode<BV>, Eigen::aligned_allocator<BVNode<BV>>>; | ||
| 327 | |||
| 328 | /// @brief Split rule to split one BV node into two children | ||
| 329 | shared_ptr<BVSplitter<BV>> bv_splitter; | ||
| 330 | |||
| 331 | /// @brief Fitting rule to fit a BV node to a set of geometry primitives | ||
| 332 | shared_ptr<BVFitter<BV>> bv_fitter; | ||
| 333 | |||
| 334 | /// @brief Default constructor to build an empty BVH | ||
| 335 | BVHModel(); | ||
| 336 | |||
| 337 | /// @brief Copy constructor from another BVH | ||
| 338 | /// | ||
| 339 | /// \param[in] other BVHModel to copy. | ||
| 340 | /// | ||
| 341 | BVHModel(const BVHModel& other); | ||
| 342 | |||
| 343 | /// @brief Clone *this into a new BVHModel | ||
| 344 | ✗ | virtual BVHModel<BV>* clone() const { return new BVHModel(*this); } | |
| 345 | |||
| 346 | /// @brief deconstruction, delete mesh data related. | ||
| 347 | 11770 | ~BVHModel() {} | |
| 348 | |||
| 349 | /// @brief We provide getBV() and getNumBVs() because BVH may be compressed | ||
| 350 | /// (in future), so we must provide some flexibility here | ||
| 351 | |||
| 352 | /// @brief Access the bv giving the its index | ||
| 353 | 640431742 | const BVNode<BV>& getBV(unsigned int i) const { | |
| 354 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 287578582 times.
|
640431742 | assert(i < num_bvs); |
| 355 | 640431742 | return (*bvs)[i]; | |
| 356 | } | ||
| 357 | |||
| 358 | /// @brief Access the bv giving the its index | ||
| 359 | ✗ | BVNode<BV>& getBV(unsigned int i) { | |
| 360 | ✗ | assert(i < num_bvs); | |
| 361 | ✗ | return (*bvs)[i]; | |
| 362 | } | ||
| 363 | |||
| 364 | /// @brief Get the number of bv in the BVH | ||
| 365 | 32 | unsigned int getNumBVs() const { return num_bvs; } | |
| 366 | |||
| 367 | /// @brief Get the BV type: default is unknown | ||
| 368 | NODE_TYPE getNodeType() const { return BV_UNKNOWN; } | ||
| 369 | |||
| 370 | /// @brief Check the number of memory used | ||
| 371 | int memUsage(const bool msg) const; | ||
| 372 | |||
| 373 | /// @brief This is a special acceleration: BVH_model default stores the BV's | ||
| 374 | /// transform in world coordinate. However, we can also store each BV's | ||
| 375 | /// transform related to its parent BV node. When traversing the BVH, this can | ||
| 376 | /// save one matrix transformation. | ||
| 377 | ✗ | void makeParentRelative() { | |
| 378 | ✗ | Matrix3s I(Matrix3s::Identity()); | |
| 379 | ✗ | makeParentRelativeRecurse(0, I, Vec3s::Zero()); | |
| 380 | } | ||
| 381 | |||
| 382 | protected: | ||
| 383 | void deleteBVs(); | ||
| 384 | bool allocateBVs(); | ||
| 385 | |||
| 386 | unsigned int num_bvs_allocated; | ||
| 387 | std::shared_ptr<std::vector<unsigned int>> primitive_indices; | ||
| 388 | |||
| 389 | /// @brief Bounding volume hierarchy | ||
| 390 | std::shared_ptr<bv_node_vector_t> bvs; | ||
| 391 | |||
| 392 | /// @brief Number of BV nodes in bounding volume hierarchy | ||
| 393 | unsigned int num_bvs; | ||
| 394 | |||
| 395 | /// @brief Build the bounding volume hierarchy | ||
| 396 | int buildTree(); | ||
| 397 | |||
| 398 | /// @brief Refit the bounding volume hierarchy | ||
| 399 | int refitTree(bool bottomup); | ||
| 400 | |||
| 401 | /// @brief Refit the bounding volume hierarchy in a top-down way (slow but | ||
| 402 | /// more compact) | ||
| 403 | int refitTree_topdown(); | ||
| 404 | |||
| 405 | /// @brief Refit the bounding volume hierarchy in a bottom-up way (fast but | ||
| 406 | /// less compact) | ||
| 407 | int refitTree_bottomup(); | ||
| 408 | |||
| 409 | /// @brief Recursive kernel for hierarchy construction | ||
| 410 | int recursiveBuildTree(int bv_id, unsigned int first_primitive, | ||
| 411 | unsigned int num_primitives); | ||
| 412 | |||
| 413 | /// @brief Recursive kernel for bottomup refitting | ||
| 414 | int recursiveRefitTree_bottomup(int bv_id); | ||
| 415 | |||
| 416 | /// @ recursively compute each bv's transform related to its parent. For | ||
| 417 | /// default BV, only the translation works. For oriented BV (OBB, RSS, | ||
| 418 | /// OBBRSS), special implementation is provided. | ||
| 419 | ✗ | void makeParentRelativeRecurse(int bv_id, Matrix3s& parent_axes, | |
| 420 | const Vec3s& parent_c) { | ||
| 421 | ✗ | bv_node_vector_t& bvs_ = *bvs; | |
| 422 | ✗ | if (!bvs_[static_cast<size_t>(bv_id)].isLeaf()) { | |
| 423 | ✗ | makeParentRelativeRecurse(bvs_[static_cast<size_t>(bv_id)].first_child, | |
| 424 | parent_axes, | ||
| 425 | ✗ | bvs_[static_cast<size_t>(bv_id)].getCenter()); | |
| 426 | |||
| 427 | ✗ | makeParentRelativeRecurse( | |
| 428 | ✗ | bvs_[static_cast<size_t>(bv_id)].first_child + 1, parent_axes, | |
| 429 | ✗ | bvs_[static_cast<size_t>(bv_id)].getCenter()); | |
| 430 | } | ||
| 431 | |||
| 432 | ✗ | bvs_[static_cast<size_t>(bv_id)].bv = | |
| 433 | ✗ | translate(bvs_[static_cast<size_t>(bv_id)].bv, -parent_c); | |
| 434 | } | ||
| 435 | |||
| 436 | private: | ||
| 437 | 21 | virtual bool isEqual(const CollisionGeometry& _other) const { | |
| 438 |
1/2✓ Branch 0 taken 21 times.
✗ Branch 1 not taken.
|
21 | const BVHModel* other_ptr = dynamic_cast<const BVHModel*>(&_other); |
| 439 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 21 times.
|
21 | if (other_ptr == nullptr) return false; |
| 440 | 21 | const BVHModel& other = *other_ptr; | |
| 441 | |||
| 442 | 21 | bool res = Base::isEqual(other); | |
| 443 |
2/2✓ Branch 0 taken 1 times.
✓ Branch 1 taken 20 times.
|
21 | if (!res) return false; |
| 444 | |||
| 445 | // unsigned int other_num_primitives = 0; | ||
| 446 | // if(other.primitive_indices) | ||
| 447 | // { | ||
| 448 | |||
| 449 | // switch(other.getModelType()) | ||
| 450 | // { | ||
| 451 | // case BVH_MODEL_TRIANGLES: | ||
| 452 | // other_num_primitives = num_tris; | ||
| 453 | // break; | ||
| 454 | // case BVH_MODEL_POINTCLOUD: | ||
| 455 | // other_num_primitives = num_vertices; | ||
| 456 | // break; | ||
| 457 | // default: | ||
| 458 | // ; | ||
| 459 | // } | ||
| 460 | // } | ||
| 461 | |||
| 462 | // unsigned int num_primitives = 0; | ||
| 463 | // if(primitive_indices) | ||
| 464 | // { | ||
| 465 | // | ||
| 466 | // switch(other.getModelType()) | ||
| 467 | // { | ||
| 468 | // case BVH_MODEL_TRIANGLES: | ||
| 469 | // num_primitives = num_tris; | ||
| 470 | // break; | ||
| 471 | // case BVH_MODEL_POINTCLOUD: | ||
| 472 | // num_primitives = num_vertices; | ||
| 473 | // break; | ||
| 474 | // default: | ||
| 475 | // ; | ||
| 476 | // } | ||
| 477 | // } | ||
| 478 | // | ||
| 479 | // if(num_primitives != other_num_primitives) | ||
| 480 | // return false; | ||
| 481 | // | ||
| 482 | // for(int k = 0; k < num_primitives; ++k) | ||
| 483 | // { | ||
| 484 | // if(primitive_indices[k] != other.primitive_indices[k]) | ||
| 485 | // return false; | ||
| 486 | // } | ||
| 487 | |||
| 488 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 20 times.
|
20 | if (num_bvs != other.num_bvs) return false; |
| 489 | |||
| 490 |
4/10✗ Branch 1 not taken.
✓ Branch 2 taken 20 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 20 times.
✗ Branch 12 not taken.
✓ Branch 13 taken 20 times.
|
20 | if ((!(bvs.get()) && other.bvs.get()) || (bvs.get() && !(other.bvs.get()))) |
| 491 | ✗ | return false; | |
| 492 |
3/6✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 20 times.
✗ Branch 7 not taken.
|
20 | if (bvs.get() && other.bvs.get()) { |
| 493 | 20 | const bv_node_vector_t& bvs_ = *bvs; | |
| 494 | 20 | const bv_node_vector_t& other_bvs_ = *(other.bvs); | |
| 495 |
2/2✓ Branch 0 taken 83252 times.
✓ Branch 1 taken 20 times.
|
83272 | for (unsigned int k = 0; k < num_bvs; ++k) { |
| 496 |
1/2✗ Branch 3 not taken.
✓ Branch 4 taken 83252 times.
|
83252 | if (bvs_[k] != other_bvs_[k]) return false; |
| 497 | } | ||
| 498 | } | ||
| 499 | |||
| 500 | 20 | return true; | |
| 501 | } | ||
| 502 | }; | ||
| 503 | |||
| 504 | /// @} | ||
| 505 | |||
| 506 | template <> | ||
| 507 | void BVHModel<OBB>::makeParentRelativeRecurse(int bv_id, Matrix3s& parent_axes, | ||
| 508 | const Vec3s& parent_c); | ||
| 509 | |||
| 510 | template <> | ||
| 511 | void BVHModel<RSS>::makeParentRelativeRecurse(int bv_id, Matrix3s& parent_axes, | ||
| 512 | const Vec3s& parent_c); | ||
| 513 | |||
| 514 | template <> | ||
| 515 | void BVHModel<OBBRSS>::makeParentRelativeRecurse(int bv_id, | ||
| 516 | Matrix3s& parent_axes, | ||
| 517 | const Vec3s& parent_c); | ||
| 518 | |||
| 519 | /// @brief Specialization of getNodeType() for BVHModel with different BV types | ||
| 520 | template <> | ||
| 521 | NODE_TYPE BVHModel<AABB>::getNodeType() const; | ||
| 522 | |||
| 523 | template <> | ||
| 524 | NODE_TYPE BVHModel<OBB>::getNodeType() const; | ||
| 525 | |||
| 526 | template <> | ||
| 527 | NODE_TYPE BVHModel<RSS>::getNodeType() const; | ||
| 528 | |||
| 529 | template <> | ||
| 530 | NODE_TYPE BVHModel<kIOS>::getNodeType() const; | ||
| 531 | |||
| 532 | template <> | ||
| 533 | NODE_TYPE BVHModel<OBBRSS>::getNodeType() const; | ||
| 534 | |||
| 535 | template <> | ||
| 536 | NODE_TYPE BVHModel<KDOP<16>>::getNodeType() const; | ||
| 537 | |||
| 538 | template <> | ||
| 539 | NODE_TYPE BVHModel<KDOP<18>>::getNodeType() const; | ||
| 540 | |||
| 541 | template <> | ||
| 542 | NODE_TYPE BVHModel<KDOP<24>>::getNodeType() const; | ||
| 543 | |||
| 544 | } // namespace coal | ||
| 545 | |||
| 546 | #endif | ||
| 547 |