BVH_model.h

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011-2014, Willow Garage, Inc.
 *  Copyright (c) 2014-2015, Open Source Robotics Foundation
 *  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/collision_object.h>
#include <hpp/fcl/BVH/BVH_internal.h>
#include <hpp/fcl/BV/BV_node.h>
#include <vector>
#include <boost/shared_ptr.hpp>
#include <boost/noncopyable.hpp>
 
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;
 
  int num_tris;
 
  int num_vertices;
 
  BVHBuildState build_state;
 
  boost::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() : 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)
  {
  }
 
  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(int num_tris = 0, int num_vertices = 0);
 
  int addVertex(const Vec3f& p);
 
  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(int msg) const = 0;
 
  virtual void makeParentRelative() = 0;
 
  Vec3f computeCOM() const
  {
    FCL_REAL vol = 0;
    Vec3f com(0,0,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;
      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
  {
    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(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;
 
  int num_tris_allocated;
  int num_vertices_allocated;
  int num_vertex_updated; 
};
 
template<typename BV>
class HPP_FCL_DLLAPI BVHModel : public BVHModelBase
{
 
public:
  boost::shared_ptr<BVSplitter<BV> > bv_splitter;
 
  boost::shared_ptr<BVFitter<BV> > bv_fitter;
 
  BVHModel();
 
  BVHModel(const BVHModel& other);
 
  ~BVHModel()
  {
    delete [] bvs;
    delete [] primitive_indices;
  }
 
  
  const BVNode<BV>& getBV(int id) const
  {
    assert (id < num_bvs);
    return bvs[id];
  }
 
  BVNode<BV>& getBV(int id)
  {
    assert (id < num_bvs);
    return bvs[id];
  }
 
  int getNumBVs() const
  {
    return num_bvs;
  }
 
  NODE_TYPE getNodeType() const { return BV_UNKNOWN; }
 
  int memUsage(int msg) const;
 
  void makeParentRelative()
  {
    Matrix3f I (Matrix3f::Identity());
    makeParentRelativeRecurse(0, I, Vec3f());
  }
 
private:
  void deleteBVs();
  bool allocateBVs();
 
  int num_bvs_allocated;
  unsigned int* primitive_indices;
 
  BVNode<BV>* bvs;
 
  int num_bvs;
 
  int buildTree();
 
  int refitTree(bool bottomup);
 
  int refitTree_topdown();
 
  int refitTree_bottomup();
 
  int recursiveBuildTree(int bv_id, int first_primitive, 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);
  }
};
 
 
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 hpp
 
#endif