From e6ac89477ad4d5cecb82432ac02fe79d4aad018c Mon Sep 17 00:00:00 2001
From: Jeongseok Lee <jslee02@gmail.com>
Date: Sun, 3 May 2015 16:08:37 -0400
Subject: [PATCH] Add tests of building bvh models
---
test/CMakeLists.txt | 1 +
test/test_fcl_bvh_models.cpp | 224 +++++++++++++++++++++++++++++++++++
test/test_fcl_utility.cpp | 44 +++++++
test/test_fcl_utility.h | 2 +
4 files changed, 271 insertions(+)
create mode 100644 test/test_fcl_bvh_models.cpp
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index ed7e36e5..d284712d 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -36,6 +36,7 @@ add_fcl_test(test_fcl_simple test_fcl_simple.cpp)
add_fcl_test(test_fcl_capsule_box_1 test_fcl_capsule_box_1.cpp)
add_fcl_test(test_fcl_capsule_box_2 test_fcl_capsule_box_2.cpp)
#add_fcl_test(test_fcl_global_penetration test_fcl_global_penetration.cpp libsvm/svm.cpp test_fcl_utility.cpp)
+add_fcl_test(test_fcl_bvh_models test_fcl_bvh_models.cpp test_fcl_utility.cpp)
if (FCL_HAVE_OCTOMAP)
add_fcl_test(test_fcl_octomap test_fcl_octomap.cpp test_fcl_utility.cpp)
diff --git a/test/test_fcl_bvh_models.cpp b/test/test_fcl_bvh_models.cpp
new file mode 100644
index 00000000..397046f5
--- /dev/null
+++ b/test/test_fcl_bvh_models.cpp
@@ -0,0 +1,224 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 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 Jeongseok Lee */
+
+
+#define BOOST_TEST_MODULE "FCL_BVH_MODELS"
+#include <boost/test/unit_test.hpp>
+
+#include "fcl/config.h"
+#include "fcl/BVH/BVH_model.h"
+#include "fcl/math/transform.h"
+#include "fcl/shape/geometric_shapes.h"
+#include "test_fcl_utility.h"
+#include <iostream>
+
+using namespace fcl;
+
+template<typename BV>
+void testBVHModelPointCloud()
+{
+ boost::shared_ptr<BVHModel<BV> > model(new BVHModel<BV>);
+
+ if (model->getNodeType() != BV_AABB
+ && model->getNodeType() != BV_KDOP16
+ && model->getNodeType() != BV_KDOP18
+ && model->getNodeType() != BV_KDOP24)
+ {
+ std::cout << "Abort test since '" << getNodeTypeName(model->getNodeType())
+ << "' does not support point cloud model. "
+ << "Please see issue #67." << std::endl;
+ return;
+ }
+
+ Box box;
+ double a = box.side[0];
+ double b = box.side[1];
+ double c = box.side[2];
+ std::vector<Vec3f> points(8);
+ points[0].setValue(0.5 * a, -0.5 * b, 0.5 * c);
+ points[1].setValue(0.5 * a, 0.5 * b, 0.5 * c);
+ points[2].setValue(-0.5 * a, 0.5 * b, 0.5 * c);
+ points[3].setValue(-0.5 * a, -0.5 * b, 0.5 * c);
+ points[4].setValue(0.5 * a, -0.5 * b, -0.5 * c);
+ points[5].setValue(0.5 * a, 0.5 * b, -0.5 * c);
+ points[6].setValue(-0.5 * a, 0.5 * b, -0.5 * c);
+ points[7].setValue(-0.5 * a, -0.5 * b, -0.5 * c);
+
+ int result;
+
+ result = model->beginModel();
+ BOOST_CHECK_EQUAL(result, BVH_OK);
+
+ for (std::size_t i = 0; i < points.size(); ++i)
+ {
+ result = model->addVertex(points[i]);
+ BOOST_CHECK_EQUAL(result, BVH_OK);
+ }
+
+ result = model->endModel();
+ BOOST_CHECK_EQUAL(result, BVH_OK);
+
+ model->computeLocalAABB();
+
+ BOOST_CHECK_EQUAL(model->num_vertices, 8);
+ BOOST_CHECK_EQUAL(model->num_tris, 0);
+ BOOST_CHECK_EQUAL(model->build_state, BVH_BUILD_STATE_PROCESSED);
+}
+
+template<typename BV>
+void testBVHModelTriangles()
+{
+ boost::shared_ptr<BVHModel<BV> > model(new BVHModel<BV>);
+ Box box;
+
+ double a = box.side[0];
+ double b = box.side[1];
+ double c = box.side[2];
+ std::vector<Vec3f> points(8);
+ std::vector<Triangle> tri_indices(12);
+ points[0].setValue(0.5 * a, -0.5 * b, 0.5 * c);
+ points[1].setValue(0.5 * a, 0.5 * b, 0.5 * c);
+ points[2].setValue(-0.5 * a, 0.5 * b, 0.5 * c);
+ points[3].setValue(-0.5 * a, -0.5 * b, 0.5 * c);
+ points[4].setValue(0.5 * a, -0.5 * b, -0.5 * c);
+ points[5].setValue(0.5 * a, 0.5 * b, -0.5 * c);
+ points[6].setValue(-0.5 * a, 0.5 * b, -0.5 * c);
+ points[7].setValue(-0.5 * a, -0.5 * b, -0.5 * 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);
+
+ int result;
+
+ result = model->beginModel();
+ BOOST_CHECK_EQUAL(result, BVH_OK);
+
+ for (std::size_t i = 0; i < tri_indices.size(); ++i)
+ {
+ result = model->addTriangle(points[tri_indices[i][0]], points[tri_indices[i][1]], points[tri_indices[i][2]]);
+ BOOST_CHECK_EQUAL(result, BVH_OK);
+ }
+
+ result = model->endModel();
+ BOOST_CHECK_EQUAL(result, BVH_OK);
+
+ model->computeLocalAABB();
+
+ BOOST_CHECK_EQUAL(model->num_vertices, 12 * 3);
+ BOOST_CHECK_EQUAL(model->num_tris, 12);
+ BOOST_CHECK_EQUAL(model->build_state, BVH_BUILD_STATE_PROCESSED);
+}
+
+template<typename BV>
+void testBVHModelSubModel()
+{
+ boost::shared_ptr<BVHModel<BV> > model(new BVHModel<BV>);
+ Box box;
+
+ double a = box.side[0];
+ double b = box.side[1];
+ double c = box.side[2];
+ std::vector<Vec3f> points(8);
+ std::vector<Triangle> tri_indices(12);
+ points[0].setValue(0.5 * a, -0.5 * b, 0.5 * c);
+ points[1].setValue(0.5 * a, 0.5 * b, 0.5 * c);
+ points[2].setValue(-0.5 * a, 0.5 * b, 0.5 * c);
+ points[3].setValue(-0.5 * a, -0.5 * b, 0.5 * c);
+ points[4].setValue(0.5 * a, -0.5 * b, -0.5 * c);
+ points[5].setValue(0.5 * a, 0.5 * b, -0.5 * c);
+ points[6].setValue(-0.5 * a, 0.5 * b, -0.5 * c);
+ points[7].setValue(-0.5 * a, -0.5 * b, -0.5 * 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);
+
+ int result;
+
+ result = model->beginModel();
+ BOOST_CHECK_EQUAL(result, BVH_OK);
+
+ result = model->addSubModel(points, tri_indices);
+ BOOST_CHECK_EQUAL(result, BVH_OK);
+
+ result = model->endModel();
+ BOOST_CHECK_EQUAL(result, BVH_OK);
+
+ model->computeLocalAABB();
+
+ BOOST_CHECK_EQUAL(model->num_vertices, 8);
+ BOOST_CHECK_EQUAL(model->num_tris, 12);
+ BOOST_CHECK_EQUAL(model->build_state, BVH_BUILD_STATE_PROCESSED);
+}
+
+template<typename BV>
+void testBVHModel()
+{
+ testBVHModelTriangles<BV>();
+ testBVHModelPointCloud<BV>();
+ testBVHModelSubModel<BV>();
+}
+
+BOOST_AUTO_TEST_CASE(building_bvh_models)
+{
+ testBVHModel<AABB>();
+ testBVHModel<OBB>();
+ testBVHModel<RSS>();
+ testBVHModel<kIOS>();
+ testBVHModel<OBBRSS>();
+ testBVHModel<KDOP<16> >();
+ testBVHModel<KDOP<18> >();
+ testBVHModel<KDOP<24> >();
+}
diff --git a/test/test_fcl_utility.cpp b/test/test_fcl_utility.cpp
index e3bda2d8..b951045d 100644
--- a/test/test_fcl_utility.cpp
+++ b/test/test_fcl_utility.cpp
@@ -400,4 +400,48 @@ bool defaultContinuousDistanceFunction(ContinuousCollisionObject* o1, Continuous
return true;
}
+std::string getNodeTypeName(NODE_TYPE node_type)
+{
+ if (node_type == BV_UNKNOWN)
+ return std::string("BV_UNKNOWN");
+ else if (node_type == BV_AABB)
+ return std::string("BV_AABB");
+ else if (node_type == BV_OBB)
+ return std::string("BV_OBB");
+ else if (node_type == BV_RSS)
+ return std::string("BV_RSS");
+ else if (node_type == BV_kIOS)
+ return std::string("BV_kIOS");
+ else if (node_type == BV_OBBRSS)
+ return std::string("BV_OBBRSS");
+ else if (node_type == BV_KDOP16)
+ return std::string("BV_KDOP16");
+ else if (node_type == BV_KDOP18)
+ return std::string("BV_KDOP18");
+ else if (node_type == BV_KDOP24)
+ return std::string("BV_KDOP24");
+ else if (node_type == GEOM_BOX)
+ return std::string("GEOM_BOX");
+ else if (node_type == GEOM_SPHERE)
+ return std::string("GEOM_SPHERE");
+ else if (node_type == GEOM_CAPSULE)
+ return std::string("GEOM_CAPSULE");
+ else if (node_type == GEOM_CONE)
+ return std::string("GEOM_CONE");
+ else if (node_type == GEOM_CYLINDER)
+ return std::string("GEOM_CYLINDER");
+ else if (node_type == GEOM_CONVEX)
+ return std::string("GEOM_CONVEX");
+ else if (node_type == GEOM_PLANE)
+ return std::string("GEOM_PLANE");
+ else if (node_type == GEOM_HALFSPACE)
+ return std::string("GEOM_HALFSPACE");
+ else if (node_type == GEOM_TRIANGLE)
+ return std::string("GEOM_TRIANGLE");
+ else if (node_type == GEOM_OCTREE)
+ return std::string("GEOM_OCTREE");
+ else
+ return std::string("invalid");
+}
+
}
diff --git a/test/test_fcl_utility.h b/test/test_fcl_utility.h
index eff52142..58e96ddb 100644
--- a/test/test_fcl_utility.h
+++ b/test/test_fcl_utility.h
@@ -183,6 +183,8 @@ bool defaultContinuousCollisionFunction(ContinuousCollisionObject* o1, Continuou
bool defaultContinuousDistanceFunction(ContinuousCollisionObject* o1, ContinuousCollisionObject* o2, void* cdata_, FCL_REAL& dist);
+std::string getNodeTypeName(NODE_TYPE node_type);
+
}
#endif
--
GitLab