Program Listing for File hfield.h

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

/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2021, 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_HEIGHT_FIELD_H
#define HPP_FCL_HEIGHT_FIELD_H

#include <hpp/fcl/fwd.hh>
#include <hpp/fcl/collision_object.h>
#include <hpp/fcl/BV/BV_node.h>
#include <hpp/fcl/BVH/BVH_internal.h>

#include <vector>

namespace hpp {
namespace fcl {


struct HPP_FCL_DLLAPI HFNodeBase {
  size_t first_child;

  Eigen::DenseIndex x_id, x_size;
  Eigen::DenseIndex y_id, y_size;

  FCL_REAL max_height;

  HFNodeBase()
      : first_child(0),
        x_id(-1),
        x_size(0),
        y_id(-1),
        y_size(0),
        max_height(std::numeric_limits<FCL_REAL>::lowest()) {}

  bool operator==(const HFNodeBase& other) const {
    return first_child == other.first_child && x_id == other.x_id &&
           x_size == other.x_size && y_id == other.y_id &&
           y_size == other.y_size && max_height == other.max_height;
  }

  bool operator!=(const HFNodeBase& other) const { return !(*this == other); }

  inline bool isLeaf() const { return x_size == 1 && y_size == 1; }

  inline size_t leftChild() const { return first_child; }

  inline size_t rightChild() const { return first_child + 1; }

  inline Eigen::Vector2i leftChildIndexes() const {
    return Eigen::Vector2i(x_id, y_id);
  }
  inline Eigen::Vector2i rightChildIndexes() const {
    return Eigen::Vector2i(x_id + x_size / 2, y_id + y_size / 2);
  }
};

template <typename BV>
struct HPP_FCL_DLLAPI HFNode : public HFNodeBase {
  typedef HFNodeBase Base;

  BV bv;

  bool operator==(const HFNode& other) const {
    return Base::operator==(other) && bv == other.bv;
  }

  bool operator!=(const HFNode& other) const { return !(*this == other); }

  bool overlap(const HFNode& other) const { return bv.overlap(other.bv); }
  bool overlap(const HFNode& other, const CollisionRequest& request,
               FCL_REAL& sqrDistLowerBound) const {
    return bv.overlap(other.bv, request, sqrDistLowerBound);
  }

  FCL_REAL distance(const HFNode& other, Vec3f* P1 = NULL,
                    Vec3f* P2 = NULL) const {
    return bv.distance(other.bv, P1, P2);
  }

  Vec3f getCenter() const { return bv.center(); }

  const Matrix3f& getOrientation() const {
    static const Matrix3f id3 = Matrix3f::Identity();
    return id3;
  }

  virtual ~HFNode() {}

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

namespace details {

template <typename BV>
struct UpdateBoundingVolume {
  static void run(const Vec3f& pointA, const Vec3f& pointB, BV& bv) {
    AABB bv_aabb(pointA, pointB);
    //      AABB bv_aabb;
    //      bv_aabb.update(pointA,pointB);
    convertBV(bv_aabb, bv);
  }
};

template <>
struct UpdateBoundingVolume<AABB> {
  static void run(const Vec3f& pointA, const Vec3f& pointB, AABB& bv) {
    AABB bv_aabb(pointA, pointB);
    convertBV(bv_aabb, bv);
    //      bv.update(pointA,pointB);
  }
};

}  // namespace details

template <typename BV>
class HPP_FCL_DLLAPI HeightField : public CollisionGeometry {
 public:
  typedef CollisionGeometry Base;

  typedef HFNode<BV> Node;
  typedef std::vector<Node> BVS;

  HeightField()
      : CollisionGeometry(),
        min_height((std::numeric_limits<FCL_REAL>::min)()),
        max_height((std::numeric_limits<FCL_REAL>::max)()) {}

  HeightField(const FCL_REAL x_dim, const FCL_REAL y_dim,
              const MatrixXf& heights, const FCL_REAL min_height = (FCL_REAL)0)
      : CollisionGeometry() {
    init(x_dim, y_dim, heights, min_height);
  }

  HeightField(const HeightField& other)
      : CollisionGeometry(other),
        x_dim(other.x_dim),
        y_dim(other.y_dim),
        heights(other.heights),
        min_height(other.min_height),
        max_height(other.max_height),
        x_grid(other.x_grid),
        y_grid(other.y_grid),
        bvs(other.bvs),
        num_bvs(other.num_bvs) {}

  const VecXf& getXGrid() const { return x_grid; }
  const VecXf& getYGrid() const { return y_grid; }

