fcl: test_fcl_frontlist.cpp Source File

Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  */
00034 
00038 #define BOOST_TEST_MODULE "FCL_FRONT_LIST"
00039 #include <boost/test/unit_test.hpp>
00040 
00041 #include "fcl/traversal/traversal_node_bvhs.h"
00042 #include "fcl/traversal/traversal_node_setup.h"
00043 #include "fcl/collision_node.h"
00044 #include "test_fcl_utility.h"
00045 
00046 #include "fcl_resources/config.h"
00047 #include <boost/filesystem.hpp>
00048 
00049 using namespace fcl;
00050 
00051 template<typename BV>
00052 bool collide_front_list_Test(const Transform3f& tf1, const Transform3f& tf2,
00053                              const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
00054                              const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2,
00055                              SplitMethodType split_method,
00056                              bool refit_bottomup, bool verbose);
00057 
00058 template<typename BV, typename TraversalNode>
00059 bool collide_front_list_Test_Oriented(const Transform3f& tf1, const Transform3f& tf2,
00060                                       const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
00061                                       const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2,
00062                                       SplitMethodType split_method, bool verbose);
00063 
00064 
00065 template<typename BV>
00066 bool collide_Test(const Transform3f& tf,
00067                   const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
00068                   const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method, bool verbose);
00069 
00070 // TODO: randomly still have some runtime error
00071 BOOST_AUTO_TEST_CASE(front_list)
00072 {
00073   std::vector<Vec3f> p1, p2;
00074   std::vector<Triangle> t1, t2;
00075   boost::filesystem::path path(TEST_RESOURCES_DIR);
00076   loadOBJFile((path / "env.obj").string().c_str(), p1, t1);
00077   loadOBJFile((path / "rob.obj").string().c_str(), p2, t2);
00078 
00079   std::vector<Transform3f> transforms; // t0
00080   std::vector<Transform3f> transforms2; // t1
00081   FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000};
00082   FCL_REAL delta_trans[] = {1, 1, 1};
00083   std::size_t n = 10;
00084   bool verbose = false;
00085 
00086   generateRandomTransforms(extents, delta_trans, 0.005 * 2 * 3.1415, transforms, transforms2, n);
00087 
00088   bool res, res2;
00089 
00090   for(std::size_t i = 0; i < transforms.size(); ++i)
00091   {
00092     res = collide_Test<AABB>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
00093     res2 = collide_front_list_Test<AABB>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose);
00094     BOOST_CHECK(res == res2);
00095     res = collide_Test<AABB>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
00096     res2 = collide_front_list_Test<AABB>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose);
00097     BOOST_CHECK(res == res2);
00098     res = collide_Test<AABB>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
00099     res2 = collide_front_list_Test<AABB>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose);
00100     BOOST_CHECK(res == res2);
00101   }
00102 
00103   for(std::size_t i = 0; i < transforms.size(); ++i)
00104   {
00105     res = collide_Test<OBB>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
00106     res2 = collide_front_list_Test<OBB>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose);
00107     BOOST_CHECK(res == res2);
00108     res = collide_Test<OBB>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
00109     res2 = collide_front_list_Test<OBB>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose);
00110     BOOST_CHECK(res == res2);
00111     res = collide_Test<OBB>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
00112     res2 = collide_front_list_Test<OBB>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose);
00113     BOOST_CHECK(res == res2);
00114   }
00115 
00116   for(std::size_t i = 0; i < transforms.size(); ++i)
00117   {
00118     res = collide_Test<RSS>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
00119     res2 = collide_front_list_Test<RSS>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose);
00120     BOOST_CHECK(res == res2);
00121     res = collide_Test<RSS>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
00122     res2 = collide_front_list_Test<RSS>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose);
00123     BOOST_CHECK(res == res2);
00124     res = collide_Test<RSS>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
00125     res2 = collide_front_list_Test<RSS>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose);
00126     BOOST_CHECK(res == res2);
00127   }
00128 
00129   for(std::size_t i = 0; i < transforms.size(); ++i)
00130   {
00131     res = collide_Test<KDOP<16> >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
00132     res2 = collide_front_list_Test<KDOP<16> >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose);
00133     BOOST_CHECK(res == res2);
00134     res = collide_Test<KDOP<16> >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
00135     res2 = collide_front_list_Test<KDOP<16> >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose);
00136     BOOST_CHECK(res == res2);
00137     res = collide_Test<KDOP<16> >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
00138     res2 = collide_front_list_Test<KDOP<16> >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose);
00139     BOOST_CHECK(res == res2);
00140   }
00141 
00142   for(std::size_t i = 0; i < transforms.size(); ++i)
00143   {
00144     res = collide_Test<KDOP<18> >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
00145     res2 = collide_front_list_Test<KDOP<18> >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose);
00146     BOOST_CHECK(res == res2);
00147     res = collide_Test<KDOP<18> >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
00148     res2 = collide_front_list_Test<KDOP<18> >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose);
00149     BOOST_CHECK(res == res2);
00150     res = collide_Test<KDOP<18> >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
00151     res2 = collide_front_list_Test<KDOP<18> >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose);
00152     BOOST_CHECK(res == res2);
00153   }
00154 
00155   for(std::size_t i = 0; i < transforms.size(); ++i)
00156   {
00157     res = collide_Test<KDOP<24> >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
00158     res2 = collide_front_list_Test<KDOP<24> >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose);
00159     BOOST_CHECK(res == res2);
00160     res = collide_Test<KDOP<24> >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
00161     res2 = collide_front_list_Test<KDOP<24> >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose);
00162     BOOST_CHECK(res == res2);
00163     res = collide_Test<KDOP<24> >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
00164     res2 = collide_front_list_Test<KDOP<24> >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose);
00165     BOOST_CHECK(res == res2);
00166   }
00167 
00168   for(std::size_t i = 0; i < transforms.size(); ++i)
00169   {
00170     res = collide_Test<RSS>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
00171     res2 = collide_front_list_Test_Oriented<RSS, MeshCollisionTraversalNodeRSS>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
00172     BOOST_CHECK(res == res2);
00173     res = collide_Test<RSS>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
00174     res2 = collide_front_list_Test_Oriented<RSS, MeshCollisionTraversalNodeRSS>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
00175     BOOST_CHECK(res == res2);
00176     res = collide_Test<RSS>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
00177     res2 = collide_front_list_Test_Oriented<RSS, MeshCollisionTraversalNodeRSS>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
00178     BOOST_CHECK(res == res2);
00179   }
00180 
00181   for(std::size_t i = 0; i < transforms.size(); ++i)
00182   {
00183     res = collide_Test<OBB>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
00184     res2 = collide_front_list_Test_Oriented<OBB, MeshCollisionTraversalNodeOBB>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
00185     BOOST_CHECK(res == res2);
00186     res = collide_Test<OBB>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
00187     res2 = collide_front_list_Test_Oriented<OBB, MeshCollisionTraversalNodeOBB>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
00188     BOOST_CHECK(res == res2);
00189     res = collide_Test<OBB>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
00190     res2 = collide_front_list_Test_Oriented<OBB, MeshCollisionTraversalNodeOBB>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
00191     BOOST_CHECK(res == res2);
00192   }
00193 
00194 }
00195 
00196 template<typename BV>
00197 bool collide_front_list_Test(const Transform3f& tf1, const Transform3f& tf2,
00198                              const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
00199                              const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2,
00200                              SplitMethodType split_method,
00201                              bool refit_bottomup, bool verbose)
00202 {
00203   BVHModel<BV> m1;
00204   BVHModel<BV> m2;
00205   m1.bv_splitter.reset(new BVSplitter<BV>(split_method));
00206   m2.bv_splitter.reset(new BVSplitter<BV>(split_method));
00207 
00208   BVHFrontList front_list;
00209 
00210 
00211   std::vector<Vec3f> vertices1_new(vertices1.size());
00212   for(std::size_t i = 0; i < vertices1_new.size(); ++i)
00213   {
00214     vertices1_new[i] = tf1.transform(vertices1[i]);
00215   }
00216 
00217   m1.beginModel();
00218   m1.addSubModel(vertices1_new, triangles1);
00219   m1.endModel();
00220 
00221   m2.beginModel();
00222   m2.addSubModel(vertices2, triangles2);
00223   m2.endModel();
00224 
00225   Transform3f pose1, pose2;
00226 
00227   CollisionResult local_result; 
00228   MeshCollisionTraversalNode<BV> node;
00229 
00230   if(!initialize<BV>(node, m1, pose1, m2, pose2,
00231                      CollisionRequest(std::numeric_limits<int>::max(), false), local_result))
00232     std::cout << "initialize error" << std::endl;
00233 
00234   node.enable_statistics = verbose;
00235 
00236   collide(&node, &front_list);
00237 
00238   if(verbose) std::cout << "front list size " << front_list.size() << std::endl;
00239 
00240 
00241   // update the mesh
00242   for(std::size_t i = 0; i < vertices1.size(); ++i)
00243   {
00244     vertices1_new[i] = tf2.transform(vertices1[i]);
00245   }
00246 
00247   m1.beginReplaceModel();
00248   m1.replaceSubModel(vertices1_new);
00249   m1.endReplaceModel(true, refit_bottomup);
00250 
00251   m2.beginReplaceModel();
00252   m2.replaceSubModel(vertices2);
00253   m2.endReplaceModel(true, refit_bottomup);
00254 
00255   local_result.clear();
00256   collide(&node, &front_list);
00257 
00258   if(local_result.numContacts() > 0)
00259     return true;
00260   else
00261     return false;
00262 }
00263 
00264 
00265 
00266 
00267 template<typename BV, typename TraversalNode>
00268 bool collide_front_list_Test_Oriented(const Transform3f& tf1, const Transform3f& tf2,
00269                                       const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
00270                                       const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2,
00271                                       SplitMethodType split_method, bool verbose)
00272 {
00273   BVHModel<BV> m1;
00274   BVHModel<BV> m2;
00275   m1.bv_splitter.reset(new BVSplitter<BV>(split_method));
00276   m2.bv_splitter.reset(new BVSplitter<BV>(split_method));
00277 
00278   BVHFrontList front_list;
00279 
00280   m1.beginModel();
00281   m1.addSubModel(vertices1, triangles1);
00282   m1.endModel();
00283 
00284   m2.beginModel();
00285   m2.addSubModel(vertices2, triangles2);
00286   m2.endModel();
00287 
00288   Transform3f pose1(tf1), pose2;
00289 
00290   CollisionResult local_result; 
00291   TraversalNode node;
00292 
00293   if(!initialize(node, (const BVHModel<BV>&)m1, pose1, (const BVHModel<BV>&)m2, pose2,
00294                  CollisionRequest(std::numeric_limits<int>::max(), false), local_result))
00295     std::cout << "initialize error" << std::endl;
00296 
00297   node.enable_statistics = verbose;
00298 
00299   collide(&node, &front_list);
00300 
00301   if(verbose) std::cout << "front list size " << front_list.size() << std::endl;
00302 
00303 
00304   // update the mesh
00305   pose1 = tf2;
00306   if(!initialize(node, (const BVHModel<BV>&)m1, pose1, (const BVHModel<BV>&)m2, pose2, CollisionRequest(), local_result))
00307     std::cout << "initialize error" << std::endl;
00308 
00309   local_result.clear();
00310   collide(&node, &front_list);
00311 
00312   if(local_result.numContacts() > 0)
00313     return true;
00314   else
00315     return false;
00316 }
00317 
00318 
00319 template<typename BV>
00320 bool collide_Test(const Transform3f& tf,
00321                   const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
00322                   const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method, bool verbose)
00323 {
00324   BVHModel<BV> m1;
00325   BVHModel<BV> m2;
00326   m1.bv_splitter.reset(new BVSplitter<BV>(split_method));
00327   m2.bv_splitter.reset(new BVSplitter<BV>(split_method));
00328 
00329   m1.beginModel();
00330   m1.addSubModel(vertices1, triangles1);
00331   m1.endModel();
00332 
00333   m2.beginModel();
00334   m2.addSubModel(vertices2, triangles2);
00335   m2.endModel();
00336 
00337   Transform3f pose1(tf), pose2;
00338 
00339   CollisionResult local_result; 
00340   MeshCollisionTraversalNode<BV> node;
00341 
00342   if(!initialize<BV>(node, m1, pose1, m2, pose2,
00343                      CollisionRequest(std::numeric_limits<int>::max(), false), local_result))
00344     std::cout << "initialize error" << std::endl;
00345 
00346   node.enable_statistics = verbose;
00347 
00348   collide(&node);
00349 
00350 
00351   if(local_result.numContacts() > 0)
00352     return true;
00353   else
00354     return false;
00355 }
00356