.. _program_listing_file__tmp_ws_src_hpp-fcl_include_hpp_fcl_BVH_BVH_model.h:

Program Listing for File BVH_model.h
====================================

|exhale_lsh| :ref:`Return to documentation for file <file__tmp_ws_src_hpp-fcl_include_hpp_fcl_BVH_BVH_model.h>` (``/tmp/ws/src/hpp-fcl/include/hpp/fcl/BVH/BVH_model.h``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   /*
    * Software License Agreement (BSD License)
    *
    *  Copyright (c) 2011-2014, Willow Garage, Inc.
    *  Copyright (c) 2014-2015, Open Source Robotics Foundation
    *  Copyright (c) 2020-2022, INRIA
    *  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.
    *   * Neither the name of Open Source Robotics Foundation nor the names of its
    *     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.
    */
   
   #ifndef HPP_FCL_BVH_MODEL_H
   #define HPP_FCL_BVH_MODEL_H
   
   #include <hpp/fcl/fwd.hh>
   #include <hpp/fcl/collision_object.h>
   #include <hpp/fcl/BVH/BVH_internal.h>
   #include <hpp/fcl/BV/BV_node.h>
   #include <vector>
   
   namespace hpp {
   namespace fcl {
   
   
   class ConvexBase;
   
   template <typename BV>
   class BVFitter;
   template <typename BV>
   class BVSplitter;
   
   class HPP_FCL_DLLAPI BVHModelBase : public CollisionGeometry {
    public:
     Vec3f* vertices;
   
     Triangle* tri_indices;
   
     Vec3f* prev_vertices;
   
     unsigned int num_tris;
   
     unsigned int num_vertices;
   
     BVHBuildState build_state;
   
     shared_ptr<ConvexBase> convex;
   
     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;
     }
   
     BVHModelBase();
   
     BVHModelBase(const BVHModelBase& other);
   
     virtual ~BVHModelBase() {
       delete[] vertices;
       delete[] tri_indices;
       delete[] prev_vertices;
     }
   
     OBJECT_TYPE getObjectType() const { return OT_BVH; }
   
     void computeLocalAABB();
   
     int beginModel(unsigned int num_tris = 0, unsigned int num_vertices = 0);
   
     int addVertex(const Vec3f& p);
   
     int addVertices(const Matrixx3f& points);
   
     int addTriangles(const Matrixx3i& triangles);
   
     int addTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3);
   
     int addSubModel(const std::vector<Vec3f>& ps,
                     const std::vector<Triangle>& ts);
   
     int addSubModel(const std::vector<Vec3f>& ps);
   
     int endModel();
   
     int beginReplaceModel();
   
     int replaceVertex(const Vec3f& p);
   
     int replaceTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3);
   
     int replaceSubModel(const std::vector<Vec3f>& ps);
   
     int endReplaceModel(bool refit = true, bool bottomup = true);
   
     int beginUpdateModel();
   
     int updateVertex(const Vec3f& p);
   
     int updateTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3);
   
     int updateSubModel(const std::vector<Vec3f>& ps);
   
     int endUpdateModel(bool refit = true, bool bottomup = true);
   
     void buildConvexRepresentation(bool share_memory);
   
     bool buildConvexHull(bool keepTriangle, const char* qhullCommand = NULL);
   
     virtual int memUsage(const bool msg = false) const = 0;
   
     virtual void makeParentRelative() = 0;
   
     Vec3f computeCOM() const {
       FCL_REAL vol = 0;
       Vec3f com(0, 0, 0);
       for (unsigned 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 (unsigned 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 {
       Matrix3f C = Matrix3f::Zero();
   
       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;
   
       for (unsigned 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]];
         Matrix3f A;
         A << v1.transpose(), v2.transpose(), v3.transpose();
         C += A.derived().transpose() * C_canonical * A * (v1.cross(v2)).dot(v3);
       }
   
       return C.trace() * Matrix3f::Identity() - C;
     }
   
    protected:
     virtual void deleteBVs() = 0;
     virtual bool allocateBVs() = 0;
   
     virtual int buildTree() = 0;
   
     virtual int refitTree(bool bottomup) = 0;
   
     unsigned int num_tris_allocated;
     unsigned int num_vertices_allocated;
     unsigned int num_vertex_updated;  
   
    protected:
     virtual bool isEqual(const CollisionGeometry& other) const;
   };
   
   template <typename BV>
   class HPP_FCL_DLLAPI BVHModel : public BVHModelBase {
     typedef BVHModelBase Base;
   
    public:
     shared_ptr<BVSplitter<BV> > bv_splitter;
   
     shared_ptr<BVFitter<BV> > bv_fitter;
   
     BVHModel();
   
     BVHModel(const BVHModel& other);
   
     virtual BVHModel<BV>* clone() const { return new BVHModel(*this); }
   
     ~BVHModel() {
       delete[] bvs;
       delete[] primitive_indices;
     }
   
   
     const BVNode<BV>& getBV(unsigned int i) const {
       assert(i < num_bvs);
       return bvs[i];
     }
   
     BVNode<BV>& getBV(unsigned int i) {
       assert(i < num_bvs);
       return bvs[i];
     }
   
     unsigned int getNumBVs() const { return num_bvs; }
   
     NODE_TYPE getNodeType() const { return BV_UNKNOWN; }
   
     int memUsage(const bool msg) const;
   
     void makeParentRelative() {
       Matrix3f I(Matrix3f::Identity());
       makeParentRelativeRecurse(0, I, Vec3f::Zero());
     }
   
    protected:
     void deleteBVs();
     bool allocateBVs();
   
     unsigned int num_bvs_allocated;
     unsigned int* primitive_indices;
   
     BVNode<BV>* bvs;
   
     unsigned int num_bvs;
   
     int buildTree();
   
     int refitTree(bool bottomup);
   
     int refitTree_topdown();
   
     int refitTree_bottomup();
   
     int recursiveBuildTree(int bv_id, unsigned int first_primitive,
                            unsigned int num_primitives);
   
     int recursiveRefitTree_bottomup(int bv_id);
   
     void makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes,
                                    const Vec3f& parent_c) {
       if (!bvs[bv_id].isLeaf()) {
         makeParentRelativeRecurse(bvs[bv_id].first_child, parent_axes,
                                   bvs[bv_id].getCenter());
   
         makeParentRelativeRecurse(bvs[bv_id].first_child + 1, parent_axes,
                                   bvs[bv_id].getCenter());
       }
   
       bvs[bv_id].bv = translate(bvs[bv_id].bv, -parent_c);
     }
   
    private:
     virtual bool isEqual(const CollisionGeometry& _other) const {
       const BVHModel* other_ptr = dynamic_cast<const BVHModel*>(&_other);
       if (other_ptr == nullptr) return false;
       const BVHModel& other = *other_ptr;
   
       bool res = Base::isEqual(other);
       if (!res) return false;
   
       // unsigned int other_num_primitives = 0;
       // if(other.primitive_indices)
       // {
   
       //   switch(other.getModelType())
       //   {
       //     case BVH_MODEL_TRIANGLES:
       //       other_num_primitives = num_tris;
       //       break;
       //     case BVH_MODEL_POINTCLOUD:
       //       other_num_primitives = num_vertices;
       //       break;
       //     default:
       //       ;
       //   }
       // }
   
       //    unsigned int num_primitives = 0;
       //    if(primitive_indices)
       //    {
       //
       //      switch(other.getModelType())
       //      {
       //        case BVH_MODEL_TRIANGLES:
       //          num_primitives = num_tris;
       //          break;
       //        case BVH_MODEL_POINTCLOUD:
       //          num_primitives = num_vertices;
       //          break;
       //        default:
       //          ;
       //      }
       //    }
       //
       //    if(num_primitives != other_num_primitives)
       //      return false;
       //
       //    for(int k = 0; k < num_primitives; ++k)
       //    {
       //      if(primitive_indices[k] != other.primitive_indices[k])
       //        return false;
       //    }
   
       if (num_bvs != other.num_bvs) return false;
   
       for (unsigned int k = 0; k < num_bvs; ++k) {
         if (bvs[k] != other.bvs[k]) return false;
       }
   
       return true;
     }
   };
   
   
   template <>
   void BVHModel<OBB>::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes,
                                                 const Vec3f& parent_c);
   
   template <>
   void BVHModel<RSS>::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes,
                                                 const Vec3f& parent_c);
   
   template <>
   void BVHModel<OBBRSS>::makeParentRelativeRecurse(int bv_id,
                                                    Matrix3f& parent_axes,
                                                    const Vec3f& parent_c);
   
   template <>
   NODE_TYPE BVHModel<AABB>::getNodeType() const;
   
   template <>
   NODE_TYPE BVHModel<OBB>::getNodeType() const;
   
   template <>
   NODE_TYPE BVHModel<RSS>::getNodeType() const;
   
   template <>
   NODE_TYPE BVHModel<kIOS>::getNodeType() const;
   
   template <>
   NODE_TYPE BVHModel<OBBRSS>::getNodeType() const;
   
   template <>
   NODE_TYPE BVHModel<KDOP<16> >::getNodeType() const;
   
   template <>
   NODE_TYPE BVHModel<KDOP<18> >::getNodeType() const;
   
   template <>
   NODE_TYPE BVHModel<KDOP<24> >::getNodeType() const;
   
   }  // namespace fcl
   
   }  // namespace hpp
   
   #endif