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