  const MatrixXf& getHeights() const { return heights; }

  FCL_REAL getXDim() const { return x_dim; }
  FCL_REAL getYDim() const { return y_dim; }

  FCL_REAL getMinHeight() const { return min_height; }
  FCL_REAL getMaxHeight() const { return max_height; }

  virtual HeightField<BV>* clone() const { return new HeightField(*this); }

  virtual ~HeightField() {}

  void computeLocalAABB() {
    const Vec3f A(x_grid[0], y_grid[0], min_height);
    const Vec3f B(x_grid[x_grid.size() - 1], y_grid[y_grid.size() - 1],
                  max_height);
    const AABB aabb_(A, B);

    aabb_radius = (A - B).norm() / 2.;
    aabb_local = aabb_;
    aabb_center = aabb_.center();
  }

  void updateHeights(const MatrixXf& new_heights) {
    if (new_heights.rows() != heights.rows() ||
        new_heights.cols() != heights.cols())
      HPP_FCL_THROW_PRETTY(
          "The matrix containing the new heights values does not have the same "
          "matrix size as the original one.\n"
          "\tinput values - rows: "
              << new_heights.rows() << " - cols: " << new_heights.cols() << "\n"
              << "\texpected values - rows: " << heights.rows()
              << " - cols: " << heights.cols() << "\n",
          std::invalid_argument);

    heights = new_heights.cwiseMax(min_height);
    this->max_height = recursiveUpdateHeight(0);
    assert(this->max_height == heights.maxCoeff());
  }

 protected:
  void init(const FCL_REAL x_dim, const FCL_REAL y_dim, const MatrixXf& heights,
            const FCL_REAL min_height) {
    this->x_dim = x_dim;
    this->y_dim = y_dim;
    this->heights = heights.cwiseMax(min_height);
    this->min_height = min_height;
    this->max_height = heights.maxCoeff();

    const Eigen::DenseIndex NX = heights.cols(), NY = heights.rows();
    assert(NX >= 2 && "The number of columns is too small.");
    assert(NY >= 2 && "The number of rows is too small.");

    x_grid = VecXf::LinSpaced(NX, -0.5 * x_dim, 0.5 * x_dim);
    y_grid = VecXf::LinSpaced(NY, 0.5 * y_dim, -0.5 * y_dim);

    // Allocate BVS
    const size_t num_tot_bvs =
        (size_t)(NX * NY) - 1 + (size_t)((NX - 1) * (NY - 1));
    bvs.resize(num_tot_bvs);
    num_bvs = 0;

    // Build tree
    buildTree();
  }

  OBJECT_TYPE getObjectType() const { return OT_HFIELD; }

  Vec3f computeCOM() const { return Vec3f::Zero(); }

  FCL_REAL computeVolume() const { return 0; }

  Matrix3f computeMomentofInertia() const { return Matrix3f::Zero(); }

 protected:
  FCL_REAL x_dim, y_dim;

  MatrixXf heights;

  FCL_REAL min_height, max_height;

  VecXf x_grid, y_grid;

  BVS bvs;
  unsigned int num_bvs;

  int buildTree() {
    num_bvs = 1;
    const FCL_REAL max_recursive_height =
        recursiveBuildTree(0, 0, heights.cols() - 1, 0, heights.rows() - 1);
    assert(max_recursive_height == max_height &&
           "the maximal height is not correct");
    HPP_FCL_UNUSED_VARIABLE(max_recursive_height);

    bvs.resize(num_bvs);
    return BVH_OK;
  }

  FCL_REAL recursiveUpdateHeight(const size_t bv_id) {
    HFNode<BV>& bv_node = bvs[bv_id];

    FCL_REAL max_height;
    if (bv_node.isLeaf()) {
      max_height = heights.block<2, 2>(bv_node.y_id, bv_node.x_id).maxCoeff();
    } else {
      FCL_REAL
      max_left_height = recursiveUpdateHeight(bv_node.leftChild()),
      max_right_height = recursiveUpdateHeight(bv_node.rightChild());

      max_height = (std::max)(max_left_height, max_right_height);
    }

    bv_node.max_height = max_height;

    const Vec3f pointA(x_grid[bv_node.x_id], y_grid[bv_node.y_id], min_height);
    const Vec3f pointB(x_grid[bv_node.x_id + bv_node.x_size],
                       y_grid[bv_node.y_id + bv_node.y_size], max_height);

    details::UpdateBoundingVolume<BV>::run(pointA, pointB, bv_node.bv);

    return max_height;
  }

