BVH_model.h 12.4 KB
 sachinc committed Aug 25, 2011 1 2 3 /* * Software License Agreement (BSD License) *  Jeongseok Lee committed Apr 24, 2015 4 5  * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation  sachinc committed Aug 25, 2011 6 7 8 9 10 11 12 13 14 15 16 17  * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution.  Jeongseok Lee committed Apr 24, 2015 18  * * Neither the name of Open Source Robotics Foundation nor the names of its  sachinc committed Aug 25, 2011 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37  * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */  Joseph Mirabel committed Feb 07, 2019 38 39 #ifndef HPP_FCL_BVH_MODEL_H #define HPP_FCL_BVH_MODEL_H  sachinc committed Aug 25, 2011 40   Florent Lamiraux committed Jun 30, 2015 41 42 43 #include #include #include  sachinc committed Aug 25, 2011 44 45 #include #include  Ioan Sucan committed Oct 29, 2012 46 #include  sachinc committed Aug 25, 2011 47   Joseph Mirabel committed Feb 07, 2019 48 49 namespace hpp {  sachinc committed Aug 25, 2011 50 51 52 namespace fcl {  Lucile Remigy committed Oct 14, 2019 53 54 55 /// @addtogroup Construction_Of_BVH /// @{  Joseph Mirabel committed Sep 18, 2019 56 57 class ConvexBase;  Joseph Mirabel committed Oct 09, 2019 58 59 60 template class BVFitter; template class BVSplitter;  Joseph Mirabel committed Sep 18, 2019 61 62 /// @brief A base class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as a degraded version of mesh) class BVHModelBase : public CollisionGeometry  sachinc committed Aug 25, 2011 63 64 { public:  Lucile Remigy committed Aug 28, 2019 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82  /// @brief Geometry point data Vec3f* vertices; /// @brief Geometry triangle index data, will be NULL for point clouds Triangle* tri_indices; /// @brief Geometry point data in previous frame Vec3f* prev_vertices; /// @brief Number of triangles int num_tris; /// @brief Number of points int num_vertices; /// @brief The state of BVH building process BVHBuildState build_state;  Joseph Mirabel committed Sep 18, 2019 83 84  /// @brief Convex representation of this object boost::shared_ptr< ConvexBase > convex;  Lucile Remigy committed Aug 28, 2019 85   jpan committed Aug 02, 2012 86  /// @brief Model type described by the instance  sachinc committed Aug 25, 2011 87 88 89 90 91 92 93 94 95 96  BVHModelType getModelType() const { if(num_tris && num_vertices) return BVH_MODEL_TRIANGLES; else if(num_vertices) return BVH_MODEL_POINTCLOUD; else return BVH_MODEL_UNKNOWN; }  jpan committed Aug 02, 2012 97  /// @brief Constructing an empty BVH  Joseph Mirabel committed Sep 18, 2019 98 99 100 101 102 103 104 105 106  BVHModelBase() : vertices(NULL), tri_indices(NULL), prev_vertices(NULL), num_tris(0), num_vertices(0), build_state(BVH_BUILD_STATE_EMPTY), num_tris_allocated(0), num_vertices_allocated(0), num_vertex_updated(0)  sachinc committed Aug 25, 2011 107 108 109  { }  jpan committed Aug 02, 2012 110  /// @brief copy from another BVH  Joseph Mirabel committed Sep 18, 2019 111  BVHModelBase(const BVHModelBase& other);  sachinc committed Aug 25, 2011 112   jpan committed Aug 02, 2012 113  /// @brief deconstruction, delete mesh data related.  Joseph Mirabel committed Sep 18, 2019 114  virtual ~BVHModelBase ()  sachinc committed Aug 25, 2011 115 116 117 118 119 120  { delete [] vertices; delete [] tri_indices; delete [] prev_vertices; }  jpan committed Aug 02, 2012 121  /// @brief Get the object type: it is a BVH  sachinc committed Aug 25, 2011 122 123  OBJECT_TYPE getObjectType() const { return OT_BVH; }  jpan committed Aug 02, 2012 124  /// @brief Compute the AABB for the BVH, used for broad-phase collision  jpan committed Sep 12, 2011 125  void computeLocalAABB();  sachinc committed Aug 25, 2011 126   jpan committed Aug 02, 2012 127  /// @brief Begin a new BVH model  sachinc committed Aug 25, 2011 128 129  int beginModel(int num_tris = 0, int num_vertices = 0);  jpan committed Aug 02, 2012 130  /// @brief Add one point in the new BVH model  sachinc committed Aug 25, 2011 131 132  int addVertex(const Vec3f& p);  jpan committed Aug 02, 2012 133  /// @brief Add one triangle in the new BVH model  sachinc committed Aug 25, 2011 134 135  int addTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3);  jpan committed Aug 02, 2012 136  /// @brief Add a set of triangles in the new BVH model  sachinc committed Aug 25, 2011 137 138  int addSubModel(const std::vector& ps, const std::vector& ts);  jpan committed Aug 02, 2012 139  /// @brief Add a set of points in the new BVH model  sachinc committed Aug 25, 2011 140 141  int addSubModel(const std::vector& ps);  jpan committed Aug 02, 2012 142  /// @brief End BVH model construction, will build the bounding volume hierarchy  sachinc committed Aug 25, 2011 143 144  int endModel();  jpan committed Aug 02, 2012 145  /// @brief Replace the geometry information of current frame (i.e. should have the same mesh topology with the previous frame)  sachinc committed Aug 25, 2011 146 147  int beginReplaceModel();  jpan committed Aug 02, 2012 148  /// @brief Replace one point in the old BVH model  sachinc committed Aug 25, 2011 149 150  int replaceVertex(const Vec3f& p);  jpan committed Aug 02, 2012 151  /// @brief Replace one triangle in the old BVH model  sachinc committed Aug 25, 2011 152 153  int replaceTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3);  jpan committed Aug 02, 2012 154  /// @brief Replace a set of points in the old BVH model  sachinc committed Aug 25, 2011 155 156  int replaceSubModel(const std::vector& ps);  jpan committed Aug 02, 2012 157  /// @brief End BVH model replacement, will also refit or rebuild the bounding volume hierarchy  sachinc committed Aug 25, 2011 158 159  int endReplaceModel(bool refit = true, bool bottomup = true);  jpan committed Aug 02, 2012 160 161  /// @brief Replace the geometry information of current frame (i.e. should have the same mesh topology with the previous frame). /// The current frame will be saved as the previous frame in prev_vertices.  sachinc committed Aug 25, 2011 162 163  int beginUpdateModel();  jpan committed Aug 02, 2012 164  /// @brief Update one point in the old BVH model  sachinc committed Aug 25, 2011 165 166  int updateVertex(const Vec3f& p);  jpan committed Aug 02, 2012 167  /// @brief Update one triangle in the old BVH model  sachinc committed Aug 25, 2011 168 169  int updateTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3);  jpan committed Aug 02, 2012 170  /// @brief Update a set of points in the old BVH model  sachinc committed Aug 25, 2011 171 172  int updateSubModel(const std::vector& ps);  jpan committed Aug 02, 2012 173  /// @brief End BVH model update, will also refit or rebuild the bounding volume hierarchy  sachinc committed Aug 25, 2011 174 175  int endUpdateModel(bool refit = true, bool bottomup = true);  Joseph Mirabel committed Sep 18, 2019 176 177 178  /// @brief Build this Convex representation of this model. /// \note this only takes the points of this model. It does not check that the /// object is convex. It does not compute a convex hull.  Joseph Mirabel committed Sep 18, 2019 179 180 181  void buildConvexRepresentation(bool share_memory); virtual int memUsage(int msg) const = 0;  Joseph Mirabel committed Sep 18, 2019 182 183  /// @brief This is a special acceleration: BVH_model default stores the BV's transform in world coordinate. However, we can also store each BV's transform related to its parent  jpan committed Aug 02, 2012 184  /// BV node. When traversing the BVH, this can save one matrix transformation.  Joseph Mirabel committed Sep 18, 2019 185  virtual void makeParentRelative() = 0;  jpan committed Dec 30, 2011 186   panjia1983 committed Aug 14, 2013 187 188 189  Vec3f computeCOM() const { FCL_REAL vol = 0;  Joseph Mirabel committed Jun 14, 2016 190  Vec3f com(0,0,0);  panjia1983 committed Aug 14, 2013 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216  for(int i = 0; i < num_tris; ++i) { const Triangle& tri = tri_indices[i]; FCL_REAL d_six_vol = (vertices[tri[0]].cross(vertices[tri[1]])).dot(vertices[tri[2]]); vol += d_six_vol; com += (vertices[tri[0]] + vertices[tri[1]] + vertices[tri[2]]) * d_six_vol; } return com / (vol * 4); } FCL_REAL computeVolume() const { FCL_REAL vol = 0; for(int i = 0; i < num_tris; ++i) { const Triangle& tri = tri_indices[i]; FCL_REAL d_six_vol = (vertices[tri[0]].cross(vertices[tri[1]])).dot(vertices[tri[2]]); vol += d_six_vol; } return vol / 6; } Matrix3f computeMomentofInertia() const {  Joseph Mirabel committed Jun 14, 2016 217  Matrix3f C = Matrix3f::Zero();  panjia1983 committed Aug 14, 2013 218   Joseph Mirabel committed Jun 14, 2016 219 220 221 222  Matrix3f C_canonical; C_canonical << 1/60.0, 1/120.0, 1/120.0, 1/120.0, 1/60.0, 1/120.0, 1/120.0, 1/120.0, 1/60.0;  panjia1983 committed Aug 14, 2013 223 224 225 226 227 228 229  for(int i = 0; i < num_tris; ++i) { const Triangle& tri = tri_indices[i]; const Vec3f& v1 = vertices[tri[0]]; const Vec3f& v2 = vertices[tri[1]]; const Vec3f& v3 = vertices[tri[2]];  Joseph Mirabel committed Jun 14, 2016 230  Matrix3f A; A << v1.transpose(), v2.transpose(), v3.transpose();  Joseph Mirabel committed Jun 14, 2016 231  C += A.derived().transpose() * C_canonical * A * (v1.cross(v2)).dot(v3);  panjia1983 committed Aug 14, 2013 232 233  }  Joseph Mirabel committed Jun 14, 2016 234  return C.trace() * Matrix3f::Identity() - C;  panjia1983 committed Aug 14, 2013 235 236  }  Joseph Mirabel committed Sep 18, 2019 237 238 239 240 241 242 243 244 245 protected: virtual void deleteBVs() = 0; virtual bool allocateBVs() = 0; /// @brief Build the bounding volume hierarchy virtual int buildTree() = 0; /// @brief Refit the bounding volume hierarchy virtual int refitTree(bool bottomup) = 0;  sachinc committed Aug 25, 2011 246   jpan committed Aug 02, 2012 247 248 249  int num_tris_allocated; int num_vertices_allocated; int num_vertex_updated; /// for ccd vertex update  Joseph Mirabel committed Sep 18, 2019 250 251 252 }; /// @brief A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as a degraded version of mesh)  Joseph Mirabel committed Jan 28, 2020 253 /// \tparam BV one of the bounding volume class in \ref Bounding_Volume.  Joseph Mirabel committed Sep 18, 2019 254 255 256 257 258 259 template class BVHModel : public BVHModelBase { public: /// @brief Split rule to split one BV node into two children  Joseph Mirabel committed Oct 08, 2019 260  boost::shared_ptr > bv_splitter;  Joseph Mirabel committed Sep 18, 2019 261 262  /// @brief Fitting rule to fit a BV node to a set of geometry primitives  Joseph Mirabel committed Oct 08, 2019 263  boost::shared_ptr > bv_fitter;  Joseph Mirabel committed Sep 18, 2019 264 265  /// @brief Constructing an empty BVH  Joseph Mirabel committed Oct 09, 2019 266  BVHModel();  Joseph Mirabel committed Sep 18, 2019 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318  /// @brief copy from another BVH BVHModel(const BVHModel& other); /// @brief deconstruction, delete mesh data related. ~BVHModel() { delete [] bvs; delete [] primitive_indices; } /// @brief We provide getBV() and getNumBVs() because BVH may be compressed (in future), so we must provide some flexibility here /// @brief Access the bv giving the its index const BVNode& getBV(int id) const { assert (id < num_bvs); return bvs[id]; } /// @brief Access the bv giving the its index BVNode& getBV(int id) { assert (id < num_bvs); return bvs[id]; } /// @brief Get the number of bv in the BVH int getNumBVs() const { return num_bvs; } /// @brief Get the BV type: default is unknown NODE_TYPE getNodeType() const { return BV_UNKNOWN; } /// @brief Check the number of memory used int memUsage(int msg) const; /// @brief This is a special acceleration: BVH_model default stores the BV's transform in world coordinate. However, we can also store each BV's transform related to its parent /// BV node. When traversing the BVH, this can save one matrix transformation. void makeParentRelative() { Matrix3f I (Matrix3f::Identity()); makeParentRelativeRecurse(0, I, Vec3f()); } private: void deleteBVs(); bool allocateBVs(); int num_bvs_allocated;  jpan committed Aug 02, 2012 319 320 321  unsigned int* primitive_indices; /// @brief Bounding volume hierarchy  sachinc committed Aug 25, 2011 322 323  BVNode* bvs;  jpan committed Aug 02, 2012 324  /// @brief Number of BV nodes in bounding volume hierarchy  sachinc committed Aug 25, 2011 325 326  int num_bvs;  jpan committed Aug 02, 2012 327  /// @brief Build the bounding volume hierarchy  sachinc committed Aug 25, 2011 328 329  int buildTree();  jpan committed Aug 02, 2012 330  /// @brief Refit the bounding volume hierarchy  sachinc committed Aug 25, 2011 331 332  int refitTree(bool bottomup);  jpan committed Aug 02, 2012 333  /// @brief Refit the bounding volume hierarchy in a top-down way (slow but more compact)  sachinc committed Aug 25, 2011 334 335  int refitTree_topdown();  jpan committed Aug 02, 2012 336  /// @brief Refit the bounding volume hierarchy in a bottom-up way (fast but less compact)  sachinc committed Aug 25, 2011 337 338  int refitTree_bottomup();  jpan committed Aug 02, 2012 339  /// @brief Recursive kernel for hierarchy construction  sachinc committed Aug 25, 2011 340 341  int recursiveBuildTree(int bv_id, int first_primitive, int num_primitives);  jpan committed Aug 02, 2012 342  /// @brief Recursive kernel for bottomup refitting  sachinc committed Aug 25, 2011 343 344  int recursiveRefitTree_bottomup(int bv_id);  Lucile Remigy committed Oct 14, 2019 345  /// @ recursively compute each bv's transform related to its parent. For default BV, only the translation works.  jpan committed Aug 02, 2012 346  /// For oriented BV (OBB, RSS, OBBRSS), special implementation is provided.  Joseph Mirabel committed Jun 14, 2016 347  void makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, const Vec3f& parent_c)  jpan committed Dec 30, 2011 348 349 350  { if(!bvs[bv_id].isLeaf()) {  Joseph Mirabel committed Jun 14, 2016 351  makeParentRelativeRecurse(bvs[bv_id].first_child, parent_axes, bvs[bv_id].getCenter());  jpan committed Dec 30, 2011 352   Joseph Mirabel committed Jun 14, 2016 353  makeParentRelativeRecurse(bvs[bv_id].first_child + 1, parent_axes, bvs[bv_id].getCenter());  jpan committed Dec 30, 2011 354 355  }  jpan committed Aug 02, 2012 356  bvs[bv_id].bv = translate(bvs[bv_id].bv, -parent_c);  jpan committed Dec 30, 2011 357  }  sachinc committed Aug 25, 2011 358 359 };  Lucile Remigy committed Oct 14, 2019 360 /// @}  jpan committed Aug 27, 2011 361   jpan committed Aug 02, 2012 362 template<>  Joseph Mirabel committed Jun 14, 2016 363 void BVHModel::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, const Vec3f& parent_c);  jpan committed Aug 02, 2012 364 365  template<>  Joseph Mirabel committed Jun 14, 2016 366 void BVHModel::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, const Vec3f& parent_c);  jpan committed Aug 02, 2012 367 368  template<>  Joseph Mirabel committed Jun 14, 2016 369 void BVHModel::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, const Vec3f& parent_c);  jpan committed Aug 02, 2012 370 371 372  /// @brief Specialization of getNodeType() for BVHModel with different BV types  sachinc committed Aug 25, 2011 373 374 375 376 377 378 379 380 381 template<> NODE_TYPE BVHModel::getNodeType() const; template<> NODE_TYPE BVHModel::getNodeType() const; template<> NODE_TYPE BVHModel::getNodeType() const;  jpan committed May 25, 2012 382 383 384 template<> NODE_TYPE BVHModel::getNodeType() const;  jpan committed May 30, 2012 385 386 387 template<> NODE_TYPE BVHModel::getNodeType() const;  sachinc committed Aug 25, 2011 388 389 390 391 392 393 394 395 396 397 398 template<> NODE_TYPE BVHModel >::getNodeType() const; template<> NODE_TYPE BVHModel >::getNodeType() const; template<> NODE_TYPE BVHModel >::getNodeType() const; }  Joseph Mirabel committed Feb 07, 2019 399 400 } // namespace hpp  sachinc committed Aug 25, 2011 401 #endif