.. _program_listing_file__tmp_ws_src_hpp-fcl_include_hpp_fcl_hfield.h:

Program Listing for File hfield.h
=================================

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

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

.. code-block:: cpp

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