.. _program_listing_file__tmp_ws_src_hpp-fcl_include_hpp_fcl_shape_geometric_shape_to_BVH_model.h:

Program Listing for File geometric_shape_to_BVH_model.h
=======================================================

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

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

.. code-block:: cpp

   /*
    * 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 GEOMETRIC_SHAPE_TO_BVH_MODEL_H
   #define GEOMETRIC_SHAPE_TO_BVH_MODEL_H
   
   #include <hpp/fcl/shape/geometric_shapes.h>
   #include <hpp/fcl/BVH/BVH_model.h>
   #include <boost/math/constants/constants.hpp>
   
   namespace hpp {
   namespace fcl {
   
   template <typename BV>
   void generateBVHModel(BVHModel<BV>& model, const Box& shape,
                         const Transform3f& pose) {
     FCL_REAL a = shape.halfSide[0];
     FCL_REAL b = shape.halfSide[1];
     FCL_REAL c = shape.halfSide[2];
     std::vector<Vec3f> points(8);
     std::vector<Triangle> tri_indices(12);
     points[0] = Vec3f(a, -b, c);
     points[1] = Vec3f(a, b, c);
     points[2] = Vec3f(-a, b, c);
     points[3] = Vec3f(-a, -b, c);
     points[4] = Vec3f(a, -b, -c);
     points[5] = Vec3f(a, b, -c);
     points[6] = Vec3f(-a, b, -c);
     points[7] = Vec3f(-a, -b, -c);
   
     tri_indices[0].set(0, 4, 1);
     tri_indices[1].set(1, 4, 5);
     tri_indices[2].set(2, 6, 3);
     tri_indices[3].set(3, 6, 7);
     tri_indices[4].set(3, 0, 2);
     tri_indices[5].set(2, 0, 1);
     tri_indices[6].set(6, 5, 7);
     tri_indices[7].set(7, 5, 4);
     tri_indices[8].set(1, 5, 2);
     tri_indices[9].set(2, 5, 6);
     tri_indices[10].set(3, 7, 0);
     tri_indices[11].set(0, 7, 4);
   
     for (unsigned int i = 0; i < points.size(); ++i) {
       points[i] = pose.transform(points[i]).eval();
     }
   
     model.beginModel();
     model.addSubModel(points, tri_indices);
     model.endModel();
     model.computeLocalAABB();
   }
   
   template <typename BV>
   void generateBVHModel(BVHModel<BV>& model, const Sphere& shape,
                         const Transform3f& pose, unsigned int seg,
                         unsigned int ring) {
     std::vector<Vec3f> points;
     std::vector<Triangle> tri_indices;
   
     FCL_REAL r = shape.radius;
     FCL_REAL phi, phid;
     const FCL_REAL pi = boost::math::constants::pi<FCL_REAL>();
     phid = pi * 2 / seg;
     phi = 0;
   
     FCL_REAL theta, thetad;
     thetad = pi / (ring + 1);
     theta = 0;
   
     for (unsigned int i = 0; i < ring; ++i) {
       FCL_REAL theta_ = theta + thetad * (i + 1);
       for (unsigned int j = 0; j < seg; ++j) {
         points.push_back(Vec3f(r * sin(theta_) * cos(phi + j * phid),
                                r * sin(theta_) * sin(phi + j * phid),
                                r * cos(theta_)));
       }
     }
     points.push_back(Vec3f(0, 0, r));
     points.push_back(Vec3f(0, 0, -r));
   
     for (unsigned int i = 0; i < ring - 1; ++i) {
       for (unsigned int j = 0; j < seg; ++j) {
         unsigned int a, b, c, d;
         a = i * seg + j;
         b = (j == seg - 1) ? (i * seg) : (i * seg + j + 1);
         c = (i + 1) * seg + j;
         d = (j == seg - 1) ? ((i + 1) * seg) : ((i + 1) * seg + j + 1);
         tri_indices.push_back(Triangle(a, c, b));
         tri_indices.push_back(Triangle(b, c, d));
       }
     }
   
     for (unsigned int j = 0; j < seg; ++j) {
       unsigned int a, b;
       a = j;
       b = (j == seg - 1) ? 0 : (j + 1);
       tri_indices.push_back(Triangle(ring * seg, a, b));
   
       a = (ring - 1) * seg + j;
       b = (j == seg - 1) ? (ring - 1) * seg : ((ring - 1) * seg + j + 1);
       tri_indices.push_back(Triangle(a, ring * seg + 1, b));
     }
   
     for (unsigned int i = 0; i < points.size(); ++i) {
       points[i] = pose.transform(points[i]);
     }
   
     model.beginModel();
     model.addSubModel(points, tri_indices);
     model.endModel();
     model.computeLocalAABB();
   }
   
   template <typename BV>
   void generateBVHModel(BVHModel<BV>& model, const Sphere& shape,
                         const Transform3f& pose,
                         unsigned int n_faces_for_unit_sphere) {
     FCL_REAL r = shape.radius;
     FCL_REAL n_low_bound =
         std::sqrt((FCL_REAL)n_faces_for_unit_sphere / FCL_REAL(2.)) * r * r;
     unsigned int ring = (unsigned int)ceil(n_low_bound);
     unsigned int seg = (unsigned int)ceil(n_low_bound);
   
     generateBVHModel(model, shape, pose, seg, ring);
   }
   
   template <typename BV>
   void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape,
                         const Transform3f& pose, unsigned int tot,
                         unsigned int h_num) {
     std::vector<Vec3f> points;
     std::vector<Triangle> tri_indices;
   
     FCL_REAL r = shape.radius;
     FCL_REAL h = shape.halfLength;
     FCL_REAL phi, phid;
     const FCL_REAL pi = boost::math::constants::pi<FCL_REAL>();
     phid = pi * 2 / tot;
     phi = 0;
   
     FCL_REAL hd = 2 * h / h_num;
   
     for (unsigned int i = 0; i < tot; ++i)
       points.push_back(
           Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), h));
   
     for (unsigned int i = 0; i < h_num - 1; ++i) {
       for (unsigned int j = 0; j < tot; ++j) {
         points.push_back(Vec3f(r * cos(phi + phid * j), r * sin(phi + phid * j),
                                h - (i + 1) * hd));
       }
     }
   
     for (unsigned int i = 0; i < tot; ++i)
       points.push_back(
           Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), -h));
   
     points.push_back(Vec3f(0, 0, h));
     points.push_back(Vec3f(0, 0, -h));
   
     for (unsigned int i = 0; i < tot; ++i) {
       Triangle tmp((h_num + 1) * tot, i, ((i == tot - 1) ? 0 : (i + 1)));
       tri_indices.push_back(tmp);
     }
   
     for (unsigned int i = 0; i < tot; ++i) {
       Triangle tmp((h_num + 1) * tot + 1,
                    h_num * tot + ((i == tot - 1) ? 0 : (i + 1)), h_num * tot + i);
       tri_indices.push_back(tmp);
     }
   
     for (unsigned int i = 0; i < h_num; ++i) {
       for (unsigned int j = 0; j < tot; ++j) {
         unsigned int a, b, c, d;
         a = j;
         b = (j == tot - 1) ? 0 : (j + 1);
         c = j + tot;
         d = (j == tot - 1) ? tot : (j + 1 + tot);
   
         unsigned int start = i * tot;
         tri_indices.push_back(Triangle(start + b, start + a, start + c));
         tri_indices.push_back(Triangle(start + b, start + c, start + d));
       }
     }
   
     for (unsigned int i = 0; i < points.size(); ++i) {
       points[i] = pose.transform(points[i]);
     }
   
     model.beginModel();
     model.addSubModel(points, tri_indices);
     model.endModel();
     model.computeLocalAABB();
   }
   
   template <typename BV>
   void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape,
                         const Transform3f& pose,
                         unsigned int tot_for_unit_cylinder) {
     FCL_REAL r = shape.radius;
     FCL_REAL h = 2 * shape.halfLength;
   
     const FCL_REAL pi = boost::math::constants::pi<FCL_REAL>();
     unsigned int tot = (unsigned int)(tot_for_unit_cylinder * r);
     FCL_REAL phid = pi * 2 / tot;
   
     FCL_REAL circle_edge = phid * r;
     unsigned int h_num = (unsigned int)ceil(h / circle_edge);
   
     generateBVHModel(model, shape, pose, tot, h_num);
   }
   
   template <typename BV>
   void generateBVHModel(BVHModel<BV>& model, const Cone& shape,
                         const Transform3f& pose, unsigned int tot,
                         unsigned int h_num) {
     std::vector<Vec3f> points;
     std::vector<Triangle> tri_indices;
   
     FCL_REAL r = shape.radius;
     FCL_REAL h = shape.halfLength;
   
     FCL_REAL phi, phid;
     const FCL_REAL pi = boost::math::constants::pi<FCL_REAL>();
     phid = pi * 2 / tot;
     phi = 0;
   
     FCL_REAL hd = 2 * h / h_num;
   
     for (unsigned int i = 0; i < h_num - 1; ++i) {
       FCL_REAL h_i = h - (i + 1) * hd;
       FCL_REAL rh = r * (0.5 - h_i / h / 2);
       for (unsigned int j = 0; j < tot; ++j) {
         points.push_back(
             Vec3f(rh * cos(phi + phid * j), rh * sin(phi + phid * j), h_i));
       }
     }
   
     for (unsigned int i = 0; i < tot; ++i)
       points.push_back(
           Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), -h));
   
     points.push_back(Vec3f(0, 0, h));
     points.push_back(Vec3f(0, 0, -h));
   
     for (unsigned int i = 0; i < tot; ++i) {
       Triangle tmp(h_num * tot, i, (i == tot - 1) ? 0 : (i + 1));
       tri_indices.push_back(tmp);
     }
   
     for (unsigned int i = 0; i < tot; ++i) {
       Triangle tmp(h_num * tot + 1,
                    (h_num - 1) * tot + ((i == tot - 1) ? 0 : (i + 1)),
                    (h_num - 1) * tot + i);
       tri_indices.push_back(tmp);
     }
   
     for (unsigned int i = 0; i < h_num - 1; ++i) {
       for (unsigned int j = 0; j < tot; ++j) {
         unsigned int a, b, c, d;
         a = j;
         b = (j == tot - 1) ? 0 : (j + 1);
         c = j + tot;
         d = (j == tot - 1) ? tot : (j + 1 + tot);
   
         unsigned int start = i * tot;
         tri_indices.push_back(Triangle(start + b, start + a, start + c));
         tri_indices.push_back(Triangle(start + b, start + c, start + d));
       }
     }
   
     for (unsigned int i = 0; i < points.size(); ++i) {
       points[i] = pose.transform(points[i]);
     }
   
     model.beginModel();
     model.addSubModel(points, tri_indices);
     model.endModel();
     model.computeLocalAABB();
   }
   
   template <typename BV>
   void generateBVHModel(BVHModel<BV>& model, const Cone& shape,
                         const Transform3f& pose, unsigned int tot_for_unit_cone) {
     FCL_REAL r = shape.radius;
     FCL_REAL h = 2 * shape.halfLength;
   
     const FCL_REAL pi = boost::math::constants::pi<FCL_REAL>();
     unsigned int tot = (unsigned int)(tot_for_unit_cone * r);
     FCL_REAL phid = pi * 2 / tot;
   
     FCL_REAL circle_edge = phid * r;
     unsigned int h_num = (unsigned int)ceil(h / circle_edge);
   
     generateBVHModel(model, shape, pose, tot, h_num);
   }
   
   }  // namespace fcl
   
   }  // namespace hpp
   
   #endif