From 40a150e7c6243b0f6b38d3aa10b10c494ef12719 Mon Sep 17 00:00:00 2001
From: jpan <jpan@253336fb-580f-4252-a368-f3cef5a2a82b>
Date: Mon, 5 Dec 2011 03:31:26 +0000
Subject: [PATCH] Add interfaces for generating simple model
git-svn-id: https://kforge.ros.org/fcl/fcl_ros@46 253336fb-580f-4252-a368-f3cef5a2a82b
---
trunk/fcl/include/fcl/BV_fitter.h | 3 +
trunk/fcl/include/fcl/BV_splitter.h | 3 +
trunk/fcl/include/fcl/collision_object.h | 3 -
.../fcl/geometric_shape_to_BVH_model.h | 155 ++++++++++++++++++
trunk/fcl/src/BVH_model.cpp | 6 +-
trunk/fcl/src/geometric_shapes_utility.cpp | 47 +++---
.../test_core_conservative_advancement.cpp | 7 +-
7 files changed, 188 insertions(+), 36 deletions(-)
diff --git a/trunk/fcl/include/fcl/BV_fitter.h b/trunk/fcl/include/fcl/BV_fitter.h
index 9b583af1..e8238b01 100644
--- a/trunk/fcl/include/fcl/BV_fitter.h
+++ b/trunk/fcl/include/fcl/BV_fitter.h
@@ -84,6 +84,9 @@ template<typename BV>
class BVFitter : public BVFitterBase<BV>
{
public:
+ /** \brief default deconstructor */
+ virtual ~BVFitter() {}
+
/** \brief Prepare the geometry primitive data for fitting */
void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_)
{
diff --git a/trunk/fcl/include/fcl/BV_splitter.h b/trunk/fcl/include/fcl/BV_splitter.h
index 61a7d37b..e1370004 100644
--- a/trunk/fcl/include/fcl/BV_splitter.h
+++ b/trunk/fcl/include/fcl/BV_splitter.h
@@ -82,6 +82,9 @@ public:
split_method = method;
}
+ /** \brief Default deconstructor */
+ virtual ~BVSplitter() {}
+
/** \brief Set the geometry data needed by the split rule */
void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_)
{
diff --git a/trunk/fcl/include/fcl/collision_object.h b/trunk/fcl/include/fcl/collision_object.h
index 22779b8c..845b233a 100644
--- a/trunk/fcl/include/fcl/collision_object.h
+++ b/trunk/fcl/include/fcl/collision_object.h
@@ -139,9 +139,6 @@ protected:
/** AABB in global coordinate */
mutable AABB aabb;
- /** AABB in local coordinate */
- AABB aabb_local;
-
/** AABB center in local coordinate */
Vec3f aabb_center;
diff --git a/trunk/fcl/include/fcl/geometric_shape_to_BVH_model.h b/trunk/fcl/include/fcl/geometric_shape_to_BVH_model.h
index a83182a3..18926dd5 100644
--- a/trunk/fcl/include/fcl/geometric_shape_to_BVH_model.h
+++ b/trunk/fcl/include/fcl/geometric_shape_to_BVH_model.h
@@ -157,6 +157,81 @@ void generateBVHModel(BVHModel<BV>& model, const Sphere& shape, unsigned int seg
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 generateBVHModel2(BVHModel<BV>& model, const Sphere& shape, unsigned int n_faces_for_unit_sphere)
+{
+ std::vector<Vec3f> points;
+ std::vector<Triangle> tri_indices;
+
+ double r = shape.radius;
+
+ double n_low_bound = sqrtf(n_faces_for_unit_sphere / 2.0) * r * r;
+ unsigned int ring = ceil(n_low_bound);
+ unsigned int seg = ceil(n_low_bound);
+
+ double phi, phid;
+ const double pi = boost::math::constants::pi<double>();
+ phid = pi * 2 / seg;
+ phi = 0;
+
+ double theta, thetad;
+ thetad = pi / (ring + 1);
+ theta = 0;
+
+ for(unsigned int i = 0; i < ring; ++i)
+ {
+ double 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)
+ {
+ Vec3f v = matMulVec(shape.getLocalRotation(), points[i]) + shape.getLocalTranslation();
+ v = matMulVec(shape.getRotation(), v) + shape.getTranslation();
+ points[i] = v;
+ }
+
+ model.beginModel();
+ model.addSubModel(points, tri_indices);
+ model.endModel();
+ model.computeLocalAABB();
+}
+
/** \brief Generate BVH model from cylinder */
template<typename BV>
@@ -234,6 +309,86 @@ void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape, unsigned int t
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 generateBVHModel2(BVHModel<BV>& model, const Cylinder& shape, unsigned int tot_for_unit_cylinder)
+{
+ std::vector<Vec3f> points;
+ std::vector<Triangle> tri_indices;
+
+ double r = shape.radius;
+ double h = shape.lz;
+ double phi, phid;
+ const double pi = boost::math::constants::pi<double>();
+ unsigned int tot = tot_for_unit_cylinder * r;
+ phid = pi * 2 / tot;
+ phi = 0;
+
+ double circle_edge = phid * r;
+ unsigned int h_num = ceil(h / circle_edge);
+ double hd = 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 / 2));
+
+ 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 / 2 - (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 / 2));
+
+ points.push_back(Vec3f(0, 0, h / 2));
+ points.push_back(Vec3f(0, 0, -h / 2));
+
+ 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)
+ {
+ 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);
+
+ 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)
+ {
+ Vec3f v = matMulVec(shape.getLocalRotation(), points[i]) + shape.getLocalTranslation();
+ v = matMulVec(shape.getRotation(), v) + shape.getTranslation();
+ points[i] = v;
+ }
+
+ model.beginModel();
+ model.addSubModel(points, tri_indices);
+ model.endModel();
+ model.computeLocalAABB();
+}
+
/** \brief Generate BVH model from cone */
template<typename BV>
diff --git a/trunk/fcl/src/BVH_model.cpp b/trunk/fcl/src/BVH_model.cpp
index 3e2343f5..25fb62f9 100644
--- a/trunk/fcl/src/BVH_model.cpp
+++ b/trunk/fcl/src/BVH_model.cpp
@@ -861,11 +861,9 @@ void BVHModel<BV>::computeLocalAABB()
aabb_ += vertices[i];
}
- aabb_local = aabb_;
+ aabb = aabb_;
- aabb = aabb_local;
-
- aabb_center = aabb_local.center();
+ aabb_center = aabb.center();
aabb_radius = 0;
for(int i = 0; i < num_vertices; ++i)
diff --git a/trunk/fcl/src/geometric_shapes_utility.cpp b/trunk/fcl/src/geometric_shapes_utility.cpp
index e1af3def..d0261cb0 100644
--- a/trunk/fcl/src/geometric_shapes_utility.cpp
+++ b/trunk/fcl/src/geometric_shapes_utility.cpp
@@ -293,58 +293,51 @@ void computeBV<OBB>(const Plane& s, OBB& bv)
void Box::computeLocalAABB()
{
- computeBV<AABB>(*this, aabb_local);
- aabb = aabb_local;
- aabb_center = aabb_local.center();
- aabb_radius = (aabb_local.min_ - aabb_center).length();
+ computeBV<AABB>(*this, aabb);
+ aabb_center = aabb.center();
+ aabb_radius = (aabb.min_ - aabb_center).length();
}
void Sphere::computeLocalAABB()
{
- computeBV<AABB>(*this, aabb_local);
- aabb = aabb_local;
- aabb_center = aabb_local.center();
+ computeBV<AABB>(*this, aabb);
+ aabb_center = aabb.center();
aabb_radius = radius;
}
void Capsule::computeLocalAABB()
{
- computeBV<AABB>(*this, aabb_local);
- aabb = aabb_local;
- aabb_center = aabb_local.center();
- aabb_radius = (aabb_local.min_ - aabb_center).length();
+ computeBV<AABB>(*this, aabb);
+ aabb_center = aabb.center();
+ aabb_radius = (aabb.min_ - aabb_center).length();
}
void Cone::computeLocalAABB()
{
- computeBV<AABB>(*this, aabb_local);
- aabb = aabb_local;
- aabb_center = aabb_local.center();
- aabb_radius = (aabb_local.min_ - aabb_center).length();
+ computeBV<AABB>(*this, aabb);
+ aabb_center = aabb.center();
+ aabb_radius = (aabb.min_ - aabb_center).length();
}
void Cylinder::computeLocalAABB()
{
- computeBV<AABB>(*this, aabb_local);
- aabb = aabb_local;
- aabb_center = aabb_local.center();
- aabb_radius = (aabb_local.min_ - aabb_center).length();
+ computeBV<AABB>(*this, aabb);
+ aabb_center = aabb.center();
+ aabb_radius = (aabb.min_ - aabb_center).length();
}
void Convex::computeLocalAABB()
{
- computeBV<AABB>(*this, aabb_local);
- aabb = aabb_local;
- aabb_center = aabb_local.center();
- aabb_radius = (aabb_local.min_ - aabb_center).length();
+ computeBV<AABB>(*this, aabb);
+ aabb_center = aabb.center();
+ aabb_radius = (aabb.min_ - aabb_center).length();
}
void Plane::computeLocalAABB()
{
- computeBV<AABB>(*this, aabb_local);
- aabb = aabb_local;
- aabb_center = aabb_local.center();
- aabb_radius = (aabb_local.min_ - aabb_center).length();
+ computeBV<AABB>(*this, aabb);
+ aabb_center = aabb.center();
+ aabb_radius = (aabb.min_ - aabb_center).length();
}
diff --git a/trunk/fcl/test/test_core_conservative_advancement.cpp b/trunk/fcl/test/test_core_conservative_advancement.cpp
index 57c6d134..2525e1bf 100644
--- a/trunk/fcl/test/test_core_conservative_advancement.cpp
+++ b/trunk/fcl/test/test_core_conservative_advancement.cpp
@@ -88,7 +88,7 @@ bool spline_interp_Test(const Transform& tf1, const Transform& tf2,
BVH_REAL& toc);
-unsigned int n_dcd_samples = 1000;
+unsigned int n_dcd_samples = 1000000;
int main()
{
@@ -107,7 +107,7 @@ int main()
for(unsigned int i = 0; i < transforms.size(); ++i)
{
- std::cout << i << std::endl;
+ //std::cout << i << std::endl;
BVH_REAL toc;
bool res = CA_linear_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, toc);
@@ -133,6 +133,9 @@ int main()
bool res8 = spline_interp_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, n_dcd_samples, toc8);
+ if(!(i == 2 || i == 44 || i == 53))
+ continue;
+
if(res) std::cout << "yes "; else std::cout << "no ";
if(res2) std::cout << "yes "; else std::cout << "no ";
--
GitLab