diff --git a/README.md b/README.md
index 96c1cbf7b116d3396a22d6fc1c942899ae12e381..ee6dade4b959d3dab56bba892b9127889ab60bf7 100644
--- a/README.md
+++ b/README.md
@@ -6,7 +6,7 @@ FCL is a library for performing three types of proximity queries on a pair of ge
- Distance computation: computing the minimum distance between a pair of models, i.e., the distance between the closest pair of points.
- Tolerance verification: determining whether two models are closer or farther than a tolerance distance.
- Continuous collision detection: detecting whether the two moving models overlap during the movement, and optionally, the time of contact.
- - Global penetration depth: detecting the minimum motion required to separate two in-collision objects, and return the configuration where the collision has been resolved.
+# - Global penetration depth: detecting the minimum motion required to separate two in-collision objects, and return the configuration where the collision has been resolved.
- Contact information: for collision detection, continuous collision detection and global penetration depth, the contact information (including contact normals and contact points) can be returned optionally.
FCL has the following features
@@ -123,20 +123,20 @@ ContinuousCollisionResult result;
// perform continuous collision test
continuousCollide(o1, tf_goal_o1, o2, tf_goal_o2, request, result);
```
-For global penetration depth computation, we need a pre-computation step. Beside that, the online penetration depth function uses exactly the same pipeline as shown above:
-```cpp
-// Given two objects o1 and o2
-CollisionObject* o1 = ...
-CollisionObject* o2 = ...
-// the precomputation step for global penetration computation (see test_fcl_xmldata.cpp for an example)
-...
-// set the penetration depth request structure, here we just use the default setting
-PenetrationDepthRequest request;
-// result will be returned via the penetration depth result structure
-PenetrationDepthResult result;
-// perform global penetration depth test
-penetrationDepth(o1, o2,request, result);
-```
+#For global penetration depth computation, we need a pre-computation step. Beside that, the online penetration depth function uses exactly the same pipeline as shown above:
+#```cpp
+#// Given two objects o1 and o2
+#CollisionObject* o1 = ...
+#CollisionObject* o2 = ...
+#// the precomputation step for global penetration computation (see test_fcl_xmldata.cpp for an example)
+#...
+#// set the penetration depth request structure, here we just use the default setting
+#PenetrationDepthRequest request;
+#// result will be returned via the penetration depth result structure
+#PenetrationDepthResult result;
+#// perform global penetration depth test
+#penetrationDepth(o1, o2,request, result);
+#```
FCL supports broadphase collision/distance between two groups of objects and can avoid the n square complexity. For collision, broadphase algorithm can return all the collision pairs. For distance, it can return the pair with the minimum distance. FCL uses a CollisionManager structure to manage all the objects involving the collision or distance operations.
```cpp
@@ -186,6 +186,6 @@ For more examples, please refer to the test folder:
- test_fcl_distance.cpp: provide examples for distance test
- test_fcl_broadphase.cpp: provide examples for broadphase collision/distance test
- test_fcl_frontlist.cpp: provide examples for frontlist collision acceleration
-- test_fcl_global_penetration.cpp: provide examples for global penetration depth test
-- test_fcl_xmldata.cpp: provide examples for more global penetration depth test based on xml data
+#- test_fcl_global_penetration.cpp: provide examples for global penetration depth test
+#- test_fcl_xmldata.cpp: provide examples for more global penetration depth test based on xml data
- test_fcl_octomap.cpp: provide examples for collision/distance computation between octomap data and other data types.
diff --git a/include/fcl/penetration_depth.h b/include/fcl/penetration_depth.h
deleted file mode 100644
index d61ed4f472fe6f482807a1fbc75843494e52fcbc..0000000000000000000000000000000000000000
--- a/include/fcl/penetration_depth.h
+++ /dev/null
@@ -1,113 +0,0 @@
-#ifndef FCL_PENETRATION_DEPTH_H
-#define FCL_PENETRATION_DEPTH_H
-
-#include "fcl/math/vec_3f.h"
-#include "fcl/collision_object.h"
-#include "fcl/collision_data.h"
-#include "fcl/learning/classifier.h"
-#include "fcl/knn/nearest_neighbors.h"
-#include <boost/mem_fn.hpp>
-#include <iostream>
-
-namespace fcl
-{
-
-typedef double (*TransformDistance)(const Transform3f&, const Transform3f&);
-
-class DefaultTransformDistancer
-{
-public:
- const CollisionGeometry* o1;
- const CollisionGeometry* o2;
-
- FCL_REAL rot_x_weight, rot_y_weight, rot_z_weight;
-
- DefaultTransformDistancer() : o1(NULL), o2(NULL)
- {
- rot_x_weight = rot_y_weight = rot_z_weight = 1;
- }
-
- DefaultTransformDistancer(const CollisionGeometry* o2_)
- {
- o2 = o2_;
- init();
- }
-
- DefaultTransformDistancer(const CollisionGeometry* o1_, const CollisionGeometry* o2_)
- {
- o1 = o1_;
- o2 = o2_;
- init();
- }
-
- void init()
- {
- rot_x_weight = rot_y_weight = rot_z_weight = 1;
-
- if(o2)
- {
- FCL_REAL V = o2->computeVolume();
- Matrix3f C = o2->computeMomentofInertiaRelatedToCOM();
- FCL_REAL eigen_values[3];
- Vec3f eigen_vectors[3];
- eigen(C, eigen_values, eigen_vectors);
- rot_x_weight = eigen_values[0] * 4 / V;
- rot_y_weight = eigen_values[1] * 4 / V;
- rot_z_weight = eigen_values[2] * 4 / V;
-
- // std::cout << rot_x_weight << " " << rot_y_weight << " " << rot_z_weight << std::endl;
- }
- }
-
- void printRotWeight() const
- {
- std::cout << rot_x_weight << " " << rot_y_weight << " " << rot_z_weight << std::endl;
- }
-
- double distance(const Transform3f& tf1, const Transform3f& tf2)
- {
- Transform3f tf;
- relativeTransform(tf1, tf2, tf);
-
- const Quaternion3f& quat = tf.getQuatRotation();
- const Vec3f& trans = tf.getTranslation();
-
- FCL_REAL d = rot_x_weight * quat[1] * quat[1] + rot_y_weight * quat[2] * quat[2] + rot_z_weight * quat[3] * quat[3] + trans[0] * trans[0] + trans[1] * trans[1] + trans[2] * trans[2];
- return d;
- }
-};
-
-static DefaultTransformDistancer default_transform_distancer;
-
-static NearestNeighbors<Transform3f>::DistanceFunction default_transform_distance_func = boost::bind(&DefaultTransformDistancer::distance, &default_transform_distancer, _1, _2);
-
-
-
-/// @brief precompute a learning model for penetration computation
-std::vector<Transform3f> penetrationDepthModelLearning(const CollisionObject* o1,
- const CollisionObject* o2,
- PenetrationDepthType pd_type,
- void* classifier,
- std::size_t n_samples,
- FCL_REAL margin,
- KNNSolverType knn_solver_type,
- std::size_t knn_k,
- NearestNeighbors<Transform3f>::DistanceFunction distance_func = default_transform_distance_func);
-
-
-
-
-/// @brief penetration depth computation between two in-collision objects
-FCL_REAL penetrationDepth(const CollisionGeometry* o1, const Transform3f& tf1,
- const CollisionGeometry* o2, const Transform3f& tf2,
- const PenetrationDepthRequest& request,
- PenetrationDepthResult& result);
-
-FCL_REAL penetrationDepth(const CollisionObject* o1,
- const CollisionObject* o2,
- const PenetrationDepthRequest& request,
- PenetrationDepthResult& result);
-
-}
-
-#endif
diff --git a/src/penetration_depth.cpp b/src/penetration_depth.cpp
deleted file mode 100644
index 3eed91ce5425d2c88a203bbd5409c5d84a01a182..0000000000000000000000000000000000000000
--- a/src/penetration_depth.cpp
+++ /dev/null
@@ -1,402 +0,0 @@
-#include "fcl/penetration_depth.h"
-#include "fcl/learning/classifier.h"
-#include "fcl/math/sampling.h"
-#include "fcl/collision.h"
-#include "fcl/exception.h"
-
-#include "fcl/knn/nearest_neighbors_linear.h"
-#include "fcl/knn/nearest_neighbors_GNAT.h"
-#include "fcl/knn/nearest_neighbors_sqrtapprox.h"
-#if FCL_HAVE_FLANN
-#include "fcl/knn/nearest_neighbors_flann.h"
-#endif
-
-#include "fcl/ccd/motion.h"
-#include "fcl/continuous_collision.h"
-
-namespace fcl
-{
-
-std::vector<Transform3f> penetrationDepthModelLearning(const CollisionObject* o1,
- const CollisionObject* o2,
- PenetrationDepthType pd_type,
- void* classifier_,
- std::size_t n_samples,
- FCL_REAL margin,
- KNNSolverType knn_solver_type,
- std::size_t knn_k,
- NearestNeighbors<Transform3f>::DistanceFunction distance_func)
-{
- std::vector<Transform3f> support_transforms_positive;
- std::vector<Transform3f> support_transforms_negative;
-
- switch(pd_type)
- {
- case PDT_TRANSLATIONAL:
- {
- SVMClassifier<3>* classifier = static_cast<SVMClassifier<3>*>(classifier_);
- SamplerR<3> sampler;
- Vecnf<3> lower_bound, upper_bound;
- AABB aabb1 = o1->getAABB();
- AABB aabb2 = o2->getAABB();
- lower_bound[0] = aabb1.min_[0] - aabb2.max_[0] - margin;
- lower_bound[1] = aabb1.min_[1] - aabb2.max_[1] - margin;
- lower_bound[2] = aabb1.min_[2] - aabb2.max_[2] - margin;
- upper_bound[0] = aabb1.max_[0] - aabb2.min_[0] + margin;
- upper_bound[1] = aabb1.max_[1] - aabb2.min_[1] + margin;
- upper_bound[2] = aabb1.max_[2] - aabb2.min_[2] + margin;
-
- sampler.setBound(lower_bound, upper_bound);
-
- std::vector<Item<3> > data(n_samples);
- for(std::size_t i = 0; i < n_samples; ++i)
- {
- Vecnf<3> q = sampler.sample();
- Transform3f tf(o2->getQuatRotation(), Vec3f(q[0], q[1], q[2]));
-
- CollisionRequest request;
- CollisionResult result;
-
- int n_collide = collide(o1->getCollisionGeometry(), o1->getTransform(),
- o2->getCollisionGeometry(), tf, request, result);
-
- data[i] = Item<3>(q, (n_collide > 0));
- }
-
- // classifier.setScaler(Scaler<3>(lower_bound, upper_bound));
- classifier->setScaler(computeScaler(data));
-
- classifier->learn(data);
-
- std::vector<Item<3> > svs = classifier->getSupportVectors();
- for(std::size_t i = 0; i < svs.size(); ++i)
- {
- const Vecnf<3>& q = svs[i].q;
- if(svs[i].label)
- support_transforms_positive.push_back(Transform3f(o2->getQuatRotation(), Vec3f(q[0], q[1], q[2])));
- else
- support_transforms_negative.push_back(Transform3f(o2->getQuatRotation(), Vec3f(q[0], q[1], q[2])));
- }
-
- break;
- }
- case PDT_GENERAL_EULER:
- {
- SVMClassifier<6>* classifier = static_cast<SVMClassifier<6>*>(classifier_);
- SamplerSE3Euler sampler;
-
- FCL_REAL r1 = o1->getCollisionGeometry()->aabb_radius;
- FCL_REAL r2 = o2->getCollisionGeometry()->aabb_radius;
- Vec3f c1 = o1->getCollisionGeometry()->aabb_center;
- Vec3f c2 = o2->getCollisionGeometry()->aabb_center;
-
- FCL_REAL r = r1 + r2 + margin;
- sampler.setBound(Vec3f(-r, -r, -r), Vec3f(r, r, r));
-
- std::vector<Item<6> > data(n_samples);
- for(std::size_t i = 0; i < n_samples; ++i)
- {
- Vecnf<6> q = sampler.sample();
- Quaternion3f quat;
- quat.fromEuler(q[3], q[4], q[5]);
- Transform3f tf(quat, -quat.transform(c2) + c1 + Vec3f(q[0], q[1], q[2]));
- CollisionRequest request;
- CollisionResult result;
-
- int n_collide = collide(o1->getCollisionGeometry(), Transform3f(),
- o2->getCollisionGeometry(), tf, request, result);
-
- data[i] = Item<6>(q, (n_collide > 0));
- }
-
- classifier->setScaler(computeScaler(data));
-
- classifier->learn(data);
-
- std::vector<Item <6> > svs = classifier->getSupportVectors();
- for(std::size_t i = 0; i < svs.size(); ++i)
- {
- const Vecnf<6>& q = svs[i].q;
- Quaternion3f quat;
- quat.fromEuler(q[3], q[4], q[5]);
- if(svs[i].label)
- support_transforms_positive.push_back(Transform3f(quat, -quat.transform(c2) + c1 + Vec3f(q[0], q[1], q[2])));
- else
- support_transforms_negative.push_back(Transform3f(quat, -quat.transform(c2) + c1 + Vec3f(q[0], q[1], q[2])));
- }
-
- break;
- }
- case PDT_GENERAL_QUAT:
- {
- SVMClassifier<7>* classifier = static_cast<SVMClassifier<7>*>(classifier_);
- SamplerSE3Quat sampler;
-
- FCL_REAL r1 = o1->getCollisionGeometry()->aabb_radius;
- FCL_REAL r2 = o2->getCollisionGeometry()->aabb_radius;
- Vec3f c1 = o1->getCollisionGeometry()->aabb_center;
- Vec3f c2 = o2->getCollisionGeometry()->aabb_center;
-
- FCL_REAL r = r1 + r2 + margin;
- sampler.setBound(Vec3f(-r, -r, -r), Vec3f(r, r, r));
-
- std::vector<Item<7> > data(n_samples);
- for(std::size_t i = 0; i < n_samples; ++i)
- {
- Vecnf<7> q = sampler.sample();
-
- Quaternion3f quat(q[3], q[4], q[5], q[6]);
- Transform3f tf(quat, -quat.transform(c2) + c1 + Vec3f(q[0], q[1], q[2]));
-
- CollisionRequest request;
- CollisionResult result;
-
- int n_collide = collide(o1->getCollisionGeometry(), Transform3f(),
- o2->getCollisionGeometry(), tf, request, result);
-
- data[i] = Item<7>(q, (n_collide > 0));
- }
-
- classifier->setScaler(computeScaler(data));
-
- classifier->learn(data);
-
- std::vector<Item<7> > svs = classifier->getSupportVectors();
- for(std::size_t i = 0; i < svs.size(); ++i)
- {
- const Vecnf<7>& q = svs[i].q;
- Quaternion3f quat(q[3], q[4], q[5], q[6]);
- if(svs[i].label)
- support_transforms_positive.push_back(Transform3f(quat, -quat.transform(c2) + c1 + Vec3f(q[0], q[1], q[2])));
- else
- support_transforms_negative.push_back(Transform3f(quat, -quat.transform(c2) + c1 + Vec3f(q[0], q[1], q[2])));
- }
-
- break;
- }
- case PDT_GENERAL_EULER_BALL:
- {
- SVMClassifier<6>* classifier = static_cast<SVMClassifier<6>*>(classifier_);
- SamplerSE3Euler_ball sampler;
-
- FCL_REAL r1 = o1->getCollisionGeometry()->aabb_radius;
- FCL_REAL r2 = o2->getCollisionGeometry()->aabb_radius;
- Vec3f c1 = o1->getCollisionGeometry()->aabb_center;
- Vec3f c2 = o2->getCollisionGeometry()->aabb_center;
-
- sampler.setBound(r1 + r2 + margin);
-
- std::vector<Item<6> > data(n_samples);
- for(std::size_t i = 0; i < n_samples; ++i)
- {
- Vecnf<6> q = sampler.sample();
-
- Quaternion3f quat;
- quat.fromEuler(q[3], q[4], q[5]);
- Transform3f tf(quat, -quat.transform(c2) + c1 + Vec3f(q[0], q[1], q[2]));
-
- CollisionRequest request;
- CollisionResult result;
-
- int n_collide = collide(o1->getCollisionGeometry(), Transform3f(),
- o2->getCollisionGeometry(), tf, request, result);
-
- data[i] = Item<6>(q, (n_collide > 0));
- }
-
- classifier->setScaler(computeScaler(data));
-
- classifier->learn(data);
-
- std::vector<Item<6> > svs = classifier->getSupportVectors();
- for(std::size_t i = 0; i < svs.size(); ++i)
- {
- const Vecnf<6>& q = svs[i].q;
- Quaternion3f quat;
- quat.fromEuler(q[3], q[4], q[5]);
- if(svs[i].label)
- support_transforms_positive.push_back(Transform3f(quat, -quat.transform(c2) + c1 + Vec3f(q[0], q[1], q[2])));
- else
- support_transforms_negative.push_back(Transform3f(quat, -quat.transform(c2) + c1 + Vec3f(q[0], q[1], q[2])));
- }
-
- break;
- }
- case PDT_GENERAL_QUAT_BALL:
- {
- SVMClassifier<7>* classifier = static_cast<SVMClassifier<7>*>(classifier_);
- SamplerSE3Quat_ball sampler;
-
- FCL_REAL r1 = o1->getCollisionGeometry()->aabb_radius;
- FCL_REAL r2 = o2->getCollisionGeometry()->aabb_radius;
- Vec3f c1 = o1->getCollisionGeometry()->aabb_center;
- Vec3f c2 = o2->getCollisionGeometry()->aabb_center;
-
- sampler.setBound(r1 + r2 + margin);
-
- std::vector<Item<7> > data(n_samples);
- for(std::size_t i = 0; i < n_samples; ++i)
- {
- Vecnf<7> q = sampler.sample();
-
- Quaternion3f quat(q[3], q[4], q[5], q[6]);
- Transform3f tf(quat, -quat.transform(c2) + c1 + Vec3f(q[0], q[1], q[2]));
-
- CollisionRequest request;
- CollisionResult result;
-
- int n_collide = collide(o1->getCollisionGeometry(), Transform3f(),
- o2->getCollisionGeometry(), tf, request, result);
-
- data[i] = Item<7>(q, (n_collide > 0));
- }
-
- classifier->setScaler(computeScaler(data));
-
- classifier->learn(data);
-
- std::vector<Item<7> > svs = classifier->getSupportVectors();
- for(std::size_t i = 0; i < svs.size(); ++i)
- {
- const Vecnf<7>& q = svs[i].q;
- Quaternion3f quat(q[3], q[4], q[5], q[6]);
- if(svs[i].label)
- support_transforms_positive.push_back(Transform3f(quat, -quat.transform(c2) + c1 + Vec3f(q[0], q[1], q[2])));
- else
- support_transforms_negative.push_back(Transform3f(quat, -quat.transform(c2) + c1 + Vec3f(q[0], q[1], q[2])));
- }
-
- break;
- }
- default:
- ;
- }
-
- // from support transforms compute contact transforms
-
- NearestNeighbors<Transform3f>* knn_solver_positive = NULL;
- NearestNeighbors<Transform3f>* knn_solver_negative = NULL;
-
- switch(knn_solver_type)
- {
- case KNN_LINEAR:
- knn_solver_positive = new NearestNeighborsLinear<Transform3f>();
- knn_solver_negative = new NearestNeighborsLinear<Transform3f>();
- break;
- case KNN_GNAT:
- knn_solver_positive = new NearestNeighborsGNAT<Transform3f>();
- knn_solver_negative = new NearestNeighborsGNAT<Transform3f>();
- break;
- case KNN_SQRTAPPROX:
- knn_solver_positive = new NearestNeighborsSqrtApprox<Transform3f>();
- knn_solver_negative = new NearestNeighborsSqrtApprox<Transform3f>();
- break;
- }
-
-
- knn_solver_positive->setDistanceFunction(distance_func);
- knn_solver_negative->setDistanceFunction(distance_func);
-
- knn_solver_positive->add(support_transforms_positive);
- knn_solver_negative->add(support_transforms_negative);
-
- std::vector<Transform3f> contact_vectors;
-
- for(std::size_t i = 0; i < support_transforms_positive.size(); ++i)
- {
- std::vector<Transform3f> nbh;
- knn_solver_negative->nearestK(support_transforms_positive[i], knn_k, nbh);
-
- for(std::size_t j = 0; j < nbh.size(); ++j)
- {
- ContinuousCollisionRequest request;
- request.ccd_motion_type = CCDM_LINEAR;
- request.num_max_iterations = 100;
- ContinuousCollisionResult result;
- continuousCollide(o1->getCollisionGeometry(), Transform3f(), Transform3f(),
- o2->getCollisionGeometry(), nbh[j], support_transforms_positive[i],
- request, result);
-
- //std::cout << result.time_of_contact << std::endl;
-
- contact_vectors.push_back(result.contact_tf2);
- }
- }
-
- for(std::size_t i = 0; i < support_transforms_negative.size(); ++i)
- {
- std::vector<Transform3f> nbh;
- knn_solver_positive->nearestK(support_transforms_negative[i], knn_k, nbh);
-
- for(std::size_t j = 0; j < nbh.size(); ++j)
- {
- ContinuousCollisionRequest request;
- request.ccd_motion_type = CCDM_LINEAR;
- request.num_max_iterations = 100;
- ContinuousCollisionResult result;
- continuousCollide(o1->getCollisionGeometry(), Transform3f(), Transform3f(),
- o2->getCollisionGeometry(), support_transforms_negative[i], nbh[j],
- request, result);
-
- //std::cout << result.time_of_contact << std::endl;
-
- contact_vectors.push_back(result.contact_tf2);
- }
- }
-
- delete knn_solver_positive;
- delete knn_solver_negative;
-
- return contact_vectors;
-
-}
-
-
-
-FCL_REAL penetrationDepth(const CollisionGeometry* o1, const Transform3f& tf1,
- const CollisionGeometry* o2, const Transform3f& tf2,
- const PenetrationDepthRequest& request,
- PenetrationDepthResult& result)
-{
- NearestNeighbors<Transform3f>* knn_solver = NULL;
- switch(request.knn_solver_type)
- {
- case KNN_LINEAR:
- knn_solver = new NearestNeighborsLinear<Transform3f>();
- break;
- case KNN_GNAT:
- knn_solver = new NearestNeighborsGNAT<Transform3f>();
- break;
- case KNN_SQRTAPPROX:
- knn_solver = new NearestNeighborsSqrtApprox<Transform3f>();
- break;
- }
-
- knn_solver->setDistanceFunction(request.distance_func);
-
- knn_solver->add(request.contact_vectors);
-
- Transform3f tf;
- relativeTransform2(tf1, tf2, tf);
-
- result.resolved_tf = tf1 * knn_solver->nearest(tf);
- result.pd_value = request.distance_func(result.resolved_tf, tf2);
- result.pd_value = std::sqrt(result.pd_value);
-
- delete knn_solver;
-
- return result.pd_value;
-}
-
-FCL_REAL penetrationDepth(const CollisionObject* o1,
- const CollisionObject* o2,
- const PenetrationDepthRequest& request,
- PenetrationDepthResult& result)
-{
- return penetrationDepth(o1->getCollisionGeometry(), o1->getTransform(),
- o2->getCollisionGeometry(), o2->getTransform(),
- request,
- result);
-}
-
-}
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 4f3794abbc2e4869eb564c932496cd051107698f..2ab2a8cc5abc446959543edb2c225dbd7725dbbe 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -29,21 +29,20 @@ add_fcl_test(test_fcl_math test_fcl_math.cpp test_fcl_utility.cpp)
add_fcl_test(test_fcl_sphere_capsule test_fcl_sphere_capsule.cpp)
add_fcl_test(test_fcl_simple test_fcl_simple.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_global_penetration test_fcl_global_penetration.cpp libsvm/svm.cpp test_fcl_utility.cpp)
if (FCL_HAVE_OCTOMAP)
add_fcl_test(test_fcl_octomap test_fcl_octomap.cpp test_fcl_utility.cpp)
endif()
-if (FCL_HAVE_TINYXML)
- add_executable(test_fcl_xmldata test_fcl_xmldata.cpp test_fcl_utility.cpp libsvm/svm.cpp)
- target_link_libraries(test_fcl_xmldata
- fcl
- ${TINYXML_LIBRARY_DIRS}
- ${Boost_SYSTEM_LIBRARY}
- ${Boost_THREAD_LIBRARY}
- ${Boost_DATE_TIME_LIBRARY}
- ${Boost_UNIT_TEST_FRAMEWORK_LIBRARY})
- add_test(test_fcl_xmldata ${EXECUTABLE_OUTPUT_PATH}/test_fcl_xmldata)
-
-endif()
+#if (FCL_HAVE_TINYXML)
+# add_executable(test_fcl_xmldata test_fcl_xmldata.cpp test_fcl_utility.cpp libsvm/svm.cpp)
+# target_link_libraries(test_fcl_xmldata
+# fcl
+# ${TINYXML_LIBRARY_DIRS}
+# ${Boost_SYSTEM_LIBRARY}
+# ${Boost_THREAD_LIBRARY}
+# ${Boost_DATE_TIME_LIBRARY}
+# ${Boost_UNIT_TEST_FRAMEWORK_LIBRARY})
+# add_test(test_fcl_xmldata ${EXECUTABLE_OUTPUT_PATH}/test_fcl_xmldata)
+#endif()
diff --git a/test/fcl_resources/config.h b/test/fcl_resources/config.h
index 32233b97e982beb9a6758372831b0a8ac0c8f47b..f4cc495f1232d42baf57d2e4865c34c1912d95e8 100644
--- a/test/fcl_resources/config.h
+++ b/test/fcl_resources/config.h
@@ -37,5 +37,5 @@
#ifndef FCL_TEST_RESOURCES_CONFIG_
#define FCL_TEST_RESOURCES_CONFIG_
-#define TEST_RESOURCES_DIR "/home/jia/Workspace/fcl2/test/fcl_resources"
+#define TEST_RESOURCES_DIR "/home/jia/Workspace/fcl/test/fcl_resources"
#endif
diff --git a/test/test_fcl_global_penetration.cpp b/test/test_fcl_global_penetration.cpp
deleted file mode 100644
index d1139686fbd1772eb069d157866a31d58e4b4e6b..0000000000000000000000000000000000000000
--- a/test/test_fcl_global_penetration.cpp
+++ /dev/null
@@ -1,69 +0,0 @@
-#define BOOST_TEST_MODULE "FCL_GLOBAL_PENETRATION"
-#include <boost/test/unit_test.hpp>
-#include "libsvm_classifier.h"
-#include "fcl/penetration_depth.h"
-#include <boost/filesystem.hpp>
-#include "fcl_resources/config.h"
-#include "test_fcl_utility.h"
-#include "fcl/collision.h"
-#include "fcl/BVH/BVH_model.h"
-
-
-
-
-using namespace fcl;
-
-
-
-BOOST_AUTO_TEST_CASE(global_penetration_test)
-{
- LibSVMClassifier<6> classifier;
-
- std::vector<Vec3f> p1, p2;
- std::vector<Triangle> t1, t2;
- boost::filesystem::path path(TEST_RESOURCES_DIR);
-
- loadOBJFile((path / "env.obj").string().c_str(), p1, t1);
- loadOBJFile((path / "rob.obj").string().c_str(), p2, t2);
-
- BVHModel<OBBRSS>* m1 = new BVHModel<OBBRSS>();
- BVHModel<OBBRSS>* m2 = new BVHModel<OBBRSS>();
- m1->beginModel();
- m1->addSubModel(p1, t1);
- m1->endModel();
- m2->beginModel();
- m2->addSubModel(p2, t2);
- m2->endModel();
-
- std::cout << m1->computeVolume() << std::endl;
- std::cout << m2->computeVolume() << std::endl;
-
- std::cout << m1->computeMomentofInertia() << std::endl;
- std::cout << m2->computeMomentofInertia() << std::endl;
-
- std::cout << m1->computeMomentofInertiaRelatedToCOM() << std::endl;
- std::cout << m2->computeMomentofInertiaRelatedToCOM() << std::endl;
-
- Transform3f id;
-
- CollisionObject o1(boost::shared_ptr<CollisionGeometry>(m1), id);
- CollisionObject o2(boost::shared_ptr<CollisionGeometry>(m2), id);
-
- default_transform_distancer = DefaultTransformDistancer(o2.getCollisionGeometry());
-
- std::size_t KNN_K = 10;
- std::vector<Transform3f> contact_vectors = penetrationDepthModelLearning(&o1, &o2, PDT_GENERAL_EULER, &classifier, 10000, 0, KNN_GNAT, KNN_K);
-
- classifier.save("model.txt");
-
- PenetrationDepthRequest request(&classifier, default_transform_distance_func);
- request.contact_vectors = contact_vectors;
-
- PenetrationDepthResult result;
- penetrationDepth(o1.getCollisionGeometry(), Transform3f(),
- o2.getCollisionGeometry(), Transform3f(),
- request, result);
-
- std::cout << result.pd_value << std::endl;
-
-}
diff --git a/test/test_fcl_xmldata.cpp b/test/test_fcl_xmldata.cpp
deleted file mode 100644
index a0c44f81efa2e5ae3d60f9f4d914127601608a6e..0000000000000000000000000000000000000000
--- a/test/test_fcl_xmldata.cpp
+++ /dev/null
@@ -1,455 +0,0 @@
-#define BOOST_TEST_MODULE "FCL_XMLDATA"
-#include <tinyxml.h>
-
-#include <boost/test/unit_test.hpp>
-
-#include "fcl/intersect.h"
-#include "fcl/collision.h"
-#include "fcl/math/sampling.h"
-#include "fcl/BVH/BVH_model.h"
-#include <boost/filesystem.hpp>
-#include <sstream>
-
-
-#include "libsvm_classifier.h"
-#include "fcl/penetration_depth.h"
-#include "fcl/collision_data.h"
-#include "fcl_resources/config.h"
-#include "test_fcl_utility.h"
-
-
-using namespace fcl;
-
-
-
-static void loadSceneFile(const std::string& filename,
- std::vector<std::vector<Vec3f> >& points_array,
- std::vector<std::vector<Triangle> >& triangles_array,
- std::vector<std::pair<Transform3f, Transform3f> >& motions)
-{
- TiXmlDocument doc(filename.c_str());
- if(doc.LoadFile())
- {
- TiXmlHandle hdoc(&doc);
- TiXmlElement* model, *motion;
- model = doc.FirstChildElement("MODEL");
- if(model)
- {
- TiXmlElement* object = model->FirstChildElement("OBJECT");
- int i = 0;
- while(object)
- {
- int object_id = -1;
- object->Attribute("id", &object_id);
- // std::cout << object_id << std::endl;
-
- std::vector<Vec3f> points;
- std::vector<Triangle> triangles;
-
- TiXmlElement* grid = object->FirstChildElement("GRID");
- int n_vertices = 0;
- while(grid)
- {
- int grid_id;
- double grid_x, grid_y, grid_z;
-
- grid->Attribute("id", &grid_id);
- grid->Attribute("x", &grid_x);
- grid->Attribute("y", &grid_y);
- grid->Attribute("z", &grid_z);
-
- Vec3f p(grid_x, grid_y, grid_z);
-
- if(grid_id - 1 == (int)points.size())
- points.push_back(p);
- else if(grid_id - 1 < (int)points.size())
- points[grid_id - 1] = p;
- else // if(grid_id - 1 > points.size())
- {
- points.resize(grid_id);
- points.back() = p;
- }
-
- n_vertices++;
- grid = grid->NextSiblingElement("GRID");
- }
-
- // std::cout << "#vertices " << n_vertices << std::endl;
-
- TiXmlElement* tri = object->FirstChildElement("TRIA");
- int n_tris = 0;
- while(tri)
- {
- int tri_id;
- int v1, v2, v3;
-
- tri->Attribute("id", &tri_id);
- tri->Attribute("g1", &v1);
- tri->Attribute("g2", &v2);
- tri->Attribute("g3", &v3);
-
- Triangle t(v1-1, v2-1, v3-1);
-
- if(tri_id - 1 == (int)triangles.size())
- triangles.push_back(t);
- else if(tri_id - 1 < (int)triangles.size())
- triangles[tri_id - 1] = t;
- else
- {
- triangles.resize(tri_id);
- triangles.back() = t;
- }
-
- n_tris++;
- tri = tri->NextSiblingElement("TRIA");
- }
-
- // std::cout << "#triangles " << n_tris << std::endl;
-
- if(object_id - 1 == (int)points_array.size())
- {
- points_array.push_back(points);
- triangles_array.push_back(triangles);
- }
- else if(object_id - 1 < (int)points_array.size())
- {
- points_array[object_id - 1] = points;
- triangles_array[object_id - 1] = triangles;
- }
- else
- {
- points_array.resize(object_id);
- triangles_array.resize(object_id);
- points_array.back() = points;
- triangles_array.back() = triangles;
- }
-
- object = object->NextSiblingElement("OBJECT");
- i++;
- }
-
- // std::cout << "#objects " << i << std::endl;
- }
-
- motion = doc.FirstChildElement("MOTION");
- if(motion)
- {
- TiXmlElement* frame = motion->FirstChildElement("FRAME");
- int n_frame = 0;
- while(frame)
- {
- int frame_id;
- double frame_time;
- frame->Attribute("id", &frame_id);
- frame->Attribute("time", &frame_time);
-
-
- Vec3f T1, T2;
- Matrix3f R1, R2;
- const char* obj1_pos_string = frame->Attribute("obj1_pos");
- const char* obj2_pos_string = frame->Attribute("obj2_pos");
- const char* obj1_dc_string = frame->Attribute("obj1_dc");
- const char* obj2_dc_string = frame->Attribute("obj2_dc");
-
- std::stringstream s1_pos(obj1_pos_string);
- s1_pos >> T1[0] >> T1[1] >> T1[2];
- std::stringstream s2_pos(obj2_pos_string);
- s2_pos >> T2[0] >> T2[1] >> T2[2];
- std::stringstream s1_mat(obj1_dc_string);
- for(int j = 0; j < 3; ++j)
- for(int k = 0; k < 3; ++k)
- s1_mat >> R1(j, k);
- std::stringstream s2_mat(obj2_dc_string);
- for(int j = 0; j < 3; ++j)
- for(int k = 0; k < 3; ++k)
- s2_mat >> R2(j, k);
-
-
- std::pair<Transform3f, Transform3f> motion = std::make_pair(Transform3f(R1, T1), Transform3f(R2, T2));
- if(frame_id - 1 == (int)motions.size())
- motions.push_back(motion);
- else if(frame_id - 1 < (int)motions.size())
- motions[frame_id - 1] = motion;
- else
- {
- motions.resize(frame_id);
- motions.back() = motion;
- }
-
-
- frame = frame->NextSiblingElement("FRAME");
- n_frame++;
- }
-
- // std::cout << "#frames " << n_frame << std::endl;
- }
- }
- else
- std::cerr << "Failed to load file " << filename << std::endl;
-}
-
-BOOST_AUTO_TEST_CASE(scene_test)
-{
- std::vector<std::vector<Vec3f> > points_array;
- std::vector<std::vector<Triangle> > triangles_array;
- std::vector<std::pair<Transform3f, Transform3f> > motions;
- boost::filesystem::path path(TEST_RESOURCES_DIR);
- std::string filename = (path / "scenario-1-2-3/Model_1_Scenario_3.txt").string();
- loadSceneFile(filename, points_array, triangles_array, motions);
-
- BVHModel<OBBRSS> m1;
- BVHModel<OBBRSS> m2;
- m1.beginModel();
- m1.addSubModel(points_array[0], triangles_array[0]);
- m1.endModel();
-
- m2.beginModel();
- m2.addSubModel(points_array[1], triangles_array[1]);
- m2.endModel();
-
-
- CollisionResult result0;
- CollisionRequest request0;
- collide(&m1, motions[0].first, &m2, motions[0].second, request0, result0);
- // std::cout << result0.numContacts() << std::endl;
-
- CollisionResult result1;
- CollisionRequest request1;
- collide(&m1, motions[1].first, &m2, motions[1].second, request1, result1);
- // std::cout << result1.numContacts() << std::endl;
-}
-
-static std::string num2string(int i)
-{
- std::string result;
- std::ostringstream convert;
- convert << i;
- result = convert.str();
- return result;
-}
-
-static void xml2obj(const std::string& in_filename, const std::string& out_filename_base)
-{
- std::vector<std::vector<Vec3f> > points_array;
- std::vector<std::vector<Triangle> > triangles_array;
- std::vector<std::pair<Transform3f, Transform3f> > motions;
- loadSceneFile(in_filename, points_array, triangles_array, motions);
-
- std::size_t n_obj = points_array.size();
- // save objs in local frame
- for(std::size_t i = 0; i < n_obj; ++i)
- {
- std::string out_filenameL = out_filename_base + num2string(i+1) + "L.obj";
- saveOBJFile(out_filenameL.c_str(), points_array[i], triangles_array[i]);
- }
-
- // save objs in frame 1
- for(std::size_t i = 0; i < n_obj; ++i)
- {
- std::string out_filenameF1 = out_filename_base + num2string(i+1) + "Frame1.obj";
- std::vector<Vec3f> points(points_array[i].size());
- for(std::size_t j = 0; j < points.size(); ++j)
- points[j] = motions[i].first.transform(points_array[i][j]);
-
- saveOBJFile(out_filenameF1.c_str(), points, triangles_array[i]);
- }
-
- // save objs in frame 2
- for(std::size_t i = 0; i < n_obj; ++i)
- {
- std::string out_filenameF2 = out_filename_base + num2string(i+1) + "Frame2.obj";
- std::vector<Vec3f> points(points_array[i].size());
- for(std::size_t j = 0; j < points.size(); ++j)
- points[j] = motions[i].second.transform(points_array[i][j]);
-
- saveOBJFile(out_filenameF2.c_str(), points, triangles_array[i]);
- }
-}
-
-
-static void scenePenetrationTest(const std::string& filename, PenetrationDepthType pd_type = PDT_GENERAL_EULER)
-{
- std::vector<std::vector<Vec3f> > points_array;
- std::vector<std::vector<Triangle> > triangles_array;
- std::vector<std::pair<Transform3f, Transform3f> > motions;
-
- loadSceneFile(filename, points_array, triangles_array, motions);
-
- BVHModel<OBBRSS>* m1 = new BVHModel<OBBRSS>();
- BVHModel<OBBRSS>* m2 = new BVHModel<OBBRSS>();
- m1->beginModel();
- m1->addSubModel(points_array[0], triangles_array[0]);
- m1->endModel();
-
- m2->beginModel();
- m2->addSubModel(points_array[1], triangles_array[1]);
- m2->endModel();
-
- Transform3f id;
- CollisionObject o1(boost::shared_ptr<CollisionGeometry>(m1), id);
- CollisionObject o2(boost::shared_ptr<CollisionGeometry>(m2), id);
-
- default_transform_distancer = DefaultTransformDistancer(o1.getCollisionGeometry());
- std::cout << "rotation weights ";
- default_transform_distancer.printRotWeight();
- std::size_t KNN_K = 10;
- LibSVMClassifier<6> classifier;
-
- std::vector<Transform3f> contact_vectors = penetrationDepthModelLearning(&o1, &o2, pd_type, &classifier, 100000, 0, KNN_GNAT, KNN_K);
-
- classifier.save(filename + "model.txt");
-
- for(std::size_t frame_id = 0; frame_id < motions.size(); ++frame_id)
- {
- PenetrationDepthRequest request(&classifier, default_transform_distance_func);
- request.contact_vectors = contact_vectors;
- PenetrationDepthResult result;
-
- penetrationDepth(m1, motions[frame_id].first, m2, motions[frame_id].second, request, result);
- std::cout << "pd value " << frame_id << " " << result.pd_value << std::endl;
- std::cout << "resolved tf translation " << result.resolved_tf.getTranslation() << std::endl;
- std::cout << "resolved tf rotation " << result.resolved_tf.getRotation() << std::endl;
-
- int num_pd_contacts = 1; // only one contact for pd, you can set a larger number if you want more
- CollisionRequest pd_contact_request(num_pd_contacts, true);
- CollisionResult pd_contact_result;
- collide(m1, motions[frame_id].first, m2, result.resolved_tf, pd_contact_request, pd_contact_result);
- if(pd_contact_result.numContacts() > 0)
- {
- Contact contact = pd_contact_result.getContact(0);
- std::cout << "contact pos " << contact.pos << std::endl;
- std::cout << "contact normal " << contact.normal << std::endl;
- }
- }
-}
-
-
-BOOST_AUTO_TEST_CASE(scene_test_penetration)
-{
- RNG::setSeed(1);
- boost::filesystem::path path(TEST_RESOURCES_DIR);
-
- std::cout << "manyframes/Model_5" << std::endl;
- std::string filename0 = (path / "manyframes/Model_5.xml").string();
- scenePenetrationTest(filename0);
-
- return;
-
-
- std::cout << "scenario-1-2-3/Model_1_Scenario_1" << std::endl;
- std::string filename1 = (path / "scenario-1-2-3/Model_1_Scenario_1.txt").string();
- scenePenetrationTest(filename1);
-
- std::cout << std::endl;
-
- std::cout << "scenario-1-2-3/Model_1_Scenario_2" << std::endl;
- std::string filename2 = (path / "scenario-1-2-3/Model_1_Scenario_2.txt").string();
- scenePenetrationTest(filename2);
-
- std::cout << std::endl;
-
- std::cout << "scenario-1-2-3/Model_1_Scenario_3" << std::endl;
- std::string filename3 = (path / "scenario-1-2-3/Model_1_Scenario_3.txt").string();
- scenePenetrationTest(filename3);
-
- std::cout << std::endl;
-
- std::cout << "scenario-1-2-3/Model_2_Scenario_1" << std::endl;
- std::string filename4 = (path / "scenario-1-2-3/Model_2_Scenario_1.txt").string();
- scenePenetrationTest(filename4);
-
- std::cout << std::endl;
-
- std::cout << "scenario-1-2-3/Model_2_Scenario_2" << std::endl;
- std::string filename5 = (path / "scenario-1-2-3/Model_2_Scenario_2.txt").string();
- scenePenetrationTest(filename5);
-
- std::cout << std::endl;
-
- std::cout << "scenario-1-2-3/Model_2_Scenario_3" << std::endl;
- std::string filename6 = (path / "scenario-1-2-3/Model_2_Scenario_3.txt").string();
- scenePenetrationTest(filename6);
-
- std::cout << std::endl;
-
- std::cout << "scenario-1-2-3/Model_3_Scenario_1" << std::endl;
- std::string filename7 = (path / "scenario-1-2-3/Model_3_Scenario_1.txt").string();
- scenePenetrationTest(filename7);
-
- std::cout << std::endl;
-
- std::cout << "scenario-1-2-3/Model_3_Scenario_2" << std::endl;
- std::string filename8 = (path / "scenario-1-2-3/Model_3_Scenario_2.txt").string();
- scenePenetrationTest(filename8);
-
- std::cout << std::endl;
-
- std::cout << "scenario-1-2-3/Model_3_Scenario_3" << std::endl;
- std::string filename9 = (path / "scenario-1-2-3/Model_3_Scenario_3.txt").string();
- scenePenetrationTest(filename9);
-
- std::cout << std::endl;
-
- std::cout << "newdata/Model_1_Scenario_3" << std::endl;
- std::string filename10 = (path / "newdata/Model_1_Scenario_3.xml").string();
- scenePenetrationTest(filename10);
-
- std::cout << std::endl;
-
-
- std::cout << "newdata/Model_2_Scenario_3" << std::endl;
- std::string filename11 = (path / "newdata/Model_2_Scenario_3.xml").string();
- scenePenetrationTest(filename11);
-
- std::cout << std::endl;
-
-
- std::cout << "newdata/Model_3_Scenario_3" << std::endl;
- std::string filename12 = (path / "newdata/Model_3_Scenario_3.xml").string();
- scenePenetrationTest(filename12);
-
- std::cout << std::endl;
-
-}
-
-BOOST_AUTO_TEST_CASE(xml2obj_test)
-{
- boost::filesystem::path path(TEST_RESOURCES_DIR);
-
- std::string filename_manyframe0 = (path / "manyframes/Model_5.xml").string();
- xml2obj(filename_manyframe0, "Model_5");
-
- std::string filename_manyframe1 = (path / "manyframes/Model_1.xml").string();
- xml2obj(filename_manyframe1, "Model_1");
-
- std::string filename_manyframe2 = (path / "manyframes/Model_4.xml").string();
- xml2obj(filename_manyframe2, "Model_4");
-
- std::string filename1 = (path / "scenario-1-2-3/Model_1_Scenario_1.txt").string();
- xml2obj(filename1, "Model_1_Scenario_1");
-
- std::string filename2 = (path / "scenario-1-2-3/Model_1_Scenario_2.txt").string();
- xml2obj(filename2, "Model_1_Scenario_2");
-
- std::string filename3 = (path / "scenario-1-2-3/Model_1_Scenario_3.txt").string();
- xml2obj(filename3, "Model_1_Scenario_3");
-
- std::string filename4 = (path / "scenario-1-2-3/Model_2_Scenario_1.txt").string();
- xml2obj(filename4, "Model_2_Scenario_1");
-
- std::string filename5 = (path / "scenario-1-2-3/Model_2_Scenario_2.txt").string();
- xml2obj(filename5, "Model_2_Scenario_2");
-
- std::string filename6 = (path / "scenario-1-2-3/Model_2_Scenario_3.txt").string();
- xml2obj(filename6, "Model_2_Scenario_3");
-
- std::string filename7 = (path / "scenario-1-2-3/Model_3_Scenario_1.txt").string();
- xml2obj(filename7, "Model_3_Scenario_1");
-
- std::string filename8 = (path / "scenario-1-2-3/Model_3_Scenario_2.txt").string();
- xml2obj(filename8, "Model_3_Scenario_2");
-
- std::string filename9 = (path / "scenario-1-2-3/Model_3_Scenario_3.txt").string();
- xml2obj(filename9, "Model_3_Scenario_3");
-
-}