Program Listing for File BVH_model.h

↰ Return to documentation for file (/tmp/ws/src/hpp-fcl/include/hpp/fcl/BVH/BVH_model.h)

/*
 * 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