  FCL_REAL recursiveBuildTree(const size_t bv_id, const Eigen::DenseIndex x_id,
                              const Eigen::DenseIndex x_size,
                              const Eigen::DenseIndex y_id,
                              const Eigen::DenseIndex y_size) {
    assert(x_id < heights.cols() && "x_id is out of bounds");
    assert(y_id < heights.rows() && "y_id is out of bounds");
    assert(x_size >= 0 && y_size >= 0 &&
           "x_size or y_size are not of correct value");
    assert(bv_id < bvs.size() && "bv_id exceeds the vector dimension");

    HFNode<BV>& bv_node = bvs[bv_id];
    FCL_REAL max_height;
    if (x_size == 1 &&
        y_size == 1)  // don't build any BV for the current child node
    {
      max_height = heights.block<2, 2>(y_id, x_id).maxCoeff();
    } else {
      bv_node.first_child = num_bvs;
      num_bvs += 2;

      FCL_REAL max_left_height = min_height, max_right_height = min_height;
      if (x_size >= y_size)  // splitting along the X axis
      {
        Eigen::DenseIndex x_size_half = x_size / 2;
        if (x_size == 1) x_size_half = 1;
        max_left_height = recursiveBuildTree(bv_node.leftChild(), x_id,
                                             x_size_half, y_id, y_size);

        max_right_height =
            recursiveBuildTree(bv_node.rightChild(), x_id + x_size_half,
                               x_size - x_size_half, y_id, y_size);
      } else  // splitting along the Y axis
      {
        Eigen::DenseIndex y_size_half = y_size / 2;
        if (y_size == 1) y_size_half = 1;
        max_left_height = recursiveBuildTree(bv_node.leftChild(), x_id, x_size,
                                             y_id, y_size_half);

        max_right_height =
            recursiveBuildTree(bv_node.rightChild(), x_id, x_size,
                               y_id + y_size_half, y_size - y_size_half);
      }

      max_height = (std::max)(max_left_height, max_right_height);
    }

    bv_node.max_height = max_height;
    //    max_height = std::max(max_height,min_height);

    const Vec3f pointA(x_grid[x_id], y_grid[y_id], min_height);
    assert(x_id + x_size < x_grid.size());
    assert(y_id + y_size < y_grid.size());
    const Vec3f pointB(x_grid[x_id + x_size], y_grid[y_id + y_size],
                       max_height);

    details::UpdateBoundingVolume<BV>::run(pointA, pointB, bv_node.bv);
    bv_node.x_id = x_id;
    bv_node.y_id = y_id;
    bv_node.x_size = x_size;
    bv_node.y_size = y_size;

    return max_height;
  }

 public:
  const HFNode<BV>& getBV(unsigned int i) const {
    if (i >= num_bvs)
      HPP_FCL_THROW_PRETTY("Index out of bounds", std::invalid_argument);
    return bvs[i];
  }

  HFNode<BV>& getBV(unsigned int i) {
    if (i >= num_bvs)
      HPP_FCL_THROW_PRETTY("Index out of bounds", std::invalid_argument);
    return bvs[i];
  }

  NODE_TYPE getNodeType() const { return BV_UNKNOWN; }

 private:
  virtual bool isEqual(const CollisionGeometry& _other) const {
    const HeightField* other_ptr = dynamic_cast<const HeightField*>(&_other);
    if (other_ptr == nullptr) return false;
    const HeightField& other = *other_ptr;

    return x_dim == other.x_dim && y_dim == other.y_dim &&
           heights == other.heights && min_height == other.min_height &&
           max_height == other.max_height && x_grid == other.x_grid &&
           y_grid == other.y_grid && bvs == other.bvs &&
           num_bvs == other.num_bvs;
  }

 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

template <>
NODE_TYPE HeightField<AABB>::getNodeType() const;

template <>
NODE_TYPE HeightField<OBB>::getNodeType() const;

template <>
NODE_TYPE HeightField<RSS>::getNodeType() const;

template <>
NODE_TYPE HeightField<kIOS>::getNodeType() const;

template <>
NODE_TYPE HeightField<OBBRSS>::getNodeType() const;

template <>
NODE_TYPE HeightField<KDOP<16> >::getNodeType() const;

template <>
NODE_TYPE HeightField<KDOP<18> >::getNodeType() const;

template <>
NODE_TYPE HeightField<KDOP<24> >::getNodeType() const;


}  // namespace fcl

}  // namespace hpp

#endif