Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/hpp/fcl/shape/geometric_shape_to_BVH_model.h	Lines:	187	188	99.5 %
Date:	2024-02-09 12:57:42	Branches:	154	236	65.3 %


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

/** \author Jia Pan */

#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 {

/// @brief Generate BVH model from box
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();
}

/// @brief Generate BVH model from sphere, given the number of segments along
/// longitude and number of rings along latitude.
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();
}

/// @brief Generate BVH model from sphere
/// The difference between generateBVHModel is that it gives the number of
/// triangles faces N for a sphere with unit radius. For sphere of radius r,
/// then the number of triangles is r * r * N so that the area represented by a
/// single triangle is approximately the same.s
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);
}

/// @brief Generate BVH model from cylinder, given the number of segments along
/// circle and the number of segments along axis.
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();
}

/// @brief Generate BVH model from cylinder
/// Difference from generateBVHModel: is that it gives the circle split number
/// tot for a cylinder with unit radius. For cylinder with larger radius, the
/// number of circle split number is r * tot.
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);
}

/// @brief Generate BVH model from cone, given the number of segments along
/// circle and the number of segments along axis.
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();
}

/// @brief Generate BVH model from cone
/// Difference from generateBVHModel: is that it gives the circle split number
/// tot for a cylinder with unit radius. For cone with larger radius, the number
/// of circle split number is r * tot.
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


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			/*
2			* Software License Agreement (BSD License)
3			*
4			* Copyright (c) 2011-2014, Willow Garage, Inc.
5			* Copyright (c) 2014-2015, Open Source Robotics Foundation
6			* All rights reserved.
7			*
8			* Redistribution and use in source and binary forms, with or without
9			* modification, are permitted provided that the following conditions
10			* are met:
11			*
12			* * Redistributions of source code must retain the above copyright
13			* notice, this list of conditions and the following disclaimer.
14			* * Redistributions in binary form must reproduce the above
15			* copyright notice, this list of conditions and the following
16			* disclaimer in the documentation and/or other materials provided
17			* with the distribution.
18			* * Neither the name of Open Source Robotics Foundation nor the names of its
19			* contributors may be used to endorse or promote products derived
20			* from this software without specific prior written permission.
21			*
22			* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23			* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24			* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25			* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26			* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27			* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28			* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29			* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30			* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31			* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32			* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33			* POSSIBILITY OF SUCH DAMAGE.
34			*/
35
36			/** \author Jia Pan */
37
38			#ifndef GEOMETRIC_SHAPE_TO_BVH_MODEL_H
39			#define GEOMETRIC_SHAPE_TO_BVH_MODEL_H
40
41			#include <hpp/fcl/shape/geometric_shapes.h>
42			#include <hpp/fcl/BVH/BVH_model.h>
43			#include <boost/math/constants/constants.hpp>
44
45			namespace hpp {
46			namespace fcl {
47
48			/// @brief Generate BVH model from box
49			template <typename BV>
50		1865	void generateBVHModel(BVHModel<BV>& model, const Box& shape,
51			const Transform3f& pose) {
52	✓✗	1865	FCL_REAL a = shape.halfSide[0];
53	✓✗	1865	FCL_REAL b = shape.halfSide[1];
54	✓✗	1865	FCL_REAL c = shape.halfSide[2];
55	✓✗	3730	std::vector<Vec3f> points(8);
56	✓✗	3730	std::vector<Triangle> tri_indices(12);
57	✓✗	1865	points[0] = Vec3f(a, -b, c);
58	✓✗	1865	points[1] = Vec3f(a, b, c);
59	✓✗	1865	points[2] = Vec3f(-a, b, c);
60	✓✗	1865	points[3] = Vec3f(-a, -b, c);
61	✓✗	1865	points[4] = Vec3f(a, -b, -c);
62	✓✗	1865	points[5] = Vec3f(a, b, -c);
63	✓✗	1865	points[6] = Vec3f(-a, b, -c);
64	✓✗	1865	points[7] = Vec3f(-a, -b, -c);
65
66		1865	tri_indices[0].set(0, 4, 1);
67		1865	tri_indices[1].set(1, 4, 5);
68		1865	tri_indices[2].set(2, 6, 3);
69		1865	tri_indices[3].set(3, 6, 7);
70		1865	tri_indices[4].set(3, 0, 2);
71		1865	tri_indices[5].set(2, 0, 1);
72		1865	tri_indices[6].set(6, 5, 7);
73		1865	tri_indices[7].set(7, 5, 4);
74		1865	tri_indices[8].set(1, 5, 2);
75		1865	tri_indices[9].set(2, 5, 6);
76		1865	tri_indices[10].set(3, 7, 0);
77		1865	tri_indices[11].set(0, 7, 4);
78
79	✓✓	16785	for (unsigned int i = 0; i < points.size(); ++i) {
80	✓✗✓✗	14920	points[i] = pose.transform(points[i]).eval();
81			}
82
83	✓✗	1865	model.beginModel();
84	✓✗	1865	model.addSubModel(points, tri_indices);
85	✓✗	1865	model.endModel();
86	✓✗	1865	model.computeLocalAABB();
87		1865	}
88
89			/// @brief Generate BVH model from sphere, given the number of segments along
90			/// longitude and number of rings along latitude.
91			template <typename BV>
92		369	void generateBVHModel(BVHModel<BV>& model, const Sphere& shape,
93			const Transform3f& pose, unsigned int seg,
94			unsigned int ring) {
95		738	std::vector<Vec3f> points;
96		738	std::vector<Triangle> tri_indices;
97
98		369	FCL_REAL r = shape.radius;
99			FCL_REAL phi, phid;
100		369	const FCL_REAL pi = boost::math::constants::pi<FCL_REAL>();
101		369	phid = pi * 2 / seg;
102		369	phi = 0;
103
104			FCL_REAL theta, thetad;
105		369	thetad = pi / (ring + 1);
106		369	theta = 0;
107
108	✓✓	6256	for (unsigned int i = 0; i < ring; ++i) {
109		5887	FCL_REAL theta_ = theta + thetad * (i + 1);
110	✓✓	99964	for (unsigned int j = 0; j < seg; ++j) {
111	✓✗	94077	points.push_back(Vec3f(r * sin(theta_) * cos(phi + j * phid),
112			r * sin(theta_) * sin(phi + j * phid),
113	✓✗	188154	r * cos(theta_)));
114			}
115			}
116	✓✗✓✗	369	points.push_back(Vec3f(0, 0, r));
117	✓✗✓✗	369	points.push_back(Vec3f(0, 0, -r));
118
119	✓✓	5887	for (unsigned int i = 0; i < ring - 1; ++i) {
120	✓✓	93708	for (unsigned int j = 0; j < seg; ++j) {
121			unsigned int a, b, c, d;
122		88190	a = i * seg + j;
123	✓✓	88190	b = (j == seg - 1) ? (i * seg) : (i * seg + j + 1);
124		88190	c = (i + 1) * seg + j;
125	✓✓	88190	d = (j == seg - 1) ? ((i + 1) * seg) : ((i + 1) * seg + j + 1);
126	✓✗✓✗	88190	tri_indices.push_back(Triangle(a, c, b));
127	✓✗✓✗	88190	tri_indices.push_back(Triangle(b, c, d));
128			}
129			}
130
131	✓✓	6256	for (unsigned int j = 0; j < seg; ++j) {
132			unsigned int a, b;
133		5887	a = j;
134	✓✓	5887	b = (j == seg - 1) ? 0 : (j + 1);
135	✓✗✓✗	5887	tri_indices.push_back(Triangle(ring * seg, a, b));
136
137		5887	a = (ring - 1) * seg + j;
138	✓✓	5887	b = (j == seg - 1) ? (ring - 1) * seg : ((ring - 1) * seg + j + 1);
139	✓✗✓✗	5887	tri_indices.push_back(Triangle(a, ring * seg + 1, b));
140			}
141
142	✓✓	95184	for (unsigned int i = 0; i < points.size(); ++i) {
143	✓✗	94815	points[i] = pose.transform(points[i]);
144			}
145
146	✓✗	369	model.beginModel();
147	✓✗	369	model.addSubModel(points, tri_indices);
148	✓✗	369	model.endModel();
149	✓✗	369	model.computeLocalAABB();
150		369	}
151
152			/// @brief Generate BVH model from sphere
153			/// The difference between generateBVHModel is that it gives the number of
154			/// triangles faces N for a sphere with unit radius. For sphere of radius r,
155			/// then the number of triangles is r * r * N so that the area represented by a
156			/// single triangle is approximately the same.s
157			template <typename BV>
158		1	void generateBVHModel(BVHModel<BV>& model, const Sphere& shape,
159			const Transform3f& pose,
160			unsigned int n_faces_for_unit_sphere) {
161		1	FCL_REAL r = shape.radius;
162		1	FCL_REAL n_low_bound =
163		1	std::sqrt((FCL_REAL)n_faces_for_unit_sphere / FCL_REAL(2.)) * r * r;
164		1	unsigned int ring = (unsigned int)ceil(n_low_bound);
165		1	unsigned int seg = (unsigned int)ceil(n_low_bound);
166
167		1	generateBVHModel(model, shape, pose, seg, ring);
168		1	}
169
170			/// @brief Generate BVH model from cylinder, given the number of segments along
171			/// circle and the number of segments along axis.
172			template <typename BV>
173		369	void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape,
174			const Transform3f& pose, unsigned int tot,
175			unsigned int h_num) {
176		738	std::vector<Vec3f> points;
177		738	std::vector<Triangle> tri_indices;
178
179		369	FCL_REAL r = shape.radius;
180		369	FCL_REAL h = shape.halfLength;
181			FCL_REAL phi, phid;
182		369	const FCL_REAL pi = boost::math::constants::pi<FCL_REAL>();
183		369	phid = pi * 2 / tot;
184		369	phi = 0;
185
186		369	FCL_REAL hd = 2 * h / h_num;
187
188	✓✓	6301	for (unsigned int i = 0; i < tot; ++i)
189	✓✗	5932	points.push_back(
190	✓✗	11864	Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), h));
191
192	✓✓	5890	for (unsigned int i = 0; i < h_num - 1; ++i) {
193	✓✓	94041	for (unsigned int j = 0; j < tot; ++j) {
194	✓✗	88520	points.push_back(Vec3f(r * cos(phi + phid * j), r * sin(phi + phid * j),
195	✓✗	177040	h - (i + 1) * hd));
196			}
197			}
198
199	✓✓	6301	for (unsigned int i = 0; i < tot; ++i)
200	✓✗	5932	points.push_back(
201	✓✗	11864	Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), -h));
202
203	✓✗✓✗	369	points.push_back(Vec3f(0, 0, h));
204	✓✗✓✗	369	points.push_back(Vec3f(0, 0, -h));
205
206	✓✓	6301	for (unsigned int i = 0; i < tot; ++i) {
207	✓✓✓✗	5932	Triangle tmp((h_num + 1) * tot, i, ((i == tot - 1) ? 0 : (i + 1)));
208	✓✗	5932	tri_indices.push_back(tmp);
209			}
210
211	✓✓	6301	for (unsigned int i = 0; i < tot; ++i) {
212		11864	Triangle tmp((h_num + 1) * tot + 1,
213	✓✓✓✗	5932	h_num * tot + ((i == tot - 1) ? 0 : (i + 1)), h_num * tot + i);
214	✓✗	5932	tri_indices.push_back(tmp);
215			}
216
217	✓✓	6259	for (unsigned int i = 0; i < h_num; ++i) {
218	✓✓	100342	for (unsigned int j = 0; j < tot; ++j) {
219			unsigned int a, b, c, d;
220		94452	a = j;
221	✓✓	94452	b = (j == tot - 1) ? 0 : (j + 1);
222		94452	c = j + tot;
223	✓✓	94452	d = (j == tot - 1) ? tot : (j + 1 + tot);
224
225		94452	unsigned int start = i * tot;
226	✓✗✓✗	94452	tri_indices.push_back(Triangle(start + b, start + a, start + c));
227	✓✗✓✗	94452	tri_indices.push_back(Triangle(start + b, start + c, start + d));
228			}
229			}
230
231	✓✓	101491	for (unsigned int i = 0; i < points.size(); ++i) {
232	✓✗	101122	points[i] = pose.transform(points[i]);
233			}
234
235	✓✗	369	model.beginModel();
236	✓✗	369	model.addSubModel(points, tri_indices);
237	✓✗	369	model.endModel();
238	✓✗	369	model.computeLocalAABB();
239		369	}
240
241			/// @brief Generate BVH model from cylinder
242			/// Difference from generateBVHModel: is that it gives the circle split number
243			/// tot for a cylinder with unit radius. For cylinder with larger radius, the
244			/// number of circle split number is r * tot.
245			template <typename BV>
246		1	void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape,
247			const Transform3f& pose,
248			unsigned int tot_for_unit_cylinder) {
249		1	FCL_REAL r = shape.radius;
250		1	FCL_REAL h = 2 * shape.halfLength;
251
252		1	const FCL_REAL pi = boost::math::constants::pi<FCL_REAL>();
253		1	unsigned int tot = (unsigned int)(tot_for_unit_cylinder * r);
254		1	FCL_REAL phid = pi * 2 / tot;
255
256		1	FCL_REAL circle_edge = phid * r;
257		1	unsigned int h_num = (unsigned int)ceil(h / circle_edge);
258
259		1	generateBVHModel(model, shape, pose, tot, h_num);
260		1	}
261
262			/// @brief Generate BVH model from cone, given the number of segments along
263			/// circle and the number of segments along axis.
264			template <typename BV>
265		2	void generateBVHModel(BVHModel<BV>& model, const Cone& shape,
266			const Transform3f& pose, unsigned int tot,
267			unsigned int h_num) {
268		4	std::vector<Vec3f> points;
269		4	std::vector<Triangle> tri_indices;
270
271		2	FCL_REAL r = shape.radius;
272		2	FCL_REAL h = shape.halfLength;
273
274			FCL_REAL phi, phid;
275		2	const FCL_REAL pi = boost::math::constants::pi<FCL_REAL>();
276		2	phid = pi * 2 / tot;
277		2	phi = 0;
278
279		2	FCL_REAL hd = 2 * h / h_num;
280
281	✓✓	18	for (unsigned int i = 0; i < h_num - 1; ++i) {
282		16	FCL_REAL h_i = h - (i + 1) * hd;
283		16	FCL_REAL rh = r * (0.5 - h_i / h / 2);
284	✓✓	456	for (unsigned int j = 0; j < tot; ++j) {
285	✓✗	440	points.push_back(
286	✓✗	880	Vec3f(rh * cos(phi + phid * j), rh * sin(phi + phid * j), h_i));
287			}
288			}
289
290	✓✓	62	for (unsigned int i = 0; i < tot; ++i)
291	✓✗	60	points.push_back(
292	✓✗	120	Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), -h));
293
294	✓✗✓✗	2	points.push_back(Vec3f(0, 0, h));
295	✓✗✓✗	2	points.push_back(Vec3f(0, 0, -h));
296
297	✓✓	62	for (unsigned int i = 0; i < tot; ++i) {
298	✓✓✓✗	60	Triangle tmp(h_num * tot, i, (i == tot - 1) ? 0 : (i + 1));
299	✓✗	60	tri_indices.push_back(tmp);
300			}
301
302	✓✓	62	for (unsigned int i = 0; i < tot; ++i) {
303		120	Triangle tmp(h_num * tot + 1,
304	✓✗	60	(h_num - 1) * tot + ((i == tot - 1) ? 0 : (i + 1)),
305	✓✓	60	(h_num - 1) * tot + i);
306	✓✗	60	tri_indices.push_back(tmp);
307			}
308
309	✓✓	18	for (unsigned int i = 0; i < h_num - 1; ++i) {
310	✓✓	456	for (unsigned int j = 0; j < tot; ++j) {
311			unsigned int a, b, c, d;
312		440	a = j;
313	✓✓	440	b = (j == tot - 1) ? 0 : (j + 1);
314		440	c = j + tot;
315	✓✓	440	d = (j == tot - 1) ? tot : (j + 1 + tot);
316
317		440	unsigned int start = i * tot;
318	✓✗✓✗	440	tri_indices.push_back(Triangle(start + b, start + a, start + c));
319	✓✗✓✗	440	tri_indices.push_back(Triangle(start + b, start + c, start + d));
320			}
321			}
322
323	✓✓	506	for (unsigned int i = 0; i < points.size(); ++i) {
324	✓✗	504	points[i] = pose.transform(points[i]);
325			}
326
327	✓✗	2	model.beginModel();
328	✓✗	2	model.addSubModel(points, tri_indices);
329	✓✗	2	model.endModel();
330	✓✗	2	model.computeLocalAABB();
331		2	}
332
333			/// @brief Generate BVH model from cone
334			/// Difference from generateBVHModel: is that it gives the circle split number
335			/// tot for a cylinder with unit radius. For cone with larger radius, the number
336			/// of circle split number is r * tot.
337			template <typename BV>
338		1	void generateBVHModel(BVHModel<BV>& model, const Cone& shape,
339			const Transform3f& pose, unsigned int tot_for_unit_cone) {
340		1	FCL_REAL r = shape.radius;
341		1	FCL_REAL h = 2 * shape.halfLength;
342
343		1	const FCL_REAL pi = boost::math::constants::pi<FCL_REAL>();
344		1	unsigned int tot = (unsigned int)(tot_for_unit_cone * r);
345		1	FCL_REAL phid = pi * 2 / tot;
346
347		1	FCL_REAL circle_edge = phid * r;
348		1	unsigned int h_num = (unsigned int)ceil(h / circle_edge);
349
350		1	generateBVHModel(model, shape, pose, tot, h_num);
351		1	}
352
353			} // namespace fcl
354
355			} // namespace hpp
356
357			#endif