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
 * 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 COAL_BVH_MODEL_H
#define COAL_BVH_MODEL_H

#include "coal/fwd.hh"
#include "coal/collision_object.h"
#include "coal/BVH/BVH_internal.h"
#include "coal/BV/BV_node.h"

#include <vector>
#include <memory>
#include <iostream>

namespace coal {


class ConvexBase;

template <typename BV>
class BVFitter;
template <typename BV>
class BVSplitter;

class COAL_DLLAPI BVHModelBase : public CollisionGeometry {
 public:
 std::shared_ptr<std::vector<Vec3s>> vertices;

 std::shared_ptr<std::vector<Triangle>> tri_indices;

 std::shared_ptr<std::vector<Vec3s>> 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() {}

 OBJECT_TYPE getObjectType() const { return OT_BVH; }

 void computeLocalAABB();

 int beginModel(unsigned int num_tris = 0, unsigned int num_vertices = 0);

 int addVertex(const Vec3s& p);

 int addVertices(const MatrixX3s& points);

 int addTriangles(const Matrixx3i& triangles);

 int addTriangle(const Vec3s& p1, const Vec3s& p2, const Vec3s& p3);

 int addSubModel(const std::vector<Vec3s>& ps,
 const std::vector<Triangle>& ts);

 int addSubModel(const std::vector<Vec3s>& ps);

 int endModel();

 int beginReplaceModel();

 int replaceVertex(const Vec3s& p);

 int replaceTriangle(const Vec3s& p1, const Vec3s& p2, const Vec3s& p3);

 int replaceSubModel(const std::vector<Vec3s>& ps);

 int endReplaceModel(bool refit = true, bool bottomup = true);

 int beginUpdateModel();

 int updateVertex(const Vec3s& p);

 int updateTriangle(const Vec3s& p1, const Vec3s& p2, const Vec3s& p3);

 int updateSubModel(const std::vector<Vec3s>& 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;

 Vec3s computeCOM() const {
 CoalScalar vol = 0;
 Vec3s com(0, 0, 0);
 if (!(vertices.get())) {
 std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain "
 "vertices."
 << std::endl;
 return com;
 }
 const std::vector<Vec3s>& vertices_ = *vertices;
 if (!(tri_indices.get())) {
 std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain "
 "triangles."
 << std::endl;
 return com;
 }
 const std::vector<Triangle>& tri_indices_ = *tri_indices;

 for (unsigned int i = 0; i < num_tris; ++i) {
 const Triangle& tri = tri_indices_[i];
 CoalScalar 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);
 }

 CoalScalar computeVolume() const {
 CoalScalar vol = 0;
 if (!(vertices.get())) {
 std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain "
 "vertices."
 << std::endl;
 return vol;
 }
 const std::vector<Vec3s>& vertices_ = *vertices;
 if (!(tri_indices.get())) {
 std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain "
 "triangles."
 << std::endl;
 return vol;
 }
 const std::vector<Triangle>& tri_indices_ = *tri_indices;
 for (unsigned int i = 0; i < num_tris; ++i) {
 const Triangle& tri = tri_indices_[i];
 CoalScalar d_six_vol =
 (vertices_[tri[0]].cross(vertices_[tri[1]])).dot(vertices_[tri[2]]);
 vol += d_six_vol;
 }

 return vol / 6;
 }

 Matrix3s computeMomentofInertia() const {
 Matrix3s C = Matrix3s::Zero();

 Matrix3s 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;

 if (!(vertices.get())) {
 std::cerr << "BVH Error in `computeMomentofInertia`! The BVHModel does "
 "not contain vertices."
 << std::endl;
 return C;
 }
 const std::vector<Vec3s>& vertices_ = *vertices;
 if (!(vertices.get())) {
 std::cerr << "BVH Error in `computeMomentofInertia`! The BVHModel does "
 "not contain vertices."
 << std::endl;
 return C;
 }
 const std::vector<Triangle>& tri_indices_ = *tri_indices;
 for (unsigned int i = 0; i < num_tris; ++i) {
 const Triangle& tri = tri_indices_[i];
 const Vec3s& v1 = vertices_[tri[0]];
 const Vec3s& v2 = vertices_[tri[1]];
 const Vec3s& v3 = vertices_[tri[2]];
 Matrix3s A;
 A << v1.transpose(), v2.transpose(), v3.transpose();
 C += A.derived().transpose() * C_canonical * A * (v1.cross(v2)).dot(v3);
 }

 return C.trace() * Matrix3s::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 COAL_DLLAPI BVHModel : public BVHModelBase {
 typedef BVHModelBase Base;

 public:
 using bv_node_vector_t =
 std::vector<BVNode<BV>, Eigen::aligned_allocator<BVNode<BV>>>;

 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() {}


 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() {
 Matrix3s I(Matrix3s::Identity());
 makeParentRelativeRecurse(0, I, Vec3s::Zero());
 }

 protected:
 void deleteBVs();
 bool allocateBVs();

 unsigned int num_bvs_allocated;
 std::shared_ptr<std::vector<unsigned int>> primitive_indices;

 std::shared_ptr<bv_node_vector_t> 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, Matrix3s& parent_axes,
 const Vec3s& parent_c) {
 bv_node_vector_t& bvs_ = *bvs;
 if (!bvs_[static_cast<size_t>(bv_id)].isLeaf()) {
 makeParentRelativeRecurse(bvs_[static_cast<size_t>(bv_id)].first_child,
 parent_axes,
 bvs_[static_cast<size_t>(bv_id)].getCenter());

 makeParentRelativeRecurse(
 bvs_[static_cast<size_t>(bv_id)].first_child + 1, parent_axes,
 bvs_[static_cast<size_t>(bv_id)].getCenter());
 }

 bvs_[static_cast<size_t>(bv_id)].bv =
 translate(bvs_[static_cast<size_t>(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;

 if ((!(bvs.get()) && other.bvs.get()) || (bvs.get() && !(other.bvs.get())))
 return false;
 if (bvs.get() && other.bvs.get()) {
 const bv_node_vector_t& bvs_ = *bvs;
 const bv_node_vector_t& other_bvs_ = *(other.bvs);
 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, Matrix3s& parent_axes,
 const Vec3s& parent_c);

template <>
void BVHModel<RSS>::makeParentRelativeRecurse(int bv_id, Matrix3s& parent_axes,
 const Vec3s& parent_c);

template <>
void BVHModel<OBBRSS>::makeParentRelativeRecurse(int bv_id,
 Matrix3s& parent_axes,
 const Vec3s& 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 coal

#endif