test_fcl_shape_mesh_consistency.cpp
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_SHAPE_MESH_CONSISTENCY"
00038 #include <boost/test/unit_test.hpp>
00039 
00040 #include "fcl/narrowphase/narrowphase.h"
00041 #include "fcl/shape/geometric_shape_to_BVH_model.h"
00042 #include "fcl/distance.h"
00043 #include "fcl/collision.h"
00044 #include "test_fcl_utility.h"
00045 
00046 
00047 using namespace fcl;
00048 
00049 #define BOOST_CHECK_FALSE(p) BOOST_CHECK(!(p))
00050 
00051 GJKSolver_libccd solver1;
00052 GJKSolver_indep solver2;
00053 
00054 FCL_REAL extents[6] = {0, 0, 0, 10, 10, 10};
00055 
00056 BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere_libccd)
00057 {
00058   Sphere s1(20);
00059   Sphere s2(20);
00060   BVHModel<RSS> s1_rss;
00061   BVHModel<RSS> s2_rss;
00062 
00063   generateBVHModel(s1_rss, s1, Transform3f(), 16, 16);
00064   generateBVHModel(s2_rss, s2, Transform3f(), 16, 16);
00065 
00066   DistanceRequest request;
00067   DistanceResult res, res1;
00068 
00069   Transform3f pose;
00070 
00071   pose.setTranslation(Vec3f(50, 0, 0));
00072 
00073   res.clear(); res1.clear();
00074   distance(&s1, Transform3f(), &s2, pose, &solver1, request, res);
00075   distance(&s1_rss, Transform3f(), &s2_rss, pose, &solver1, request, res1);
00076   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);  
00077 
00078   res1.clear();
00079   distance(&s1, Transform3f(), &s2_rss, pose, &solver1, request, res1);
00080   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
00081   
00082   res1.clear();
00083   distance(&s1_rss, Transform3f(), &s2, pose, &solver1, request, res1);
00084   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
00085 
00086   for(std::size_t i = 0; i < 10; ++i)
00087   {
00088     Transform3f t;
00089     generateRandomTransform(extents, t);
00090     
00091     Transform3f pose1(t);
00092     Transform3f pose2 = t * pose;
00093 
00094     res.clear(); res1.clear();
00095     distance(&s1, pose1, &s2, pose2, request, res);
00096     distance(&s1_rss, pose1, &s2_rss, pose2, &solver1, request, res1);
00097     BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
00098 
00099     res1.clear();
00100     distance(&s1, pose1, &s2_rss, pose2, &solver1, request, res1);
00101     BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
00102 
00103     res1.clear();
00104     distance(&s1_rss, pose1, &s2, pose2, &solver1, request, res1);
00105     BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
00106   }
00107 
00108   pose.setTranslation(Vec3f(40.1, 0, 0));
00109 
00110   res.clear(), res1.clear();
00111   distance(&s1, Transform3f(), &s2, pose, &solver1, request, res);
00112   distance(&s1_rss, Transform3f(), &s2_rss, pose, &solver1, request, res1);
00113   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
00114 
00115   res1.clear();
00116   distance(&s1, Transform3f(), &s2_rss, pose, &solver1, request, res1);
00117   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
00118 
00119   res1.clear();
00120   distance(&s1_rss, Transform3f(), &s2, pose, &solver1, request, res1);
00121   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
00122 
00123 
00124   for(std::size_t i = 0; i < 10; ++i)
00125   {
00126     Transform3f t;
00127     generateRandomTransform(extents, t);
00128     
00129     Transform3f pose1(t);
00130     Transform3f pose2 = t * pose;
00131 
00132     res.clear(); res1.clear();
00133     distance(&s1, pose1, &s2, pose2, &solver1, request, res);
00134     distance(&s1_rss, pose1, &s2_rss, pose2, &solver1, request, res1);
00135     BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
00136 
00137     res1.clear();
00138     distance(&s1, pose1, &s2_rss, pose2, &solver1, request, res1);
00139     BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
00140 
00141     res1.clear();
00142     distance(&s1_rss, pose1, &s2, pose2, &solver1, request, res1);
00143     BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
00144   }
00145 }
00146 
00147 BOOST_AUTO_TEST_CASE(consistency_distance_boxbox_libccd)
00148 {
00149   Box s1(20, 40, 50);
00150   Box s2(10, 10, 10);
00151 
00152   BVHModel<RSS> s1_rss;
00153   BVHModel<RSS> s2_rss;
00154 
00155   generateBVHModel(s1_rss, s1, Transform3f());
00156   generateBVHModel(s2_rss, s2, Transform3f());
00157 
00158   DistanceRequest request;
00159   DistanceResult res, res1;
00160 
00161   Transform3f pose;
00162 
00163   pose.setTranslation(Vec3f(50, 0, 0));
00164 
00165   res.clear(); res1.clear();
00166   distance(&s1, Transform3f(), &s2, pose, &solver1, request, res);
00167   distance(&s1_rss, Transform3f(), &s2_rss, pose, &solver1, request, res1);
00168   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
00169   
00170   res1.clear();
00171   distance(&s1, Transform3f(), &s2_rss, pose, &solver1, request, res1);
00172   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
00173 
00174   res1.clear();
00175   distance(&s1_rss, Transform3f(), &s2, pose, &solver1, request, res1);
00176   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
00177 
00178   for(std::size_t i = 0; i < 10; ++i)
00179   {
00180     Transform3f t;
00181     generateRandomTransform(extents, t);
00182     
00183     Transform3f pose1(t);
00184     Transform3f pose2 = t * pose;
00185 
00186     res.clear(); res1.clear();
00187     distance(&s1, pose1, &s2, pose2, &solver1, request, res);
00188     distance(&s1_rss, pose1, &s2_rss, pose2, &solver1, request, res1);
00189     BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
00190 
00191     res1.clear();
00192     distance(&s1, pose1, &s2_rss, pose2, &solver1, request, res1);
00193     BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
00194 
00195     res1.clear();
00196     distance(&s1_rss, pose1, &s2, pose2, &solver1, request, res1);
00197     BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
00198   }
00199 
00200   pose.setTranslation(Vec3f(15.1, 0, 0));
00201   
00202   res.clear(); res1.clear();
00203   distance(&s1, Transform3f(), &s2, pose, &solver1, request, res);
00204   distance(&s1_rss, Transform3f(), &s2_rss, pose, &solver1, request, res1);
00205   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
00206   
00207   res1.clear();
00208   distance(&s1, Transform3f(), &s2_rss, pose, &solver1, request, res1);
00209   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
00210 
00211   res1.clear();
00212   distance(&s1_rss, Transform3f(), &s2, pose, &solver1, request, res1);
00213   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
00214 
00215   for(std::size_t i = 0; i < 10; ++i)
00216   {
00217     Transform3f t;
00218     generateRandomTransform(extents, t);
00219     
00220     Transform3f pose1(t);
00221     Transform3f pose2 = t * pose;
00222 
00223     res.clear(); res1.clear();
00224     distance(&s1, pose1, &s2, pose2, &solver1, request, res);
00225     distance(&s1_rss, pose1, &s2_rss, pose2, &solver1, request, res1);
00226     BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
00227 
00228     res1.clear();
00229     distance(&s1, pose1, &s2_rss, pose2, &solver1, request, res1);
00230     BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
00231 
00232     res1.clear();
00233     distance(&s1_rss, pose1, &s2, pose2, &solver1, request, res1);
00234     BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
00235   }
00236 }
00237 
00238 BOOST_AUTO_TEST_CASE(consistency_distance_cylindercylinder_libccd)
00239 {
00240   Cylinder s1(5, 10);
00241   Cylinder s2(5, 10);
00242 
00243   BVHModel<RSS> s1_rss;
00244   BVHModel<RSS> s2_rss;
00245 
00246   generateBVHModel(s1_rss, s1, Transform3f(), 16, 16);
00247   generateBVHModel(s2_rss, s2, Transform3f(), 16, 16);
00248 
00249   DistanceRequest request;
00250   DistanceResult res, res1;
00251 
00252   Transform3f pose;
00253 
00254   pose.setTranslation(Vec3f(20, 0, 0));
00255 
00256   res.clear(); res1.clear();
00257   distance(&s1, Transform3f(), &s2, pose, &solver1, request, res);
00258   distance(&s1_rss, Transform3f(), &s2_rss, pose, &solver1, request, res1);
00259   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
00260   
00261   res1.clear();
00262   distance(&s1, Transform3f(), &s2_rss, pose, &solver1, request, res1);
00263   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
00264   
00265   res1.clear();
00266   distance(&s1_rss, Transform3f(), &s2, pose, &solver1, request, res1);
00267   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
00268   
00269   for(std::size_t i = 0; i < 10; ++i)
00270   {
00271     Transform3f t;
00272     generateRandomTransform(extents, t);
00273     
00274     Transform3f pose1(t);
00275     Transform3f pose2 = t * pose;
00276 
00277     res.clear(); res1.clear();
00278     distance(&s1, pose1, &s2, pose2, &solver1, request, res);
00279     distance(&s1_rss, pose1, &s2_rss, pose2, &solver1, request, res1);
00280     BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
00281 
00282     res1.clear();
00283     distance(&s1, pose1, &s2_rss, pose2, &solver1, request, res1);
00284     BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
00285 
00286     res1.clear();
00287     distance(&s1_rss, pose1, &s2, pose2, &solver1, request, res1);
00288     BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
00289   }
00290   
00291   pose.setTranslation(Vec3f(15, 0, 0)); // libccd cannot use small value here :( 
00292   
00293   res.clear(); res1.clear();
00294   distance(&s1, Transform3f(), &s2, pose, &solver1, request, res);
00295   distance(&s1_rss, Transform3f(), &s2_rss, pose, &solver1, request, res1);
00296   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
00297   
00298   res1.clear();
00299   distance(&s1, Transform3f(), &s2_rss, pose, &solver1, request, res1);
00300   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
00301 
00302   res1.clear();
00303   distance(&s1_rss, Transform3f(), &s2, pose, &solver1, request, res1);
00304   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
00305   
00306   for(std::size_t i = 0; i < 10; ++i)
00307   {
00308     Transform3f t;
00309     generateRandomTransform(extents, t);
00310     
00311     Transform3f pose1(t);
00312     Transform3f pose2 = t * pose;
00313 
00314     res.clear(); res1.clear();
00315     distance(&s1, pose1, &s2, pose2, &solver1, request, res);
00316     distance(&s1_rss, pose1, &s2_rss, pose2, &solver1, request, res1);
00317     BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
00318 
00319     res1.clear();
00320     distance(&s1, pose1, &s2_rss, pose2, &solver1, request, res1);
00321     BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
00322 
00323     res1.clear();
00324     distance(&s1_rss, pose1, &s2, pose2, &solver1, request, res1);
00325     BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
00326   }
00327 }
00328 
00329 BOOST_AUTO_TEST_CASE(consistency_distance_conecone_libccd)
00330 {
00331   Cone s1(5, 10);
00332   Cone s2(5, 10);
00333 
00334   BVHModel<RSS> s1_rss;
00335   BVHModel<RSS> s2_rss;
00336 
00337   generateBVHModel(s1_rss, s1, Transform3f(), 16, 16);
00338   generateBVHModel(s2_rss, s2, Transform3f(), 16, 16);
00339 
00340   DistanceRequest request;
00341   DistanceResult res, res1;
00342 
00343   Transform3f pose;
00344 
00345   pose.setTranslation(Vec3f(20, 0, 0));
00346   
00347   res.clear(); res1.clear();
00348   distance(&s1, Transform3f(), &s2, pose, &solver1, request, res);
00349   distance(&s1_rss, Transform3f(), &s2_rss, pose, &solver1, request, res1);
00350   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
00351   
00352   res1.clear();
00353   distance(&s1, Transform3f(), &s2_rss, pose, &solver1, request, res1);
00354   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
00355   
00356   distance(&s1_rss, Transform3f(), &s2, pose, &solver1, request, res1);
00357   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
00358   
00359   for(std::size_t i = 0; i < 10; ++i)
00360   {
00361     Transform3f t;
00362     generateRandomTransform(extents, t);
00363     
00364     Transform3f pose1(t);
00365     Transform3f pose2 = t * pose;
00366 
00367     res.clear(); res1.clear();
00368     distance(&s1, pose1, &s2, pose2, &solver1, request, res);
00369     distance(&s1_rss, pose1, &s2_rss, pose2, &solver1, request, res1);
00370     BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
00371 
00372     res1.clear();
00373     distance(&s1, pose1, &s2_rss, pose2, &solver1, request, res1);
00374     BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
00375 
00376     res1.clear();
00377     distance(&s1_rss, pose1, &s2, pose2, &solver1, request, res1);
00378     BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
00379   }
00380   
00381   pose.setTranslation(Vec3f(15, 0, 0)); // libccd cannot use small value here :( 
00382   
00383   res.clear(); res1.clear();
00384   distance(&s1, Transform3f(), &s2, pose, &solver1, request, res);
00385   distance(&s1_rss, Transform3f(), &s2_rss, pose, &solver1, request, res1);
00386   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
00387   
00388   res1.clear();
00389   distance(&s1, Transform3f(), &s2_rss, pose, &solver1, request, res1);
00390   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
00391 
00392   res1.clear();
00393   distance(&s1_rss, Transform3f(), &s2, pose, &solver1, request, res1);
00394   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
00395   
00396   for(std::size_t i = 0; i < 10; ++i)
00397   {
00398     Transform3f t;
00399     generateRandomTransform(extents, t);
00400     
00401     Transform3f pose1(t);
00402     Transform3f pose2 = t * pose;
00403 
00404     res.clear(); res1.clear();
00405     distance(&s1, pose1, &s2, pose2, &solver1, request, res);
00406     distance(&s1_rss, pose1, &s2_rss, pose2, &solver1, request, res1);
00407     BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
00408 
00409     res1.clear();
00410     distance(&s1, pose1, &s2_rss, pose2, &solver1, request, res1);
00411     BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
00412 
00413     res1.clear();
00414     distance(&s1_rss, pose1, &s2, pose2, &solver1, request, res1);
00415     BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
00416   }
00417 }
00418 
00419 
00420 BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere_GJK)
00421 {
00422   Sphere s1(20);
00423   Sphere s2(20);
00424   BVHModel<RSS> s1_rss;
00425   BVHModel<RSS> s2_rss;
00426 
00427   generateBVHModel(s1_rss, s1, Transform3f(), 16, 16);
00428   generateBVHModel(s2_rss, s2, Transform3f(), 16, 16);
00429 
00430   DistanceRequest request;
00431   DistanceResult res, res1;
00432 
00433   Transform3f pose;
00434 
00435   pose.setTranslation(Vec3f(50, 0, 0));
00436 
00437   res.clear(); res1.clear();
00438   distance(&s1, Transform3f(), &s2, pose, &solver2, request, res);
00439   distance(&s1_rss, Transform3f(), &s2_rss, pose, &solver2, request, res1);
00440   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);  
00441 
00442   res1.clear();
00443   distance(&s1, Transform3f(), &s2_rss, pose, &solver2, request, res1);
00444   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
00445   
00446   res1.clear();
00447   distance(&s1_rss, Transform3f(), &s2, pose, &solver2, request, res1);
00448   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
00449 
00450 
00451   for(std::size_t i = 0; i < 10; ++i)
00452   {
00453     Transform3f t;
00454     generateRandomTransform(extents, t);
00455     
00456     Transform3f pose1(t);
00457     Transform3f pose2 = t * pose;
00458 
00459     res.clear(); res1.clear();
00460     distance(&s1, pose1, &s2, pose2, request, res);
00461     distance(&s1_rss, pose1, &s2_rss, pose2, &solver2, request, res1);
00462     BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
00463 
00464     res1.clear();
00465     distance(&s1, pose1, &s2_rss, pose2, &solver2, request, res1);
00466     BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
00467 
00468     res1.clear();
00469     distance(&s1_rss, pose1, &s2, pose2, &solver2, request, res1);
00470     BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
00471   }
00472 
00473   pose.setTranslation(Vec3f(40.1, 0, 0));
00474 
00475   res.clear(); res1.clear();
00476   distance(&s1, Transform3f(), &s2, pose, &solver2, request, res);
00477   distance(&s1_rss, Transform3f(), &s2_rss, pose, &solver2, request, res1);
00478   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
00479 
00480   res1.clear();
00481   distance(&s1, Transform3f(), &s2_rss, pose, &solver2, request, res1);
00482   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
00483 
00484   res1.clear();
00485   distance(&s1_rss, Transform3f(), &s2, pose, &solver2, request, res1);
00486   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 4);
00487 
00488 
00489   for(std::size_t i = 0; i < 10; ++i)
00490   {
00491     Transform3f t;
00492     generateRandomTransform(extents, t);
00493     
00494     Transform3f pose1(t);
00495     Transform3f pose2 = t * pose;
00496 
00497     res.clear(); res1.clear();
00498     distance(&s1, pose1, &s2, pose2, &solver2, request, res);
00499     distance(&s1_rss, pose1, &s2_rss, pose2, &solver2, request, res1);
00500     BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
00501 
00502     res1.clear();
00503     distance(&s1, pose1, &s2_rss, pose2, &solver2, request, res1);
00504     BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
00505 
00506     res1.clear();
00507     distance(&s1_rss, pose1, &s2, pose2, &solver2, request, res1);
00508     BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 4);
00509   }
00510 }
00511 
00512 BOOST_AUTO_TEST_CASE(consistency_distance_boxbox_GJK)
00513 {
00514   Box s1(20, 40, 50);
00515   Box s2(10, 10, 10);
00516 
00517   BVHModel<RSS> s1_rss;
00518   BVHModel<RSS> s2_rss;
00519 
00520   generateBVHModel(s1_rss, s1, Transform3f());
00521   generateBVHModel(s2_rss, s2, Transform3f());
00522 
00523   DistanceRequest request;
00524   DistanceResult res, res1;
00525 
00526   Transform3f pose;
00527 
00528   pose.setTranslation(Vec3f(50, 0, 0));
00529 
00530   res.clear(); res1.clear();
00531   distance(&s1, Transform3f(), &s2, pose, &solver2, request, res);
00532   distance(&s1_rss, Transform3f(), &s2_rss, pose, &solver2, request, res1);
00533   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
00534   
00535   res1.clear();
00536   distance(&s1, Transform3f(), &s2_rss, pose, &solver2, request, res1);
00537   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
00538 
00539   res1.clear();
00540   distance(&s1_rss, Transform3f(), &s2, pose, &solver2, request, res1);
00541   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
00542 
00543   for(std::size_t i = 0; i < 10; ++i)
00544   {
00545     Transform3f t;
00546     generateRandomTransform(extents, t);
00547     
00548     Transform3f pose1(t);
00549     Transform3f pose2 = t * pose;
00550 
00551     res.clear(); res1.clear();
00552     distance(&s1, pose1, &s2, pose2, &solver2, request, res);
00553     distance(&s1_rss, pose1, &s2_rss, pose2, &solver2, request, res1);
00554     BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
00555 
00556     res1.clear();
00557     distance(&s1, pose1, &s2_rss, pose2, &solver2, request, res1);
00558     BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
00559 
00560     res1.clear();
00561     distance(&s1_rss, pose1, &s2, pose2, &solver2, request, res1);
00562     BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
00563   }
00564 
00565   pose.setTranslation(Vec3f(15.1, 0, 0));
00566   
00567   res.clear(); res1.clear();
00568   distance(&s1, Transform3f(), &s2, pose, &solver2, request, res);
00569   distance(&s1_rss, Transform3f(), &s2_rss, pose, &solver2, request, res1);
00570   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
00571   
00572   res1.clear();
00573   distance(&s1, Transform3f(), &s2_rss, pose, &solver2, request, res1);
00574   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
00575 
00576   res1.clear();
00577   distance(&s1_rss, Transform3f(), &s2, pose, &solver2, request, res1);
00578   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
00579 
00580   for(std::size_t i = 0; i < 10; ++i)
00581   {
00582     Transform3f t;
00583     generateRandomTransform(extents, t);
00584     
00585     Transform3f pose1(t);
00586     Transform3f pose2 = t * pose;
00587 
00588     res.clear(); res1.clear();
00589     distance(&s1, pose1, &s2, pose2, &solver2, request, res);
00590     distance(&s1_rss, pose1, &s2_rss, pose2, &solver2, request, res1);
00591     BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
00592 
00593     res1.clear();
00594     distance(&s1, pose1, &s2_rss, pose2, &solver2, request, res1);
00595     BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
00596 
00597     res1.clear();
00598     distance(&s1_rss, pose1, &s2, pose2, &solver2, request, res1);
00599     BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
00600   }
00601 }
00602 
00603 BOOST_AUTO_TEST_CASE(consistency_distance_cylindercylinder_GJK)
00604 {
00605   Cylinder s1(5, 10);
00606   Cylinder s2(5, 10);
00607 
00608   BVHModel<RSS> s1_rss;
00609   BVHModel<RSS> s2_rss;
00610 
00611   generateBVHModel(s1_rss, s1, Transform3f(), 16, 16);
00612   generateBVHModel(s2_rss, s2, Transform3f(), 16, 16);
00613   
00614   DistanceRequest request;
00615   DistanceResult res, res1;
00616 
00617   Transform3f pose;
00618 
00619   pose.setTranslation(Vec3f(20, 0, 0));
00620   
00621   res.clear(); res1.clear();
00622   distance(&s1, Transform3f(), &s2, pose, &solver2, request, res);
00623   distance(&s1_rss, Transform3f(), &s2_rss, pose, &solver2, request, res1);
00624   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
00625   
00626   res1.clear();
00627   distance(&s1, Transform3f(), &s2_rss, pose, &solver2, request, res1);
00628   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
00629   
00630   res1.clear();
00631   distance(&s1_rss, Transform3f(), &s2, pose, &solver2, request, res1);
00632   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
00633   
00634   for(std::size_t i = 0; i < 10; ++i)
00635   {
00636     Transform3f t;
00637     generateRandomTransform(extents, t);
00638     
00639     Transform3f pose1(t);
00640     Transform3f pose2 = t * pose;
00641 
00642     res.clear(); res1.clear();
00643     distance(&s1, pose1, &s2, pose2, &solver2, request, res);
00644     distance(&s1_rss, pose1, &s2_rss, pose2, &solver2, request, res1);
00645     if(fabs(res1.min_distance - res.min_distance) / res.min_distance > 0.05)
00646       std::cout << "low resolution: " << res1.min_distance << " " << res.min_distance << std::endl;
00647     else
00648       BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
00649 
00650     res1.clear();
00651     distance(&s1, pose1, &s2_rss, pose2, &solver2, request, res1);
00652     if(fabs(res1.min_distance - res.min_distance) / res.min_distance > 0.05)
00653       std::cout << "low resolution: " << res1.min_distance << " " << res.min_distance << std::endl;
00654     else
00655       BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
00656 
00657     res1.clear();
00658     distance(&s1_rss, pose1, &s2, pose2, &solver2, request, res1);
00659     if(fabs(res1.min_distance - res.min_distance) / res.min_distance > 0.05)
00660       std::cout << "low resolution: " << res1.min_distance << " " << res.min_distance << std::endl;
00661     else
00662       BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
00663   }
00664   
00665   pose.setTranslation(Vec3f(10.1, 0, 0));
00666   
00667   res.clear(); res1.clear();
00668   distance(&s1, Transform3f(), &s2, pose, &solver2, request, res);
00669   distance(&s1_rss, Transform3f(), &s2_rss, pose, &solver2, request, res1);
00670   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
00671   
00672   res1.clear();
00673   distance(&s1, Transform3f(), &s2_rss, pose, &solver2, request, res1);
00674   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
00675 
00676   res1.clear();
00677   distance(&s1_rss, Transform3f(), &s2, pose, &solver2, request, res1);
00678   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
00679   
00680   for(std::size_t i = 0; i < 10; ++i)
00681   {
00682     Transform3f t;
00683     generateRandomTransform(extents, t);
00684     
00685     Transform3f pose1(t);
00686     Transform3f pose2 = t * pose;
00687 
00688     res.clear(); res1.clear();
00689     distance(&s1, pose1, &s2, pose2, &solver2, request, res);
00690     distance(&s1_rss, pose1, &s2_rss, pose2, &solver2, request, res1);
00691     BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
00692 
00693     res1.clear();
00694     distance(&s1, pose1, &s2_rss, pose2, &solver2, request, res1);
00695     BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
00696 
00697     res1.clear();
00698     distance(&s1_rss, pose1, &s2, pose2, &solver2, request, res1);
00699     BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
00700   }
00701 }
00702 
00703 BOOST_AUTO_TEST_CASE(consistency_distance_conecone_GJK)
00704 {
00705   Cone s1(5, 10);
00706   Cone s2(5, 10);
00707 
00708   BVHModel<RSS> s1_rss;
00709   BVHModel<RSS> s2_rss;
00710 
00711   generateBVHModel(s1_rss, s1, Transform3f(), 16, 16);
00712   generateBVHModel(s2_rss, s2, Transform3f(), 16, 16);
00713 
00714   DistanceRequest request;
00715   DistanceResult res, res1;
00716 
00717   Transform3f pose;
00718 
00719   pose.setTranslation(Vec3f(20, 0, 0));
00720 
00721   res.clear(); res1.clear();
00722   distance(&s1, Transform3f(), &s2, pose, &solver2, request, res);
00723   distance(&s1_rss, Transform3f(), &s2_rss, pose, &solver2, request, res1);
00724   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
00725   
00726   res1.clear();
00727   distance(&s1, Transform3f(), &s2_rss, pose, &solver2, request, res1);
00728   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
00729   
00730   res1.clear();
00731   distance(&s1_rss, Transform3f(), &s2, pose, &solver2, request, res1);
00732   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
00733   
00734   for(std::size_t i = 0; i < 10; ++i)
00735   {
00736     Transform3f t;
00737     generateRandomTransform(extents, t);
00738     
00739     Transform3f pose1(t);
00740     Transform3f pose2 = t * pose;
00741 
00742     res.clear(); res1.clear();
00743     distance(&s1, pose1, &s2, pose2, &solver2, request, res);
00744     distance(&s1_rss, pose1, &s2_rss, pose2, &solver2, request, res1);
00745     BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
00746 
00747     res1.clear();
00748     distance(&s1, pose1, &s2_rss, pose2, &solver2, request, res1);
00749     BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
00750 
00751     res1.clear();
00752     distance(&s1_rss, pose1, &s2, pose2, &solver2, request, res1);
00753     BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
00754   }
00755   
00756   pose.setTranslation(Vec3f(10.1, 0, 0));
00757   
00758   res.clear(); res1.clear();
00759   distance(&s1, Transform3f(), &s2, pose, &solver2, request, res);
00760   distance(&s1_rss, Transform3f(), &s2_rss, pose, &solver2, request, res1);
00761   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
00762   
00763   res1.clear();
00764   distance(&s1, Transform3f(), &s2_rss, pose, &solver2, request, res1);
00765   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
00766 
00767   res1.clear();
00768   distance(&s1_rss, Transform3f(), &s2, pose, &solver2, request, res1);
00769   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
00770   
00771   for(std::size_t i = 0; i < 10; ++i)
00772   {
00773     Transform3f t;
00774     generateRandomTransform(extents, t);
00775     
00776     Transform3f pose1(t);
00777     Transform3f pose2 = t * pose;
00778 
00779     res.clear(); res1.clear();
00780     distance(&s1, pose1, &s2, pose2, &solver2, request, res);
00781     distance(&s1_rss, pose1, &s2_rss, pose2, &solver2, request, res1);
00782     BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
00783 
00784     res1.clear();
00785     distance(&s1, pose1, &s2_rss, pose2, &solver2, request, res1);
00786     BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
00787 
00788     res1.clear();
00789     distance(&s1_rss, pose1, &s2, pose2, &solver2, request, res1);
00790     BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
00791   }
00792 }
00793 
00794 
00795 
00796 BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_libccd)
00797 {
00798   Sphere s1(20);
00799   Sphere s2(10);
00800   BVHModel<AABB> s1_aabb;
00801   BVHModel<AABB> s2_aabb;
00802   BVHModel<OBB> s1_obb;
00803   BVHModel<OBB> s2_obb;
00804 
00805   generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16);
00806   generateBVHModel(s2_aabb, s2, Transform3f(), 16, 16);
00807   generateBVHModel(s1_obb, s1, Transform3f(), 16, 16);
00808   generateBVHModel(s2_obb, s2, Transform3f(), 16, 16);
00809 
00810   CollisionRequest request;
00811   CollisionResult result;
00812 
00813   bool res;
00814 
00815   Transform3f pose, pose_aabb, pose_obb;
00816 
00817 
00818   // s2 is within s1
00819   // both are shapes --> collision
00820   // s1 is shape, s2 is mesh --> in collision
00821   // s1 is mesh, s2 is shape --> collision free
00822   // all are reasonable
00823   result.clear();
00824   res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
00825   BOOST_CHECK(res);
00826   result.clear();
00827   res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
00828   BOOST_CHECK(res);
00829   result.clear();
00830   res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
00831   BOOST_CHECK(res);
00832   result.clear();
00833   res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
00834   BOOST_CHECK(res);
00835   result.clear();
00836   res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
00837   BOOST_CHECK(res);
00838   result.clear();
00839   res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
00840   BOOST_CHECK_FALSE(res);
00841   result.clear();
00842   res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
00843   BOOST_CHECK_FALSE(res);
00844   result.clear();
00845   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
00846   BOOST_CHECK_FALSE(res);
00847   result.clear();
00848   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
00849   BOOST_CHECK_FALSE(res);
00850 
00851   pose.setTranslation(Vec3f(40, 0, 0));
00852   pose_aabb.setTranslation(Vec3f(40, 0, 0));
00853   pose_obb.setTranslation(Vec3f(40, 0, 0));
00854 
00855   result.clear();
00856   res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
00857   BOOST_CHECK_FALSE(res);
00858   result.clear();
00859   res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
00860   BOOST_CHECK_FALSE(res);
00861   result.clear();
00862   res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
00863   BOOST_CHECK_FALSE(res);
00864   result.clear();
00865   res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
00866   BOOST_CHECK_FALSE(res);
00867   result.clear();
00868   res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
00869   BOOST_CHECK_FALSE(res);
00870   result.clear();
00871   res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
00872   BOOST_CHECK_FALSE(res);
00873   result.clear();
00874   res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
00875   BOOST_CHECK_FALSE(res);
00876   result.clear();
00877   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
00878   BOOST_CHECK_FALSE(res);
00879   result.clear();
00880   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
00881   BOOST_CHECK_FALSE(res);
00882   
00883   pose.setTranslation(Vec3f(30, 0, 0));
00884   pose_aabb.setTranslation(Vec3f(30, 0, 0));
00885   pose_obb.setTranslation(Vec3f(30, 0, 0));
00886 
00887   result.clear();
00888   res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
00889   BOOST_CHECK(res);
00890   result.clear();
00891   res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
00892   BOOST_CHECK_FALSE(res);
00893   result.clear();
00894   res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
00895   BOOST_CHECK(res);
00896   result.clear();
00897   res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
00898   BOOST_CHECK_FALSE(res);
00899   result.clear();
00900   res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
00901   BOOST_CHECK(res);
00902   result.clear();
00903   res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
00904   BOOST_CHECK_FALSE(res);
00905   result.clear();
00906   res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
00907   BOOST_CHECK(res);
00908   result.clear();
00909   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
00910   BOOST_CHECK_FALSE(res);
00911   result.clear();
00912   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
00913   BOOST_CHECK_FALSE(res);
00914   
00915   pose.setTranslation(Vec3f(29.9, 0, 0));
00916   pose_aabb.setTranslation(Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
00917   pose_obb.setTranslation(Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
00918 
00919   result.clear();
00920   res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
00921   BOOST_CHECK(res);
00922   result.clear();
00923   res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
00924   BOOST_CHECK(res);
00925   result.clear();
00926   res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
00927   BOOST_CHECK(res);
00928   result.clear();
00929   res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
00930   BOOST_CHECK(res);
00931   result.clear();
00932   res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
00933   BOOST_CHECK(res);
00934   result.clear();
00935   res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
00936   BOOST_CHECK(res);
00937   result.clear();
00938   res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
00939   BOOST_CHECK(res);
00940   result.clear();
00941   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
00942   BOOST_CHECK(res);
00943   result.clear();
00944   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
00945   BOOST_CHECK(res);
00946 
00947 
00948   pose.setTranslation(Vec3f(-29.9, 0, 0));
00949   pose_aabb.setTranslation(Vec3f(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
00950   pose_obb.setTranslation(Vec3f(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
00951 
00952   result.clear();
00953   res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
00954   BOOST_CHECK(res);
00955   result.clear();
00956   res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
00957   BOOST_CHECK(res);
00958   result.clear();
00959   res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
00960   BOOST_CHECK(res);
00961   result.clear();
00962   res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
00963   BOOST_CHECK(res);
00964   result.clear();
00965   res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
00966   BOOST_CHECK(res);
00967   result.clear();
00968   res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
00969   BOOST_CHECK(res);
00970   result.clear();
00971   res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
00972   BOOST_CHECK(res);
00973   result.clear();
00974   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
00975   BOOST_CHECK(res);
00976   result.clear();
00977   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
00978   BOOST_CHECK(res);
00979 
00980   pose.setTranslation(Vec3f(-30, 0, 0));
00981   pose_aabb.setTranslation(Vec3f(-30, 0, 0));
00982   pose_obb.setTranslation(Vec3f(-30, 0, 0));
00983 
00984   result.clear();
00985   res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
00986   BOOST_CHECK(res);
00987   result.clear();
00988   res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
00989   BOOST_CHECK_FALSE(res);
00990   result.clear();
00991   res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
00992   BOOST_CHECK(res);
00993   result.clear();
00994   res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
00995   BOOST_CHECK_FALSE(res);
00996   result.clear();
00997   res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
00998   BOOST_CHECK(res);
00999   result.clear();
01000   res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
01001   BOOST_CHECK_FALSE(res);
01002   result.clear();
01003   res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
01004   BOOST_CHECK(res);
01005   result.clear();
01006   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
01007   BOOST_CHECK_FALSE(res);
01008   result.clear();
01009   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
01010   BOOST_CHECK_FALSE(res);
01011 }
01012 
01013 BOOST_AUTO_TEST_CASE(consistency_collision_boxbox_libccd)
01014 {
01015   Box s1(20, 40, 50);
01016   Box s2(10, 10, 10);
01017 
01018   BVHModel<AABB> s1_aabb;
01019   BVHModel<AABB> s2_aabb;
01020   BVHModel<OBB> s1_obb;
01021   BVHModel<OBB> s2_obb;
01022 
01023   generateBVHModel(s1_aabb, s1, Transform3f());
01024   generateBVHModel(s2_aabb, s2, Transform3f());
01025   generateBVHModel(s1_obb, s1, Transform3f());
01026   generateBVHModel(s2_obb, s2, Transform3f());
01027 
01028   CollisionRequest request;
01029   CollisionResult result;
01030 
01031   bool res;
01032 
01033   Transform3f pose, pose_aabb, pose_obb;
01034 
01035   // s2 is within s1
01036   // both are shapes --> collision
01037   // s1 is shape, s2 is mesh --> in collision
01038   // s1 is mesh, s2 is shape --> collision free
01039   // all are reasonable
01040   result.clear();
01041   res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
01042   BOOST_CHECK(res);
01043   result.clear();
01044   res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
01045   BOOST_CHECK(res);
01046   result.clear();
01047   res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
01048   BOOST_CHECK(res);
01049   result.clear();
01050   res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
01051   BOOST_CHECK(res);
01052   result.clear();
01053   res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
01054   BOOST_CHECK(res);
01055   result.clear();
01056   res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
01057   BOOST_CHECK_FALSE(res);
01058   result.clear();
01059   res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
01060   BOOST_CHECK_FALSE(res);
01061   result.clear();
01062   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
01063   BOOST_CHECK_FALSE(res);
01064   result.clear();
01065   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
01066   BOOST_CHECK_FALSE(res);
01067 
01068   pose.setTranslation(Vec3f(15.01, 0, 0));
01069   pose_aabb.setTranslation(Vec3f(15.01, 0, 0));
01070   pose_obb.setTranslation(Vec3f(15.01, 0, 0));
01071 
01072   result.clear();
01073   res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
01074   BOOST_CHECK_FALSE(res);
01075   result.clear();
01076   res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
01077   BOOST_CHECK_FALSE(res);
01078   result.clear();
01079   res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
01080   BOOST_CHECK_FALSE(res);
01081   result.clear();
01082   res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
01083   BOOST_CHECK_FALSE(res);
01084   result.clear();
01085   res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
01086   BOOST_CHECK_FALSE(res);
01087   result.clear();
01088   res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
01089   BOOST_CHECK_FALSE(res);
01090   result.clear();
01091   res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
01092   BOOST_CHECK_FALSE(res);
01093   result.clear();
01094   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
01095   BOOST_CHECK_FALSE(res);
01096   result.clear();
01097   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
01098   BOOST_CHECK_FALSE(res);
01099 
01100   pose.setTranslation(Vec3f(14.99, 0, 0));
01101   pose_aabb.setTranslation(Vec3f(14.99, 0, 0));
01102   pose_obb.setTranslation(Vec3f(14.99, 0, 0));
01103 
01104   result.clear();
01105   res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
01106   BOOST_CHECK(res);
01107   result.clear();
01108   res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
01109   BOOST_CHECK(res);
01110   result.clear();
01111   res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
01112   BOOST_CHECK(res);
01113   result.clear();
01114   res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
01115   BOOST_CHECK(res);
01116   result.clear();
01117   res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
01118   BOOST_CHECK(res);
01119   result.clear();
01120   res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
01121   BOOST_CHECK(res);
01122   result.clear();
01123   res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
01124   BOOST_CHECK(res);
01125   result.clear();
01126   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
01127   BOOST_CHECK(res);
01128   result.clear();
01129   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
01130   BOOST_CHECK(res);
01131 }
01132 
01133 BOOST_AUTO_TEST_CASE(consistency_collision_spherebox_libccd)
01134 {
01135   Sphere s1(20);
01136   Box s2(5, 5, 5);
01137 
01138   BVHModel<AABB> s1_aabb;
01139   BVHModel<AABB> s2_aabb;
01140   BVHModel<OBB> s1_obb;
01141   BVHModel<OBB> s2_obb;
01142 
01143   generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16);
01144   generateBVHModel(s2_aabb, s2, Transform3f());
01145   generateBVHModel(s1_obb, s1, Transform3f(), 16, 16);
01146   generateBVHModel(s2_obb, s2, Transform3f());
01147 
01148   CollisionRequest request;
01149   CollisionResult result;
01150 
01151   bool res;
01152 
01153   Transform3f pose, pose_aabb, pose_obb;
01154 
01155   // s2 is within s1
01156   // both are shapes --> collision
01157   // s1 is shape, s2 is mesh --> in collision
01158   // s1 is mesh, s2 is shape --> collision free
01159   // all are reasonable
01160   result.clear();
01161   res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
01162   BOOST_CHECK(res);
01163   result.clear();
01164   res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
01165   BOOST_CHECK(res);
01166   result.clear();
01167   res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
01168   BOOST_CHECK(res);
01169   result.clear();
01170   res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
01171   BOOST_CHECK(res);
01172   result.clear();
01173   res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
01174   BOOST_CHECK(res);
01175   result.clear();
01176   res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
01177   BOOST_CHECK_FALSE(res);
01178   result.clear();
01179   res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
01180   BOOST_CHECK_FALSE(res);
01181   result.clear();
01182   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
01183   BOOST_CHECK_FALSE(res);
01184   result.clear();
01185   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
01186   BOOST_CHECK_FALSE(res);
01187 
01188   pose.setTranslation(Vec3f(22.4, 0, 0));
01189   pose_aabb.setTranslation(Vec3f(22.4, 0, 0));
01190   pose_obb.setTranslation(Vec3f(22.4, 0, 0));
01191 
01192   result.clear();
01193   res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
01194   BOOST_CHECK(res);
01195   result.clear();
01196   res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
01197   BOOST_CHECK(res);
01198   result.clear();
01199   res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
01200   BOOST_CHECK(res);
01201   result.clear();
01202   res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
01203   BOOST_CHECK(res);
01204   result.clear();
01205   res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
01206   BOOST_CHECK(res);
01207   result.clear();
01208   res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
01209   BOOST_CHECK(res);
01210   result.clear();
01211   res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
01212   BOOST_CHECK(res);
01213   result.clear();
01214   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
01215   BOOST_CHECK(res);
01216   result.clear();
01217   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
01218   BOOST_CHECK(res);
01219 
01220   pose.setTranslation(Vec3f(22.51, 0, 0));
01221   pose_aabb.setTranslation(Vec3f(22.51, 0, 0));
01222   pose_obb.setTranslation(Vec3f(22.51, 0, 0));
01223 
01224   result.clear();
01225   res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
01226   BOOST_CHECK_FALSE(res);
01227   result.clear();
01228   res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
01229   BOOST_CHECK_FALSE(res);
01230   result.clear();
01231   res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
01232   BOOST_CHECK_FALSE(res);
01233   result.clear();
01234   res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
01235   BOOST_CHECK_FALSE(res);
01236   result.clear();
01237   res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
01238   BOOST_CHECK_FALSE(res);
01239   result.clear();
01240   res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
01241   BOOST_CHECK_FALSE(res);
01242   result.clear();
01243   res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
01244   BOOST_CHECK_FALSE(res);
01245   result.clear();
01246   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
01247   BOOST_CHECK_FALSE(res);
01248   result.clear();
01249   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
01250   BOOST_CHECK_FALSE(res);
01251 }
01252 
01253 BOOST_AUTO_TEST_CASE(consistency_collision_cylindercylinder_libccd)
01254 {
01255   Cylinder s1(5, 10);
01256   Cylinder s2(5, 10);
01257 
01258   BVHModel<AABB> s1_aabb;
01259   BVHModel<AABB> s2_aabb;
01260   BVHModel<OBB> s1_obb;
01261   BVHModel<OBB> s2_obb;
01262 
01263   generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16);
01264   generateBVHModel(s2_aabb, s2, Transform3f(), 16, 16);
01265   generateBVHModel(s1_obb, s1, Transform3f(), 16, 16);
01266   generateBVHModel(s2_obb, s2, Transform3f(), 16, 16);
01267 
01268   CollisionRequest request;
01269   CollisionResult result;
01270 
01271   bool res;
01272 
01273   Transform3f pose, pose_aabb, pose_obb;
01274 
01275   pose.setTranslation(Vec3f(9.99, 0, 0));
01276   pose_aabb.setTranslation(Vec3f(9.99, 0, 0));
01277   pose_obb.setTranslation(Vec3f(9.99, 0, 0));
01278 
01279   result.clear();
01280   res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
01281   BOOST_CHECK(res);
01282   result.clear();
01283   res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
01284   BOOST_CHECK(res);
01285   result.clear();
01286   res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
01287   BOOST_CHECK(res);
01288   result.clear();
01289   res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
01290   BOOST_CHECK(res);
01291   result.clear();
01292   res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
01293   BOOST_CHECK(res);
01294   result.clear();
01295   res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
01296   BOOST_CHECK(res);
01297   result.clear();
01298   res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
01299   BOOST_CHECK(res);
01300   result.clear();
01301   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
01302   BOOST_CHECK(res);
01303   result.clear();
01304   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
01305   BOOST_CHECK(res);
01306 
01307   pose.setTranslation(Vec3f(10.01, 0, 0));
01308   pose_aabb.setTranslation(Vec3f(10.01, 0, 0));
01309   pose_obb.setTranslation(Vec3f(10.01, 0, 0));
01310 
01311   result.clear();
01312   res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
01313   BOOST_CHECK_FALSE(res);
01314   result.clear();
01315   res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
01316   BOOST_CHECK_FALSE(res);
01317   result.clear();
01318   res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
01319   BOOST_CHECK_FALSE(res);
01320   result.clear();
01321   res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
01322   BOOST_CHECK_FALSE(res);
01323   result.clear();
01324   res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
01325   BOOST_CHECK_FALSE(res);
01326   result.clear();
01327   res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
01328   BOOST_CHECK_FALSE(res);
01329   result.clear();
01330   res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
01331   BOOST_CHECK_FALSE(res);
01332   result.clear();
01333   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
01334   BOOST_CHECK_FALSE(res);
01335   result.clear();
01336   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
01337   BOOST_CHECK_FALSE(res);
01338 }
01339 
01340 BOOST_AUTO_TEST_CASE(consistency_collision_conecone_libccd)
01341 {
01342   Cone s1(5, 10);
01343   Cone s2(5, 10);
01344 
01345   BVHModel<AABB> s1_aabb;
01346   BVHModel<AABB> s2_aabb;
01347   BVHModel<OBB> s1_obb;
01348   BVHModel<OBB> s2_obb;
01349 
01350   generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16);
01351   generateBVHModel(s2_aabb, s2, Transform3f(), 16, 16);
01352   generateBVHModel(s1_obb, s1, Transform3f(), 16, 16);
01353   generateBVHModel(s2_obb, s2, Transform3f(), 16, 16);
01354 
01355   CollisionRequest request;
01356   CollisionResult result;
01357 
01358   bool res;
01359 
01360   Transform3f pose, pose_aabb, pose_obb;
01361 
01362   pose.setTranslation(Vec3f(9.9, 0, 0));
01363   pose_aabb.setTranslation(Vec3f(9.9, 0, 0));
01364   pose_obb.setTranslation(Vec3f(9.9, 0, 0));
01365 
01366   result.clear();
01367   res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
01368   BOOST_CHECK(res);
01369   result.clear();
01370   res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
01371   BOOST_CHECK(res);
01372   result.clear();
01373   res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
01374   BOOST_CHECK(res);
01375   result.clear();
01376   res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
01377   BOOST_CHECK(res);
01378   result.clear();
01379   res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
01380   BOOST_CHECK(res);
01381   result.clear();
01382   res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
01383   BOOST_CHECK(res);
01384   result.clear();
01385   res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
01386   BOOST_CHECK(res);
01387   result.clear();
01388   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
01389   BOOST_CHECK(res);
01390   result.clear();
01391   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
01392   BOOST_CHECK(res);
01393 
01394   pose.setTranslation(Vec3f(10.1, 0, 0));
01395   pose_aabb.setTranslation(Vec3f(10.1, 0, 0));
01396   pose_obb.setTranslation(Vec3f(10.1, 0, 0));
01397 
01398   result.clear();
01399   res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
01400   BOOST_CHECK_FALSE(res);
01401   result.clear();
01402   res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
01403   BOOST_CHECK_FALSE(res);
01404   result.clear();
01405   res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
01406   BOOST_CHECK_FALSE(res);
01407   result.clear();
01408   res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
01409   BOOST_CHECK_FALSE(res);
01410   result.clear();
01411   res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
01412   BOOST_CHECK_FALSE(res);
01413   result.clear();
01414   res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
01415   BOOST_CHECK_FALSE(res);
01416   result.clear();
01417   res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
01418   BOOST_CHECK_FALSE(res);
01419   result.clear();
01420   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
01421   BOOST_CHECK_FALSE(res);
01422   result.clear();
01423   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
01424   BOOST_CHECK_FALSE(res);
01425 
01426   pose.setTranslation(Vec3f(0, 0, 9.9));
01427   pose_aabb.setTranslation(Vec3f(0, 0, 9.9));
01428   pose_obb.setTranslation(Vec3f(0, 0, 9.9));
01429 
01430   result.clear();
01431   res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
01432   BOOST_CHECK(res);
01433   result.clear();
01434   res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
01435   BOOST_CHECK(res);
01436   result.clear();
01437   res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
01438   BOOST_CHECK(res);
01439   result.clear();
01440   res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
01441   BOOST_CHECK(res);
01442   result.clear();
01443   res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
01444   BOOST_CHECK(res);
01445   result.clear();
01446   res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
01447   BOOST_CHECK(res);
01448   result.clear();
01449   res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
01450   BOOST_CHECK(res);
01451   result.clear();
01452   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
01453   BOOST_CHECK(res);
01454   result.clear();
01455   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
01456   BOOST_CHECK(res);
01457 
01458   pose.setTranslation(Vec3f(0, 0, 10.1));
01459   pose_aabb.setTranslation(Vec3f(0, 0, 10.1));
01460   pose_obb.setTranslation(Vec3f(0, 0, 10.1));
01461 
01462   result.clear();
01463   res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
01464   BOOST_CHECK_FALSE(res);
01465   result.clear();
01466   res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
01467   BOOST_CHECK_FALSE(res);
01468   result.clear();
01469   res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
01470   BOOST_CHECK_FALSE(res);
01471   result.clear();
01472   res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
01473   BOOST_CHECK_FALSE(res);
01474   result.clear();
01475   res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
01476   BOOST_CHECK_FALSE(res);
01477   result.clear();
01478   res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
01479   BOOST_CHECK_FALSE(res);
01480   result.clear();
01481   res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
01482   BOOST_CHECK_FALSE(res);
01483   result.clear();
01484   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
01485   BOOST_CHECK_FALSE(res);
01486   result.clear();
01487   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
01488   BOOST_CHECK_FALSE(res);
01489 }
01490 
01491 
01492 
01493 
01494 BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_GJK)
01495 {
01496   Sphere s1(20);
01497   Sphere s2(10);
01498   BVHModel<AABB> s1_aabb;
01499   BVHModel<AABB> s2_aabb;
01500   BVHModel<OBB> s1_obb;
01501   BVHModel<OBB> s2_obb;
01502 
01503   generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16);
01504   generateBVHModel(s2_aabb, s2, Transform3f(), 16, 16);
01505   generateBVHModel(s1_obb, s1, Transform3f(), 16, 16);
01506   generateBVHModel(s2_obb, s2, Transform3f(), 16, 16);
01507 
01508   CollisionRequest request;
01509   CollisionResult result;
01510 
01511   bool res;
01512 
01513   Transform3f pose, pose_aabb, pose_obb;
01514 
01515 
01516   // s2 is within s1
01517   // both are shapes --> collision
01518   // s1 is shape, s2 is mesh --> in collision
01519   // s1 is mesh, s2 is shape --> collision free
01520   // all are reasonable
01521   result.clear();
01522   res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
01523   BOOST_CHECK(res);
01524   result.clear();
01525   res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
01526   BOOST_CHECK(res);
01527   result.clear();
01528   res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
01529   BOOST_CHECK(res);
01530   result.clear();
01531   res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
01532   BOOST_CHECK(res);
01533   result.clear();
01534   res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
01535   BOOST_CHECK(res);
01536   result.clear();
01537   res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
01538   BOOST_CHECK_FALSE(res);
01539   result.clear();
01540   res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
01541   BOOST_CHECK_FALSE(res);
01542   result.clear();
01543   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
01544   BOOST_CHECK_FALSE(res);
01545   result.clear();
01546   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
01547   BOOST_CHECK_FALSE(res);
01548 
01549   pose.setTranslation(Vec3f(40, 0, 0));
01550   pose_aabb.setTranslation(Vec3f(40, 0, 0));
01551   pose_obb.setTranslation(Vec3f(40, 0, 0));
01552 
01553   result.clear();
01554   res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
01555   BOOST_CHECK_FALSE(res);
01556   result.clear();
01557   res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
01558   BOOST_CHECK_FALSE(res);
01559   result.clear();
01560   res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
01561   BOOST_CHECK_FALSE(res);
01562   result.clear();
01563   res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
01564   BOOST_CHECK_FALSE(res);
01565   result.clear();
01566   res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
01567   BOOST_CHECK_FALSE(res);
01568   result.clear();
01569   res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
01570   BOOST_CHECK_FALSE(res);
01571   result.clear();
01572   res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
01573   BOOST_CHECK_FALSE(res);
01574   result.clear();
01575   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
01576   BOOST_CHECK_FALSE(res);
01577   result.clear();
01578   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
01579   BOOST_CHECK_FALSE(res);
01580   
01581   pose.setTranslation(Vec3f(30, 0, 0));
01582   pose_aabb.setTranslation(Vec3f(30, 0, 0));
01583   pose_obb.setTranslation(Vec3f(30, 0, 0));
01584 
01585   result.clear();
01586   res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
01587   BOOST_CHECK(res);
01588   result.clear();
01589   res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
01590   BOOST_CHECK_FALSE(res);
01591   result.clear();
01592   res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
01593   BOOST_CHECK(res);
01594   result.clear();
01595   res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
01596   BOOST_CHECK_FALSE(res);
01597   result.clear();
01598   res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
01599   BOOST_CHECK(res);
01600   result.clear();
01601   res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
01602   BOOST_CHECK_FALSE(res);
01603   result.clear();
01604   res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
01605   BOOST_CHECK(res);
01606   result.clear();
01607   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
01608   BOOST_CHECK_FALSE(res);
01609   result.clear();
01610   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
01611   BOOST_CHECK_FALSE(res);
01612   
01613   pose.setTranslation(Vec3f(29.9, 0, 0));
01614   pose_aabb.setTranslation(Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
01615   pose_obb.setTranslation(Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
01616 
01617   result.clear();
01618   res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
01619   BOOST_CHECK(res);
01620   result.clear();
01621   res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
01622   BOOST_CHECK(res);
01623   result.clear();
01624   res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
01625   BOOST_CHECK(res);
01626   result.clear();
01627   res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
01628   BOOST_CHECK(res);
01629   result.clear();
01630   res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
01631   BOOST_CHECK(res);
01632   result.clear();
01633   res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
01634   BOOST_CHECK(res);
01635   result.clear();
01636   res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
01637   BOOST_CHECK(res);
01638   result.clear();
01639   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
01640   BOOST_CHECK(res);
01641   result.clear();
01642   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
01643   BOOST_CHECK(res);
01644 
01645 
01646   pose.setTranslation(Vec3f(-29.9, 0, 0));
01647   pose_aabb.setTranslation(Vec3f(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
01648   pose_obb.setTranslation(Vec3f(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
01649 
01650   result.clear();
01651   res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
01652   BOOST_CHECK(res);
01653   result.clear();
01654   res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
01655   BOOST_CHECK(res);
01656   result.clear();
01657   res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
01658   BOOST_CHECK(res);
01659   result.clear();
01660   res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
01661   BOOST_CHECK(res);
01662   result.clear();
01663   res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
01664   BOOST_CHECK(res);
01665   result.clear();
01666   res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
01667   BOOST_CHECK(res);
01668   result.clear();
01669   res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
01670   BOOST_CHECK(res);
01671   result.clear();
01672   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
01673   BOOST_CHECK(res);
01674   result.clear();
01675   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
01676   BOOST_CHECK(res);
01677 
01678   pose.setTranslation(Vec3f(-30, 0, 0));
01679   pose_aabb.setTranslation(Vec3f(-30, 0, 0));
01680   pose_obb.setTranslation(Vec3f(-30, 0, 0));
01681 
01682   result.clear();
01683   res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
01684   BOOST_CHECK(res);
01685   result.clear();
01686   res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
01687   BOOST_CHECK_FALSE(res);
01688   result.clear();
01689   res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
01690   BOOST_CHECK(res);
01691   result.clear();
01692   res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
01693   BOOST_CHECK_FALSE(res);
01694   result.clear();
01695   res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
01696   BOOST_CHECK(res);
01697   result.clear();
01698   res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
01699   BOOST_CHECK_FALSE(res);
01700   result.clear();
01701   res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
01702   BOOST_CHECK(res);
01703   result.clear();
01704   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
01705   BOOST_CHECK_FALSE(res);
01706   result.clear();
01707   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
01708   BOOST_CHECK_FALSE(res);
01709 }
01710 
01711 BOOST_AUTO_TEST_CASE(consistency_collision_boxbox_GJK)
01712 {
01713   Box s1(20, 40, 50);
01714   Box s2(10, 10, 10);
01715 
01716   BVHModel<AABB> s1_aabb;
01717   BVHModel<AABB> s2_aabb;
01718   BVHModel<OBB> s1_obb;
01719   BVHModel<OBB> s2_obb;
01720 
01721   generateBVHModel(s1_aabb, s1, Transform3f());
01722   generateBVHModel(s2_aabb, s2, Transform3f());
01723   generateBVHModel(s1_obb, s1, Transform3f());
01724   generateBVHModel(s2_obb, s2, Transform3f());
01725 
01726   CollisionRequest request;
01727   CollisionResult result;
01728 
01729   bool res;
01730 
01731   Transform3f pose, pose_aabb, pose_obb;
01732 
01733   // s2 is within s1
01734   // both are shapes --> collision
01735   // s1 is shape, s2 is mesh --> in collision
01736   // s1 is mesh, s2 is shape --> collision free
01737   // all are reasonable
01738   result.clear();
01739   res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
01740   BOOST_CHECK(res);
01741   result.clear();
01742   res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
01743   BOOST_CHECK(res);
01744   result.clear();
01745   res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
01746   BOOST_CHECK(res);
01747   result.clear();
01748   res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
01749   BOOST_CHECK(res);
01750   result.clear();
01751   res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
01752   BOOST_CHECK(res);
01753   result.clear();
01754   res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
01755   BOOST_CHECK_FALSE(res);
01756   result.clear();
01757   res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
01758   BOOST_CHECK_FALSE(res);
01759   result.clear();
01760   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
01761   BOOST_CHECK_FALSE(res);
01762   result.clear();
01763   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
01764   BOOST_CHECK_FALSE(res);
01765 
01766   pose.setTranslation(Vec3f(15.01, 0, 0));
01767   pose_aabb.setTranslation(Vec3f(15.01, 0, 0));
01768   pose_obb.setTranslation(Vec3f(15.01, 0, 0));
01769 
01770   result.clear();
01771   res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
01772   BOOST_CHECK_FALSE(res);
01773   result.clear();
01774   res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
01775   BOOST_CHECK_FALSE(res);
01776   result.clear();
01777   res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
01778   BOOST_CHECK_FALSE(res);
01779   result.clear();
01780   res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
01781   BOOST_CHECK_FALSE(res);
01782   result.clear();
01783   res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
01784   BOOST_CHECK_FALSE(res);
01785   result.clear();
01786   res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
01787   BOOST_CHECK_FALSE(res);
01788   result.clear();
01789   res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
01790   BOOST_CHECK_FALSE(res);
01791   result.clear();
01792   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
01793   BOOST_CHECK_FALSE(res);
01794   result.clear();
01795   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
01796   BOOST_CHECK_FALSE(res);
01797 
01798   pose.setTranslation(Vec3f(14.99, 0, 0));
01799   pose_aabb.setTranslation(Vec3f(14.99, 0, 0));
01800   pose_obb.setTranslation(Vec3f(14.99, 0, 0));
01801 
01802   result.clear();
01803   res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
01804   BOOST_CHECK(res);
01805   result.clear();
01806   res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
01807   BOOST_CHECK(res);
01808   result.clear();
01809   res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
01810   BOOST_CHECK(res);
01811   result.clear();
01812   res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
01813   BOOST_CHECK(res);
01814   result.clear();
01815   res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
01816   BOOST_CHECK(res);
01817   result.clear();
01818   res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
01819   BOOST_CHECK(res);
01820   result.clear();
01821   res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
01822   BOOST_CHECK(res);
01823   result.clear();
01824   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
01825   BOOST_CHECK(res);
01826   result.clear();
01827   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
01828   BOOST_CHECK(res);
01829 }
01830 
01831 BOOST_AUTO_TEST_CASE(consistency_collision_spherebox_GJK)
01832 {
01833   Sphere s1(20);
01834   Box s2(5, 5, 5);
01835 
01836   BVHModel<AABB> s1_aabb;
01837   BVHModel<AABB> s2_aabb;
01838   BVHModel<OBB> s1_obb;
01839   BVHModel<OBB> s2_obb;
01840 
01841   generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16);
01842   generateBVHModel(s2_aabb, s2, Transform3f());
01843   generateBVHModel(s1_obb, s1, Transform3f(), 16, 16);
01844   generateBVHModel(s2_obb, s2, Transform3f());
01845 
01846   CollisionRequest request;
01847   CollisionResult result;
01848 
01849   bool res;
01850 
01851   Transform3f pose, pose_aabb, pose_obb;
01852 
01853   // s2 is within s1
01854   // both are shapes --> collision
01855   // s1 is shape, s2 is mesh --> in collision
01856   // s1 is mesh, s2 is shape --> collision free
01857   // all are reasonable
01858   result.clear();
01859   res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
01860   BOOST_CHECK(res);
01861   result.clear();
01862   res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
01863   BOOST_CHECK(res);
01864   result.clear();
01865   res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
01866   BOOST_CHECK(res);
01867   result.clear();
01868   res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
01869   BOOST_CHECK(res);
01870   result.clear();
01871   res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
01872   BOOST_CHECK(res);
01873   result.clear();
01874   res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
01875   BOOST_CHECK_FALSE(res);
01876   result.clear();
01877   res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
01878   BOOST_CHECK_FALSE(res);
01879   result.clear();
01880   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
01881   BOOST_CHECK_FALSE(res);
01882   result.clear();
01883   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
01884   BOOST_CHECK_FALSE(res);
01885 
01886   pose.setTranslation(Vec3f(22.4, 0, 0));
01887   pose_aabb.setTranslation(Vec3f(22.4, 0, 0));
01888   pose_obb.setTranslation(Vec3f(22.4, 0, 0));
01889 
01890   result.clear();
01891   res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
01892   BOOST_CHECK(res);
01893   result.clear();
01894   res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
01895   BOOST_CHECK(res);
01896   result.clear();
01897   res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
01898   BOOST_CHECK(res);
01899   result.clear();
01900   res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
01901   BOOST_CHECK(res);
01902   result.clear();
01903   res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
01904   BOOST_CHECK(res);
01905   result.clear();
01906   res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
01907   BOOST_CHECK(res);
01908   result.clear();
01909   res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
01910   BOOST_CHECK(res);
01911   result.clear();
01912   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
01913   BOOST_CHECK(res);
01914   result.clear();
01915   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
01916   BOOST_CHECK(res);
01917 
01918   pose.setTranslation(Vec3f(22.51, 0, 0));
01919   pose_aabb.setTranslation(Vec3f(22.51, 0, 0));
01920   pose_obb.setTranslation(Vec3f(22.51, 0, 0));
01921 
01922   result.clear();
01923   res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
01924   BOOST_CHECK_FALSE(res);
01925   result.clear();
01926   res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
01927   BOOST_CHECK_FALSE(res);
01928   result.clear();
01929   res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
01930   BOOST_CHECK_FALSE(res);
01931   result.clear();
01932   res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
01933   BOOST_CHECK_FALSE(res);
01934   result.clear();
01935   res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
01936   BOOST_CHECK_FALSE(res);
01937   result.clear();
01938   res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
01939   BOOST_CHECK_FALSE(res);
01940   result.clear();
01941   res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
01942   BOOST_CHECK_FALSE(res);
01943   result.clear();
01944   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
01945   BOOST_CHECK_FALSE(res);
01946   result.clear();
01947   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
01948   BOOST_CHECK_FALSE(res);
01949 }
01950 
01951 BOOST_AUTO_TEST_CASE(consistency_collision_cylindercylinder_GJK)
01952 {
01953   Cylinder s1(5, 10);
01954   Cylinder s2(5, 10);
01955 
01956   BVHModel<AABB> s1_aabb;
01957   BVHModel<AABB> s2_aabb;
01958   BVHModel<OBB> s1_obb;
01959   BVHModel<OBB> s2_obb;
01960 
01961   generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16);
01962   generateBVHModel(s2_aabb, s2, Transform3f(), 16, 16);
01963   generateBVHModel(s1_obb, s1, Transform3f(), 16, 16);
01964   generateBVHModel(s2_obb, s2, Transform3f(), 16, 16);
01965 
01966   CollisionRequest request;
01967   CollisionResult result;
01968 
01969   bool res;
01970 
01971   Transform3f pose, pose_aabb, pose_obb;
01972 
01973   pose.setTranslation(Vec3f(9.99, 0, 0));
01974   pose_aabb.setTranslation(Vec3f(9.99, 0, 0));
01975   pose_obb.setTranslation(Vec3f(9.99, 0, 0));
01976 
01977   result.clear();
01978   res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
01979   BOOST_CHECK(res);
01980   result.clear();
01981   res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
01982   BOOST_CHECK(res);
01983   result.clear();
01984   res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
01985   BOOST_CHECK(res);
01986   result.clear();
01987   res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
01988   BOOST_CHECK(res);
01989   result.clear();
01990   res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
01991   BOOST_CHECK(res);
01992   result.clear();
01993   res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
01994   BOOST_CHECK(res);
01995   result.clear();
01996   res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
01997   BOOST_CHECK(res);
01998   result.clear();
01999   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
02000   BOOST_CHECK(res);
02001   result.clear();
02002   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
02003   BOOST_CHECK(res);
02004 
02005   pose.setTranslation(Vec3f(10.01, 0, 0));
02006   pose_aabb.setTranslation(Vec3f(10.01, 0, 0));
02007   pose_obb.setTranslation(Vec3f(10.01, 0, 0));
02008 
02009   result.clear();
02010   res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
02011   BOOST_CHECK_FALSE(res);
02012   result.clear();
02013   res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
02014   BOOST_CHECK_FALSE(res);
02015   result.clear();
02016   res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
02017   BOOST_CHECK_FALSE(res);
02018   result.clear();
02019   res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
02020   BOOST_CHECK_FALSE(res);
02021   result.clear();
02022   res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
02023   BOOST_CHECK_FALSE(res);
02024   result.clear();
02025   res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
02026   BOOST_CHECK_FALSE(res);
02027   result.clear();
02028   res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
02029   BOOST_CHECK_FALSE(res);
02030   result.clear();
02031   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
02032   BOOST_CHECK_FALSE(res);
02033   result.clear();
02034   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
02035   BOOST_CHECK_FALSE(res);
02036 }
02037 
02038 BOOST_AUTO_TEST_CASE(consistency_collision_conecone_GJK)
02039 {
02040   Cone s1(5, 10);
02041   Cone s2(5, 10);
02042 
02043   BVHModel<AABB> s1_aabb;
02044   BVHModel<AABB> s2_aabb;
02045   BVHModel<OBB> s1_obb;
02046   BVHModel<OBB> s2_obb;
02047 
02048   generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16);
02049   generateBVHModel(s2_aabb, s2, Transform3f(), 16, 16);
02050   generateBVHModel(s1_obb, s1, Transform3f(), 16, 16);
02051   generateBVHModel(s2_obb, s2, Transform3f(), 16, 16);
02052 
02053   CollisionRequest request;
02054   CollisionResult result;
02055 
02056   bool res;
02057 
02058   Transform3f pose, pose_aabb, pose_obb;
02059 
02060   pose.setTranslation(Vec3f(9.9, 0, 0));
02061   pose_aabb.setTranslation(Vec3f(9.9, 0, 0));
02062   pose_obb.setTranslation(Vec3f(9.9, 0, 0));
02063 
02064   result.clear();
02065   res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
02066   BOOST_CHECK(res);
02067   result.clear();
02068   res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
02069   BOOST_CHECK(res);
02070   result.clear();
02071   res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
02072   BOOST_CHECK(res);
02073   result.clear();
02074   res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
02075   BOOST_CHECK(res);
02076   result.clear();
02077   res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
02078   BOOST_CHECK(res);
02079   result.clear();
02080   res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
02081   BOOST_CHECK(res);
02082   result.clear();
02083   res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
02084   BOOST_CHECK(res);
02085   result.clear();
02086   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
02087   BOOST_CHECK(res);
02088   result.clear();
02089   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
02090   BOOST_CHECK(res);
02091 
02092   pose.setTranslation(Vec3f(10.1, 0, 0));
02093   pose_aabb.setTranslation(Vec3f(10.1, 0, 0));
02094   pose_obb.setTranslation(Vec3f(10.1, 0, 0));
02095 
02096   result.clear();
02097   res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
02098   BOOST_CHECK_FALSE(res);
02099   result.clear();
02100   res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
02101   BOOST_CHECK_FALSE(res);
02102   result.clear();
02103   res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
02104   BOOST_CHECK_FALSE(res);
02105   result.clear();
02106   res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
02107   BOOST_CHECK_FALSE(res);
02108   result.clear();
02109   res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
02110   BOOST_CHECK_FALSE(res);
02111   result.clear();
02112   res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
02113   BOOST_CHECK_FALSE(res);
02114   result.clear();
02115   res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
02116   BOOST_CHECK_FALSE(res);
02117   result.clear();
02118   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
02119   BOOST_CHECK_FALSE(res);
02120   result.clear();
02121   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
02122   BOOST_CHECK_FALSE(res);
02123 
02124   pose.setTranslation(Vec3f(0, 0, 9.9));
02125   pose_aabb.setTranslation(Vec3f(0, 0, 9.9));
02126   pose_obb.setTranslation(Vec3f(0, 0, 9.9));
02127 
02128   result.clear();
02129   res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
02130   BOOST_CHECK(res);
02131   result.clear();
02132   res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
02133   BOOST_CHECK(res);
02134   result.clear();
02135   res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
02136   BOOST_CHECK(res);
02137   result.clear();
02138   res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
02139   BOOST_CHECK(res);
02140   result.clear();
02141   res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
02142   BOOST_CHECK(res);
02143   result.clear();
02144   res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
02145   BOOST_CHECK(res);
02146   result.clear();
02147   res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
02148   BOOST_CHECK(res);
02149   result.clear();
02150   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
02151   BOOST_CHECK(res);
02152   result.clear();
02153   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
02154   BOOST_CHECK(res);
02155 
02156   pose.setTranslation(Vec3f(0, 0, 10.1));
02157   pose_aabb.setTranslation(Vec3f(0, 0, 10.1));
02158   pose_obb.setTranslation(Vec3f(0, 0, 10.1));
02159 
02160   result.clear();
02161   res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
02162   BOOST_CHECK_FALSE(res);
02163   result.clear();
02164   res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
02165   BOOST_CHECK_FALSE(res);
02166   result.clear();
02167   res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
02168   BOOST_CHECK_FALSE(res);
02169   result.clear();
02170   res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
02171   BOOST_CHECK_FALSE(res);
02172   result.clear();
02173   res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
02174   BOOST_CHECK_FALSE(res);
02175   result.clear();
02176   res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
02177   BOOST_CHECK_FALSE(res);
02178   result.clear();
02179   res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
02180   BOOST_CHECK_FALSE(res);
02181   result.clear();
02182   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
02183   BOOST_CHECK_FALSE(res);
02184   result.clear();
02185   res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
02186   BOOST_CHECK_FALSE(res);
02187 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines

fcl
Author(s): Jia Pan
autogenerated on Tue Jan 15 2013 16:05:31