fcl: test_fcl_distance.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 
00037 #define BOOST_TEST_MODULE "FCL_DISTANCE"
00038 #include <boost/test/unit_test.hpp>
00039 
00040 #include "fcl/traversal/traversal_node_bvhs.h"
00041 #include "fcl/traversal/traversal_node_setup.h"
00042 #include "fcl/collision_node.h"
00043 #include "test_fcl_utility.h"
00044 #include <boost/timer.hpp>
00045 #include "fcl_resources/config.h"
00046 #include <boost/filesystem.hpp>
00047 
00048 using namespace fcl;
00049 
00050 bool verbose = false;
00051 FCL_REAL DELTA = 0.001;
00052 
00053 
00054 template<typename BV>
00055 void distance_Test(const Transform3f& tf,
00056                    const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
00057                    const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method,
00058                    int qsize,
00059                    DistanceRes& distance_result,
00060                    bool verbose = true);
00061 
00062 bool collide_Test_OBB(const Transform3f& tf,
00063                       const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
00064                       const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method, bool verbose);
00065 
00066 
00067 template<typename BV, typename TraversalNode>
00068 void distance_Test_Oriented(const Transform3f& tf,
00069                             const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
00070                             const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method,
00071                             int qsize,
00072                             DistanceRes& distance_result,
00073                             bool verbose = true);
00074 
00075 inline bool nearlyEqual(const Vec3f& a, const Vec3f& b)
00076 {
00077   if(fabs(a[0] - b[0]) > DELTA) return false;
00078   if(fabs(a[1] - b[1]) > DELTA) return false;
00079   if(fabs(a[2] - b[2]) > DELTA) return false;
00080   return true;
00081 }
00082 
00083 
00084 BOOST_AUTO_TEST_CASE(mesh_distance)
00085 {
00086   std::vector<Vec3f> p1, p2;
00087   std::vector<Triangle> t1, t2;
00088   boost::filesystem::path path(TEST_RESOURCES_DIR);
00089   loadOBJFile((path / "env.obj").string().c_str(), p1, t1);
00090   loadOBJFile((path / "rob.obj").string().c_str(), p2, t2);
00091 
00092   std::vector<Transform3f> transforms; // t0
00093   FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000};
00094   std::size_t n = 10;
00095 
00096   generateRandomTransforms(extents, transforms, n);
00097 
00098   double dis_time = 0;
00099   double col_time = 0;
00100 
00101   DistanceRes res, res_now;
00102   for(std::size_t i = 0; i < transforms.size(); ++i)
00103   {
00104     boost::timer timer_col;
00105     collide_Test_OBB(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
00106     col_time += timer_col.elapsed();
00107 
00108     boost::timer timer_dist;
00109     distance_Test_Oriented<RSS, MeshDistanceTraversalNodeRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res, verbose);
00110     dis_time += timer_dist.elapsed();
00111 
00112     distance_Test_Oriented<RSS, MeshDistanceTraversalNodeRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose);
00113 
00114     BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
00115     BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
00116 
00117     distance_Test_Oriented<RSS, MeshDistanceTraversalNodeRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose);
00118 
00119     BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
00120     BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
00121 
00122     distance_Test_Oriented<RSS, MeshDistanceTraversalNodeRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose);
00123 
00124     BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
00125     BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
00126 
00127     distance_Test_Oriented<RSS, MeshDistanceTraversalNodeRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose);
00128 
00129     BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
00130     BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
00131 
00132     distance_Test_Oriented<RSS, MeshDistanceTraversalNodeRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose);
00133 
00134     BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
00135     BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
00136     
00137     distance_Test_Oriented<kIOS, MeshDistanceTraversalNodekIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose);
00138 
00139     BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
00140     BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
00141     
00142     distance_Test_Oriented<kIOS, MeshDistanceTraversalNodekIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose);
00143     
00144     BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
00145     BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
00146 
00147     distance_Test_Oriented<kIOS, MeshDistanceTraversalNodekIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose);
00148     
00149     BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
00150     BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
00151 
00152     distance_Test_Oriented<kIOS, MeshDistanceTraversalNodekIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose);
00153     
00154     BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
00155     BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
00156 
00157     distance_Test_Oriented<kIOS, MeshDistanceTraversalNodekIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose);
00158     
00159     BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
00160     BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
00161 
00162     distance_Test_Oriented<kIOS, MeshDistanceTraversalNodekIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose);
00163     
00164     BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
00165     BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
00166 
00167     distance_Test_Oriented<OBBRSS, MeshDistanceTraversalNodeOBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose);
00168 
00169     BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
00170     BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
00171     
00172     distance_Test_Oriented<OBBRSS, MeshDistanceTraversalNodeOBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose);
00173     
00174     BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
00175     BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
00176 
00177     distance_Test_Oriented<OBBRSS, MeshDistanceTraversalNodeOBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose);
00178     
00179     BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
00180     BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
00181 
00182     distance_Test_Oriented<OBBRSS, MeshDistanceTraversalNodeOBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose);
00183     
00184     BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
00185     BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
00186 
00187     distance_Test_Oriented<OBBRSS, MeshDistanceTraversalNodeOBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose);
00188     
00189     BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
00190     BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
00191 
00192     distance_Test_Oriented<OBBRSS, MeshDistanceTraversalNodeOBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose);
00193     
00194     BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
00195     BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
00196 
00197 
00198 
00199     distance_Test<RSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose);
00200 
00201     BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
00202     BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
00203 
00204     distance_Test<RSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose);
00205 
00206     BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
00207     BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
00208 
00209     distance_Test<RSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose);
00210 
00211     BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
00212     BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
00213 
00214     distance_Test<RSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose);
00215 
00216     BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
00217     BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
00218 
00219     distance_Test<RSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose);
00220 
00221     BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
00222     BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
00223 
00224     distance_Test<RSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose);
00225 
00226     BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
00227     BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
00228     
00229     distance_Test<kIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose);
00230 
00231     BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
00232     BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
00233     
00234     distance_Test<kIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose);
00235 
00236     BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
00237     BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
00238     
00239     distance_Test<kIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose);
00240 
00241     BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
00242     BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
00243 
00244     distance_Test<kIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose);
00245 
00246     BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
00247     BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
00248 
00249     distance_Test<kIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose);
00250 
00251     BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
00252     BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
00253 
00254     distance_Test<kIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose);
00255 
00256     BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
00257     BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
00258 
00259 
00260     distance_Test<OBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose);
00261 
00262     BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
00263     BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
00264     
00265     distance_Test<OBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose);
00266 
00267     BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
00268     BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
00269     
00270     distance_Test<OBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose);
00271 
00272     BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
00273     BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
00274 
00275     distance_Test<OBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose);
00276 
00277     BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
00278     BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
00279 
00280     distance_Test<OBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose);
00281 
00282     BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
00283     BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
00284 
00285     distance_Test<OBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose);
00286 
00287     BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
00288     BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
00289 
00290   }
00291 
00292   std::cout << "distance timing: " << dis_time << " sec" << std::endl;
00293   std::cout << "collision timing: " << col_time << " sec" << std::endl;
00294 }
00295 
00296 template<typename BV, typename TraversalNode>
00297 void distance_Test_Oriented(const Transform3f& tf,
00298                             const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
00299                             const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method,
00300                             int qsize,
00301                             DistanceRes& distance_result,
00302                             bool verbose)
00303 {
00304   BVHModel<BV> m1;
00305   BVHModel<BV> m2;
00306   m1.bv_splitter.reset(new BVSplitter<BV>(split_method));
00307   m2.bv_splitter.reset(new BVSplitter<BV>(split_method));
00308 
00309 
00310   m1.beginModel();
00311   m1.addSubModel(vertices1, triangles1);
00312   m1.endModel();
00313 
00314   m2.beginModel();
00315   m2.addSubModel(vertices2, triangles2);
00316   m2.endModel();
00317 
00318   DistanceResult local_result;
00319   TraversalNode node;
00320   if(!initialize(node, (const BVHModel<BV>&)m1, tf, (const BVHModel<BV>&)m2, Transform3f(), DistanceRequest(true), local_result))
00321     std::cout << "initialize error" << std::endl;
00322 
00323   node.enable_statistics = verbose;
00324 
00325   distance(&node, NULL, qsize);
00326 
00327   // points are in local coordinate, to global coordinate
00328   Vec3f p1 = local_result.nearest_points[0];
00329   Vec3f p2 = local_result.nearest_points[1];
00330 
00331 
00332   distance_result.distance = local_result.min_distance;
00333   distance_result.p1 = p1;
00334   distance_result.p2 = p2;
00335 
00336   if(verbose)
00337   {
00338     std::cout << "distance " << local_result.min_distance << std::endl;
00339 
00340     std::cout << p1[0] << " " << p1[1] << " " << p1[2] << std::endl;
00341     std::cout << p2[0] << " " << p2[1] << " " << p2[2] << std::endl;
00342     std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl;
00343   }
00344 }
00345 
00346 template<typename BV>
00347 void distance_Test(const Transform3f& tf,
00348                    const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
00349                    const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method,
00350                    int qsize,
00351                    DistanceRes& distance_result,
00352                    bool verbose)
00353 {
00354   BVHModel<BV> m1;
00355   BVHModel<BV> m2;
00356   m1.bv_splitter.reset(new BVSplitter<BV>(split_method));
00357   m2.bv_splitter.reset(new BVSplitter<BV>(split_method));
00358 
00359 
00360   m1.beginModel();
00361   m1.addSubModel(vertices1, triangles1);
00362   m1.endModel();
00363 
00364   m2.beginModel();
00365   m2.addSubModel(vertices2, triangles2);
00366   m2.endModel();
00367 
00368   Transform3f pose1(tf), pose2;
00369 
00370   DistanceResult local_result;
00371   MeshDistanceTraversalNode<BV> node;
00372 
00373   if(!initialize<BV>(node, m1, pose1, m2, pose2, DistanceRequest(true), local_result))
00374     std::cout << "initialize error" << std::endl;
00375 
00376   node.enable_statistics = verbose;
00377 
00378   distance(&node, NULL, qsize);
00379 
00380   distance_result.distance = local_result.min_distance;
00381   distance_result.p1 = local_result.nearest_points[0];
00382   distance_result.p2 = local_result.nearest_points[1];
00383 
00384   if(verbose)
00385   {
00386     std::cout << "distance " << local_result.min_distance << std::endl;
00387 
00388     std::cout << local_result.nearest_points[0][0] << " " << local_result.nearest_points[0][1] << " " << local_result.nearest_points[0][2] << std::endl;
00389     std::cout << local_result.nearest_points[1][0] << " " << local_result.nearest_points[1][1] << " " << local_result.nearest_points[1][2] << std::endl;
00390     std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl;
00391   }
00392 }
00393 
00394 
00395 bool collide_Test_OBB(const Transform3f& tf,
00396                       const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
00397                       const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method, bool verbose)
00398 {
00399   BVHModel<OBB> m1;
00400   BVHModel<OBB> m2;
00401   m1.bv_splitter.reset(new BVSplitter<OBB>(split_method));
00402   m2.bv_splitter.reset(new BVSplitter<OBB>(split_method));
00403 
00404   m1.beginModel();
00405   m1.addSubModel(vertices1, triangles1);
00406   m1.endModel();
00407 
00408   m2.beginModel();
00409   m2.addSubModel(vertices2, triangles2);
00410   m2.endModel();
00411 
00412   CollisionResult local_result; 
00413   MeshCollisionTraversalNodeOBB node;
00414   if(!initialize(node, (const BVHModel<OBB>&)m1, tf, (const BVHModel<OBB>&)m2, Transform3f(),
00415                  CollisionRequest(), local_result))
00416     std::cout << "initialize error" << std::endl;
00417 
00418   node.enable_statistics = verbose;
00419 
00420   collide(&node);
00421 
00422   if(local_result.numContacts() > 0)
00423     return true;
00424   else
00425     return false;
00426 }
00427 
00428