test_fcl_geometric_shapes.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 
00038 #define BOOST_TEST_MODULE "FCL_GEOMETRIC_SHAPES"
00039 #include <boost/test/unit_test.hpp>
00040 
00041 #include "fcl/narrowphase/narrowphase.h"
00042 #include "fcl/collision.h"
00043 #include "test_fcl_utility.h"
00044 #include <iostream>
00045 
00046 using namespace fcl;
00047 
00048 FCL_REAL extents [6] = {0, 0, 0, 10, 10, 10};
00049 
00050 GJKSolver_libccd solver1;
00051 GJKSolver_indep solver2;
00052 
00053 #define BOOST_CHECK_FALSE(p) BOOST_CHECK(!(p))
00054 
00055 BOOST_AUTO_TEST_CASE(shapeIntersection_spheresphere)
00056 {
00057   Sphere s1(20);
00058   Sphere s2(10);
00059 
00060   Transform3f transform;
00061   generateRandomTransform(extents, transform);
00062   Transform3f identity;
00063 
00064   CollisionRequest request;
00065   CollisionResult result;
00066   bool res;
00067 
00068   res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), NULL, NULL, NULL);
00069   BOOST_CHECK_FALSE(res);
00070   result.clear();
00071   res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(40, 0, 0)), &solver1, request, result) > 0);
00072   BOOST_CHECK_FALSE(res);
00073 
00074   res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), NULL, NULL, NULL);
00075   BOOST_CHECK_FALSE(res);
00076   result.clear();
00077   res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(40, 0, 0)), &solver1, request, result) > 0);
00078   BOOST_CHECK_FALSE(res);
00079 
00080   res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(30, 0, 0)), NULL, NULL, NULL);
00081   BOOST_CHECK(res);
00082   result.clear();
00083   res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(30, 0, 0)), &solver1, request, result) > 0);
00084   BOOST_CHECK(res);
00085 
00086   res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(30.01, 0, 0)), NULL, NULL, NULL);
00087   BOOST_CHECK_FALSE(res);
00088   result.clear();
00089   res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(30.01, 0, 0)), &solver1, request, result) > 0);
00090   BOOST_CHECK_FALSE(res);
00091 
00092   res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(29.9, 0, 0)), NULL, NULL, NULL);
00093   BOOST_CHECK(res);
00094   result.clear();
00095   res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(29.9, 0, 0)), &solver1, request, result) > 0);
00096   BOOST_CHECK(res);
00097 
00098   res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(29.9, 0, 0)), NULL, NULL, NULL);
00099   BOOST_CHECK(res);
00100   result.clear();
00101   res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(29.9, 0, 0)), &solver1, request, result) > 0);
00102   BOOST_CHECK(res);
00103 
00104   res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
00105   BOOST_CHECK(res);
00106   result.clear();
00107   res = (collide(&s1, Transform3f(), &s2, Transform3f(), &solver1, request, result) > 0);
00108   BOOST_CHECK(res);
00109 
00110   res = solver1.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
00111   BOOST_CHECK(res);
00112   result.clear();
00113   res = (collide(&s1, transform, &s2, transform, &solver1, request, result) > 0);
00114   BOOST_CHECK(res);
00115 
00116   res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(-29.9, 0, 0)), NULL, NULL, NULL);
00117   BOOST_CHECK(res);
00118   result.clear();
00119   res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(-29.9, 0, 0)), &solver1, request, result) > 0);
00120   BOOST_CHECK(res);
00121 
00122   res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(-29.9, 0, 0)), NULL, NULL, NULL);
00123   BOOST_CHECK(res);
00124   result.clear();
00125   res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(-29.9, 0, 0)), &solver1, request, result) > 0);
00126   BOOST_CHECK(res);
00127 
00128   res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(-30, 0, 0)), NULL, NULL, NULL);
00129   BOOST_CHECK(res);
00130   result.clear();
00131   res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(-30, 0, 0)), &solver1, request, result) > 0);
00132   BOOST_CHECK(res);
00133 
00134   res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(-30.01, 0, 0)), NULL, NULL, NULL);
00135   BOOST_CHECK_FALSE(res);
00136   result.clear();
00137   res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(-30.01, 0, 0)), &solver1, request, result) > 0);
00138   BOOST_CHECK_FALSE(res);
00139 }
00140 
00141 BOOST_AUTO_TEST_CASE(shapeIntersection_boxbox)
00142 {
00143   Box s1(20, 40, 50);
00144   Box s2(10, 10, 10);
00145 
00146   Transform3f transform;
00147   generateRandomTransform(extents, transform);
00148   Transform3f identity;
00149 
00150   CollisionRequest request;
00151   CollisionResult result;
00152 
00153   bool res;
00154 
00155   res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
00156   BOOST_CHECK(res);
00157   result.clear();
00158   res = (collide(&s1, Transform3f(), &s2, Transform3f(), &solver1, request, result) > 0);
00159   BOOST_CHECK(res);
00160 
00161   res = solver1.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
00162   BOOST_CHECK(res);
00163   result.clear();
00164   res = (collide(&s1, transform, &s2, transform, &solver1, request, result) > 0);
00165   BOOST_CHECK(res);
00166 
00167   res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(15, 0, 0)), NULL, NULL, NULL);
00168   BOOST_CHECK(res);
00169   result.clear();
00170   res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(15, 0, 0)), &solver1, request, result) > 0);
00171   BOOST_CHECK(res);
00172 
00173   res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(15.01, 0, 0)), NULL, NULL, NULL);
00174   BOOST_CHECK_FALSE(res);
00175   result.clear();
00176   res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(15.01, 0, 0)), &solver1, request, result) > 0);
00177   BOOST_CHECK_FALSE(res);
00178 
00179   Quaternion3f q;
00180   q.fromAxisAngle(Vec3f(0, 0, 1), (FCL_REAL)3.140 / 6);
00181   res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(q), NULL, NULL, NULL);
00182   BOOST_CHECK(res);
00183   result.clear();
00184   res = (collide(&s1, Transform3f(), &s2, Transform3f(q), &solver1, request, result) > 0);
00185   BOOST_CHECK(res);
00186 
00187   res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(q), NULL, NULL, NULL);
00188   BOOST_CHECK(res);
00189   result.clear();
00190   res = (collide(&s1, transform, &s2, transform * Transform3f(q), &solver1, request, result) > 0);
00191   BOOST_CHECK(res);
00192 }
00193 
00194 BOOST_AUTO_TEST_CASE(shapeIntersection_spherebox)
00195 {
00196   Sphere s1(20);
00197   Box s2(5, 5, 5);
00198 
00199   Transform3f transform;
00200   generateRandomTransform(extents, transform);
00201   Transform3f identity;
00202 
00203   CollisionRequest request;
00204   CollisionResult result;
00205 
00206   bool res;
00207 
00208   res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
00209   BOOST_CHECK(res);
00210   result.clear();
00211   res = (collide(&s1, Transform3f(), &s2, Transform3f(), &solver1, request, result) > 0);
00212   BOOST_CHECK(res);
00213 
00214   res = solver1.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
00215   BOOST_CHECK(res);
00216   result.clear();
00217   res = (collide(&s1, transform, &s2, transform, &solver1, request, result) > 0);
00218   BOOST_CHECK(res);
00219 
00220 
00221   res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(22.5, 0, 0)), NULL, NULL, NULL);
00222   BOOST_CHECK_FALSE(res);
00223   result.clear();
00224   res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(22.5, 0, 0)), &solver1, request, result) > 0);
00225   BOOST_CHECK_FALSE(res);
00226 
00227   res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(22.501, 0, 0)), NULL, NULL, NULL);
00228   BOOST_CHECK_FALSE(res);
00229   result.clear();
00230   res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(22.501, 0, 0)), &solver1, request, result) > 0);
00231   BOOST_CHECK_FALSE(res);
00232 
00233   res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(22.4, 0, 0)), NULL, NULL, NULL);
00234   BOOST_CHECK(res);
00235   result.clear();
00236   res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(22.4, 0, 0)), &solver1, request, result) > 0);
00237   BOOST_CHECK(res);
00238 
00239   res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(22.4, 0, 0)), NULL, NULL, NULL);
00240   BOOST_CHECK(res);
00241   result.clear();
00242   res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(22.4, 0, 0)), &solver1, request, result) > 0);
00243   BOOST_CHECK(res);
00244 }
00245 
00246 BOOST_AUTO_TEST_CASE(shapeIntersection_cylindercylinder)
00247 {
00248   Cylinder s1(5, 10);
00249   Cylinder s2(5, 10);
00250 
00251   Transform3f transform;
00252   generateRandomTransform(extents, transform);
00253   Transform3f identity;
00254 
00255   CollisionRequest request;
00256   CollisionResult result;
00257 
00258   bool res;
00259 
00260   res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
00261   BOOST_CHECK(res);
00262   result.clear();
00263   res = (collide(&s1, Transform3f(), &s2, Transform3f(), &solver1, request, result) > 0);
00264   BOOST_CHECK(res);
00265 
00266   res = solver1.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
00267   BOOST_CHECK(res);
00268   result.clear();
00269   res = (collide(&s1, transform, &s2, transform, &solver1, request, result) > 0);
00270   BOOST_CHECK(res);
00271 
00272   res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
00273   BOOST_CHECK(res);
00274   result.clear();
00275   res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(9.9, 0, 0)), &solver1, request, result) > 0);
00276   BOOST_CHECK(res);
00277 
00278   res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
00279   BOOST_CHECK(res);
00280   result.clear();
00281   res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(9.9, 0, 0)), &solver1, request, result) > 0);
00282   BOOST_CHECK(res);
00283 
00284   res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10, 0, 0)), NULL, NULL, NULL);
00285   BOOST_CHECK_FALSE(res);
00286   result.clear();
00287   res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(10, 0, 0)), &solver1, request, result) > 0);
00288   BOOST_CHECK_FALSE(res);
00289 
00290   res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10.01, 0, 0)), NULL, NULL, NULL);
00291   BOOST_CHECK_FALSE(res);
00292   result.clear();
00293   res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(10.01, 0, 0)), &solver1, request, result) > 0);
00294   BOOST_CHECK_FALSE(res);
00295 }
00296 
00297 BOOST_AUTO_TEST_CASE(shapeIntersection_conecone)
00298 {
00299   Cone s1(5, 10);
00300   Cone s2(5, 10);
00301 
00302   Transform3f transform;
00303   generateRandomTransform(extents, transform);
00304   Transform3f identity;
00305 
00306   CollisionRequest request;
00307   CollisionResult result;
00308 
00309   bool res;
00310 
00311   res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
00312   BOOST_CHECK(res);
00313   result.clear();
00314   res = (collide(&s1, Transform3f(), &s2, Transform3f(), &solver1, request, result) > 0);
00315   BOOST_CHECK(res);
00316 
00317   res = solver1.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
00318   BOOST_CHECK(res);
00319   result.clear();
00320   res = (collide(&s1, transform, &s2, transform, &solver1, request, result) > 0);
00321   BOOST_CHECK(res);
00322 
00323   res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
00324   BOOST_CHECK(res);
00325   result.clear();
00326   res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(9.9, 0, 0)), &solver1, request, result) > 0);
00327   BOOST_CHECK(res);
00328 
00329   res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
00330   BOOST_CHECK(res);
00331   result.clear();
00332   res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(9.9, 0, 0)), &solver1, request, result) > 0);
00333   BOOST_CHECK(res);
00334 
00335   res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10.001, 0, 0)), NULL, NULL, NULL);
00336   BOOST_CHECK_FALSE(res);
00337   result.clear();
00338   res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(10.001, 0, 0)), &solver1, request, result) > 0);
00339   BOOST_CHECK_FALSE(res);
00340 
00341   res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10.001, 0, 0)), NULL, NULL, NULL);
00342   BOOST_CHECK_FALSE(res);
00343   result.clear();
00344   res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(10.001, 0, 0)), &solver1, request, result) > 0);
00345   BOOST_CHECK_FALSE(res);
00346 
00347   res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 9.9)), NULL, NULL, NULL);
00348   BOOST_CHECK(res);
00349   result.clear();
00350   res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(0, 0, 9.9)), &solver1, request, result) > 0);
00351   BOOST_CHECK(res);
00352 
00353   res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 9.9)), NULL, NULL, NULL);
00354   BOOST_CHECK(res);
00355   result.clear();
00356   res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(0, 0, 9.9)), &solver1, request, result) > 0);
00357   BOOST_CHECK(res);
00358 }
00359 
00360 BOOST_AUTO_TEST_CASE(shapeIntersection_conecylinder)
00361 {
00362   Cylinder s1(5, 10);
00363   Cone s2(5, 10);
00364 
00365   Transform3f transform;
00366   generateRandomTransform(extents, transform);
00367   Transform3f identity;
00368 
00369   CollisionRequest request;
00370   CollisionResult result;
00371 
00372   bool res;
00373 
00374   res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
00375   BOOST_CHECK(res);
00376   result.clear();
00377   res = (collide(&s1, Transform3f(), &s2, Transform3f(), &solver1, request, result) > 0);
00378   BOOST_CHECK(res);
00379 
00380   res = solver1.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
00381   BOOST_CHECK(res);
00382   result.clear();
00383   res = (collide(&s1, transform, &s2, transform, &solver1, request, result) > 0);
00384   BOOST_CHECK(res);
00385 
00386   res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
00387   BOOST_CHECK(res);
00388   result.clear();
00389   res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(9.9, 0, 0)), &solver1, request, result) > 0);
00390   BOOST_CHECK(res);
00391 
00392   res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
00393   BOOST_CHECK(res);
00394   result.clear();
00395   res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(9.9, 0, 0)), &solver1, request, result) > 0);
00396   BOOST_CHECK(res);
00397 
00398   res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10.01, 0, 0)), NULL, NULL, NULL);
00399   BOOST_CHECK_FALSE(res);
00400   result.clear();
00401   res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(10, 0, 0)), &solver1, request, result) > 0);
00402   BOOST_CHECK_FALSE(res);
00403 
00404   res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10.01, 0, 0)), NULL, NULL, NULL);
00405   BOOST_CHECK_FALSE(res);
00406   result.clear();
00407   res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(10.01, 0, 0)), &solver1, request, result) > 0);
00408   BOOST_CHECK_FALSE(res);
00409 
00410   res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 9.9)), NULL, NULL, NULL);
00411   BOOST_CHECK(res);
00412   result.clear();
00413   res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(0, 0, 9.9)), &solver1, request, result) > 0);
00414   BOOST_CHECK(res);
00415 
00416   res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 9.9)), NULL, NULL, NULL);
00417   BOOST_CHECK(res);
00418   result.clear();
00419   res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(0, 0, 9.9)), &solver1, request, result) > 0);
00420   BOOST_CHECK(res);
00421 
00422   res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 10.01)), NULL, NULL, NULL);
00423   BOOST_CHECK_FALSE(res);
00424   result.clear();
00425   res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(0, 0, 10)), &solver1, request, result) > 0);
00426   BOOST_CHECK_FALSE(res);
00427 
00428   res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 10.01)), NULL, NULL, NULL);
00429   BOOST_CHECK_FALSE(res);
00430   result.clear();
00431   res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(0, 0, 10.01)), &solver1, request, result) > 0);
00432   BOOST_CHECK_FALSE(res);
00433 }
00434 
00435 BOOST_AUTO_TEST_CASE(shapeIntersection_spheretriangle)
00436 {
00437   Sphere s(10);
00438   Vec3f t[3];
00439   t[0].setValue(20, 0, 0);
00440   t[1].setValue(-20, 0, 0);
00441   t[2].setValue(0, 20, 0);
00442 
00443   Transform3f transform;
00444   generateRandomTransform(extents, transform);
00445   Transform3f identity;
00446 
00447   bool res;
00448 
00449   res = solver1.shapeTriangleIntersect(s, Transform3f(), t[0], t[1], t[2], NULL, NULL, NULL);
00450   BOOST_CHECK(res);
00451 
00452   res =  solver1.shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL);
00453   BOOST_CHECK(res);
00454 
00455 
00456   t[0].setValue(30, 0, 0);
00457   t[1].setValue(9.9, -20, 0);
00458   t[2].setValue(9.9, 20, 0);
00459   res = solver1.shapeTriangleIntersect(s, Transform3f(), t[0], t[1], t[2], NULL, NULL, NULL);
00460   BOOST_CHECK(res);
00461 
00462   res =  solver1.shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL);
00463   BOOST_CHECK(res);
00464 }
00465 
00466 BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacesphere)
00467 {
00468   Sphere s(10);
00469   Halfspace hs(Vec3f(1, 0, 0), 0);
00470 
00471   Transform3f transform;
00472   generateRandomTransform(extents, transform);
00473 
00474   Vec3f contact;
00475   FCL_REAL depth;
00476   Vec3f normal;
00477   bool res;
00478 
00479   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
00480   BOOST_CHECK(res);
00481   BOOST_CHECK(std::abs(depth - 10) < 0.001);
00482   BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
00483   BOOST_CHECK(contact.equal(Vec3f(-5, 0, 0)));
00484 
00485   res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
00486   BOOST_CHECK(res);
00487   BOOST_CHECK(std::abs(depth - 10) < 0.001);
00488   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
00489   BOOST_CHECK(contact.equal(transform.transform(Vec3f(-5, 0, 0))));
00490 
00491   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(5, 0, 0)), &contact, &depth, &normal);
00492   BOOST_CHECK(res);
00493   BOOST_CHECK(std::abs(depth - 15) < 0.001);
00494   BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
00495   BOOST_CHECK(contact.equal(Vec3f(-2.5, 0, 0)));
00496 
00497   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(5, 0, 0)), &contact, &depth, &normal);
00498   BOOST_CHECK(res);
00499   BOOST_CHECK(std::abs(depth - 15) < 0.001);
00500   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
00501   BOOST_CHECK(contact.equal(transform.transform(Vec3f(-2.5, 0, 0))));
00502 
00503   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-5, 0, 0)), &contact, &depth, &normal);
00504   BOOST_CHECK(res);
00505   BOOST_CHECK(std::abs(depth - 5) < 0.001);
00506   BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
00507   BOOST_CHECK(contact.equal(Vec3f(-7.5, 0, 0)));
00508 
00509   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-5, 0, 0)), &contact, &depth, &normal);
00510   BOOST_CHECK(res);
00511   BOOST_CHECK(std::abs(depth - 5) < 0.001);
00512   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
00513   BOOST_CHECK(contact.equal(transform.transform(Vec3f(-7.5, 0, 0))));
00514 
00515   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-10.1, 0, 0)), &contact, &depth, &normal);
00516   BOOST_CHECK_FALSE(res);
00517 
00518   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-10.1, 0, 0)), &contact, &depth, &normal);
00519   BOOST_CHECK_FALSE(res);
00520 
00521   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(10.1, 0, 0)), &contact, &depth, &normal);
00522   BOOST_CHECK(res);
00523   BOOST_CHECK(std::abs(depth - 20.1) < 0.001);
00524   BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
00525   BOOST_CHECK(contact.equal(Vec3f(0.05, 0, 0)));
00526 
00527   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(10.1, 0, 0)), &contact, &depth, &normal);
00528   BOOST_CHECK(res);
00529   BOOST_CHECK(std::abs(depth - 20.1) < 0.001);
00530   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
00531   BOOST_CHECK(contact.equal(transform.transform(Vec3f(0.05, 0, 0))));
00532 }
00533 
00534 BOOST_AUTO_TEST_CASE(shapeIntersection_planesphere)
00535 {
00536   Sphere s(10);
00537   Plane hs(Vec3f(1, 0, 0), 0);
00538 
00539   Transform3f transform;
00540   generateRandomTransform(extents, transform);
00541 
00542   Vec3f contact;
00543   FCL_REAL depth;
00544   Vec3f normal;
00545   bool res;
00546 
00547   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
00548   BOOST_CHECK(res);
00549   BOOST_CHECK(std::abs(depth - 10) < 0.001);
00550   BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)) || normal.equal(Vec3f(1, 0, 0)));
00551   BOOST_CHECK(contact.equal(Vec3f(0, 0, 0)));
00552 
00553   res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
00554   BOOST_CHECK(res);
00555   BOOST_CHECK(std::abs(depth - 10) < 0.001);
00556   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))) || normal.equal(transform.getQuatRotation().transform(Vec3f(1, 0, 0))));
00557   BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0))));
00558 
00559   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(5, 0, 0)), &contact, &depth, &normal);
00560   BOOST_CHECK(res);
00561   BOOST_CHECK(std::abs(depth - 5) < 0.001);
00562   BOOST_CHECK(normal.equal(Vec3f(1, 0, 0)));
00563   BOOST_CHECK(contact.equal(Vec3f(5, 0, 0)));
00564 
00565   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(5, 0, 0)), &contact, &depth, &normal);
00566   BOOST_CHECK(res);
00567   BOOST_CHECK(std::abs(depth - 5) < 0.001);
00568   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(1, 0, 0))));
00569   BOOST_CHECK(contact.equal(transform.transform(Vec3f(5, 0, 0))));
00570 
00571   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-5, 0, 0)), &contact, &depth, &normal);
00572   BOOST_CHECK(res);
00573   BOOST_CHECK(std::abs(depth - 5) < 0.001);
00574   BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
00575   BOOST_CHECK(contact.equal(Vec3f(-5, 0, 0)));
00576 
00577   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-5, 0, 0)), &contact, &depth, &normal);
00578   BOOST_CHECK(res);
00579   BOOST_CHECK(std::abs(depth - 5) < 0.001);
00580   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
00581   BOOST_CHECK(contact.equal(transform.transform(Vec3f(-5, 0, 0))));
00582 
00583 
00584   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-10.1, 0, 0)), &contact, &depth, &normal);
00585   BOOST_CHECK_FALSE(res);
00586 
00587   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-10.1, 0, 0)), &contact, &depth, &normal);
00588   BOOST_CHECK_FALSE(res);
00589 
00590   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(10.1, 0, 0)), &contact, &depth, &normal);
00591   BOOST_CHECK_FALSE(res);
00592 
00593   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(10.1, 0, 0)), &contact, &depth, &normal);
00594   BOOST_CHECK_FALSE(res);
00595 }
00596 
00597 BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacebox)
00598 {
00599   Box s(5, 10, 20);
00600   Halfspace hs(Vec3f(1, 0, 0), 0);
00601   
00602   Transform3f transform;
00603   generateRandomTransform(extents, transform);
00604 
00605   Vec3f contact;
00606   FCL_REAL depth;
00607   Vec3f normal;
00608   bool res;
00609 
00610   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
00611   BOOST_CHECK(res);
00612   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
00613   BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
00614   BOOST_CHECK(contact.equal(Vec3f(-1.25, 0, 0)));
00615 
00616   res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
00617   BOOST_CHECK(res);
00618   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
00619   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
00620   BOOST_CHECK(contact.equal(transform.transform(Vec3f(-1.25, 0, 0))));
00621 
00622   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(1.25, 0, 0)), &contact, &depth, &normal);
00623   BOOST_CHECK(res);
00624   BOOST_CHECK(std::abs(depth - 3.75) < 0.001);
00625   BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
00626   BOOST_CHECK(contact.equal(Vec3f(-0.625, 0, 0)));
00627 
00628   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(1.25, 0, 0)), &contact, &depth, &normal);
00629   BOOST_CHECK(res);
00630   BOOST_CHECK(std::abs(depth - 3.75) < 0.001);
00631   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
00632   BOOST_CHECK(contact.equal(transform.transform(Vec3f(-0.625, 0, 0))));
00633 
00634   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-1.25, 0, 0)), &contact, &depth, &normal);
00635   BOOST_CHECK(res);
00636   BOOST_CHECK(std::abs(depth - 1.25) < 0.001);
00637   BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
00638   BOOST_CHECK(contact.equal(Vec3f(-1.875, 0, 0)));
00639 
00640   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-1.25, 0, 0)), &contact, &depth, &normal);
00641   BOOST_CHECK(res);
00642   BOOST_CHECK(std::abs(depth - 1.25) < 0.001);
00643   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
00644   BOOST_CHECK(contact.equal(transform.transform(Vec3f(-1.875, 0, 0))));
00645 
00646   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(2.51, 0, 0)), &contact, &depth, &normal);
00647   BOOST_CHECK(res);
00648   BOOST_CHECK(std::abs(depth - 5.01) < 0.001);
00649   BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
00650   BOOST_CHECK(contact.equal(Vec3f(0.005, 0, 0)));
00651 
00652   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(2.51, 0, 0)), &contact, &depth, &normal);
00653   BOOST_CHECK(res);
00654   BOOST_CHECK(std::abs(depth - 5.01) < 0.001);
00655   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
00656   BOOST_CHECK(contact.equal(transform.transform(Vec3f(0.005, 0, 0))));
00657 
00658   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-2.51, 0, 0)), &contact, &depth, &normal);
00659   BOOST_CHECK_FALSE(res);
00660 
00661   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-2.51, 0, 0)), &contact, &depth, &normal);
00662   BOOST_CHECK_FALSE(res);
00663 
00664   res = solver1.shapeIntersect(s, Transform3f(transform.getQuatRotation()), hs, Transform3f(), &contact, &depth, &normal);
00665   BOOST_CHECK(res);
00666 }
00667 
00668 BOOST_AUTO_TEST_CASE(shapeIntersection_planebox)
00669 {
00670   Box s(5, 10, 20);
00671   Plane hs(Vec3f(1, 0, 0), 0);
00672   
00673   Transform3f transform;
00674   generateRandomTransform(extents, transform);
00675 
00676   Vec3f contact;
00677   FCL_REAL depth;
00678   Vec3f normal;
00679   bool res;
00680 
00681   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
00682   BOOST_CHECK(res);
00683   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
00684   BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)) || normal.equal(Vec3f(1, 0, 0)));
00685   BOOST_CHECK(contact.equal(Vec3f(0, 0, 0)));
00686 
00687   res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
00688   BOOST_CHECK(res);
00689   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
00690   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))) || normal.equal(transform.getQuatRotation().transform(Vec3f(1, 0, 0))));
00691   BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0))));
00692 
00693   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(1.25, 0, 0)), &contact, &depth, &normal);
00694   BOOST_CHECK(res);
00695   BOOST_CHECK(std::abs(depth - 1.25) < 0.001);
00696   BOOST_CHECK(normal.equal(Vec3f(1, 0, 0)));
00697   BOOST_CHECK(contact.equal(Vec3f(1.25, 0, 0)));
00698 
00699   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(1.25, 0, 0)), &contact, &depth, &normal);
00700   BOOST_CHECK(res);
00701   BOOST_CHECK(std::abs(depth - 1.25) < 0.001);
00702   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(1, 0, 0))));
00703   BOOST_CHECK(contact.equal(transform.transform(Vec3f(1.25, 0, 0))));
00704 
00705   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-1.25, 0, 0)), &contact, &depth, &normal);
00706   BOOST_CHECK(res);
00707   BOOST_CHECK(std::abs(depth - 1.25) < 0.001);
00708   BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
00709   BOOST_CHECK(contact.equal(Vec3f(-1.25, 0, 0)));
00710 
00711   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-1.25, 0, 0)), &contact, &depth, &normal);
00712   BOOST_CHECK(res);
00713   BOOST_CHECK(std::abs(depth - 1.25) < 0.001);
00714   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
00715   BOOST_CHECK(contact.equal(transform.transform(Vec3f(-1.25, 0, 0))));
00716 
00717   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(2.51, 0, 0)), &contact, &depth, &normal);
00718   BOOST_CHECK_FALSE(res);
00719 
00720   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(2.51, 0, 0)), &contact, &depth, &normal);
00721   BOOST_CHECK_FALSE(res);
00722 
00723   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-2.51, 0, 0)), &contact, &depth, &normal);
00724   BOOST_CHECK_FALSE(res);
00725 
00726   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-2.51, 0, 0)), &contact, &depth, &normal);
00727   BOOST_CHECK_FALSE(res);
00728 
00729   res = solver1.shapeIntersect(s, Transform3f(transform.getQuatRotation()), hs, Transform3f(), &contact, &depth, &normal);
00730   BOOST_CHECK(res);
00731 }
00732 
00733 BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule)
00734 {
00735   Capsule s(5, 10);
00736   Halfspace hs(Vec3f(1, 0, 0), 0);
00737 
00738   Transform3f transform;
00739   generateRandomTransform(extents, transform);
00740 
00741   Vec3f contact;
00742   FCL_REAL depth;
00743   Vec3f normal;
00744   bool res;
00745 
00746   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
00747   BOOST_CHECK(res);
00748   BOOST_CHECK(std::abs(depth - 5) < 0.001);
00749   BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
00750   BOOST_CHECK(contact.equal(Vec3f(-2.5, 0, 0)));
00751 
00752   res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
00753   BOOST_CHECK(res);
00754   BOOST_CHECK(std::abs(depth - 5) < 0.001);
00755   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
00756   BOOST_CHECK(contact.equal(transform.transform(Vec3f(-2.5, 0, 0))));
00757 
00758   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal);
00759   BOOST_CHECK(res);
00760   BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
00761   BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
00762   BOOST_CHECK(contact.equal(Vec3f(-1.25, 0, 0)));
00763 
00764   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal);
00765   BOOST_CHECK(res);
00766   BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
00767   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
00768   BOOST_CHECK(contact.equal(transform.transform(Vec3f(-1.25, 0, 0))));
00769   
00770   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal);
00771   BOOST_CHECK(res);
00772   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
00773   BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
00774   BOOST_CHECK(contact.equal(Vec3f(-3.75, 0, 0)));
00775 
00776   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal);
00777   BOOST_CHECK(res);
00778   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
00779   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
00780   BOOST_CHECK(contact.equal(transform.transform(Vec3f(-3.75, 0, 0))));
00781 
00782   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal);
00783   BOOST_CHECK(res);
00784   BOOST_CHECK(std::abs(depth - 10.1) < 0.001);
00785   BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
00786   BOOST_CHECK(contact.equal(Vec3f(0.05, 0, 0)));
00787 
00788   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal);
00789   BOOST_CHECK(res);
00790   BOOST_CHECK(std::abs(depth - 10.1) < 0.001);
00791   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
00792   BOOST_CHECK(contact.equal(transform.transform(Vec3f(0.05, 0, 0))));
00793 
00794   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal);
00795   BOOST_CHECK_FALSE(res);
00796 
00797   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal);
00798   BOOST_CHECK_FALSE(res);
00799 
00800 
00801 
00802 
00803   hs = Halfspace(Vec3f(0, 1, 0), 0);
00804 
00805   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
00806   BOOST_CHECK(res);
00807   BOOST_CHECK(std::abs(depth - 5) < 0.001);
00808   BOOST_CHECK(normal.equal(Vec3f(0, -1, 0)));
00809   BOOST_CHECK(contact.equal(Vec3f(0, -2.5, 0)));
00810 
00811   res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
00812   BOOST_CHECK(res);
00813   BOOST_CHECK(std::abs(depth - 5) < 0.001);
00814   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
00815   BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, -2.5, 0))));
00816 
00817   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal);
00818   BOOST_CHECK(res);
00819   BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
00820   BOOST_CHECK(normal.equal(Vec3f(0, -1, 0)));
00821   BOOST_CHECK(contact.equal(Vec3f(0, -1.25, 0)));
00822 
00823   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal);
00824   BOOST_CHECK(res);
00825   BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
00826   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
00827   BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, -1.25, 0))));
00828 
00829   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal);
00830   BOOST_CHECK(res);
00831   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
00832   BOOST_CHECK(normal.equal(Vec3f(0, -1, 0)));
00833   BOOST_CHECK(contact.equal(Vec3f(0, -3.75, 0)));
00834 
00835   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal);
00836   BOOST_CHECK(res);
00837   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
00838   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
00839   BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, -3.75, 0))));
00840 
00841   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal);
00842   BOOST_CHECK(res);
00843   BOOST_CHECK(std::abs(depth - 10.1) < 0.001);
00844   BOOST_CHECK(normal.equal(Vec3f(0, -1, 0)));
00845   BOOST_CHECK(contact.equal(Vec3f(0, 0.05, 0)));
00846 
00847   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal);
00848   BOOST_CHECK(res);
00849   BOOST_CHECK(std::abs(depth - 10.1) < 0.001);
00850   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
00851   BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0.05, 0))));
00852 
00853   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal);
00854   BOOST_CHECK_FALSE(res);
00855 
00856   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal);
00857   BOOST_CHECK_FALSE(res);
00858 
00859 
00860 
00861 
00862   hs = Halfspace(Vec3f(0, 0, 1), 0);
00863 
00864   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
00865   BOOST_CHECK(res);
00866   BOOST_CHECK(std::abs(depth - 10) < 0.001);
00867   BOOST_CHECK(normal.equal(Vec3f(0, 0, -1)));
00868   BOOST_CHECK(contact.equal(Vec3f(0, 0, -5)));
00869 
00870   res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
00871   BOOST_CHECK(res);
00872   BOOST_CHECK(std::abs(depth - 10) < 0.001);
00873   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))));
00874   BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, -5))));
00875 
00876   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal);
00877   BOOST_CHECK(res);
00878   BOOST_CHECK(std::abs(depth - 12.5) < 0.001);
00879   BOOST_CHECK(normal.equal(Vec3f(0, 0, -1)));
00880   BOOST_CHECK(contact.equal(Vec3f(0, 0, -3.75)));
00881 
00882   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal);
00883   BOOST_CHECK(res);
00884   BOOST_CHECK(std::abs(depth - 12.5) < 0.001);
00885   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))));
00886   BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, -3.75))));
00887 
00888   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal);
00889   BOOST_CHECK(res);
00890   BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
00891   BOOST_CHECK(normal.equal(Vec3f(0, 0, -1)));
00892   BOOST_CHECK(contact.equal(Vec3f(0, 0, -6.25)));
00893 
00894   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal);
00895   BOOST_CHECK(res);
00896   BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
00897   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))));
00898   BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, -6.25))));
00899 
00900   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 10.1)), &contact, &depth, &normal);
00901   BOOST_CHECK(res);
00902   BOOST_CHECK(std::abs(depth - 20.1) < 0.001);
00903   BOOST_CHECK(normal.equal(Vec3f(0, 0, -1)));
00904   BOOST_CHECK(contact.equal(Vec3f(0, 0, 0.05)));
00905 
00906   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 10.1)), &contact, &depth, &normal);
00907   BOOST_CHECK(res);
00908   BOOST_CHECK(std::abs(depth - 20.1) < 0.001);
00909   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))));
00910   BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0.05))));
00911 
00912   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -10.1)), &contact, &depth, &normal);
00913   BOOST_CHECK_FALSE(res);
00914 
00915   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -10.1)), &contact, &depth, &normal);
00916   BOOST_CHECK_FALSE(res);
00917 }
00918 
00919 BOOST_AUTO_TEST_CASE(shapeIntersection_planecapsule)
00920 {
00921   Capsule s(5, 10);
00922   Plane hs(Vec3f(1, 0, 0), 0);
00923 
00924   Transform3f transform;
00925   generateRandomTransform(extents, transform);
00926 
00927   Vec3f contact;
00928   FCL_REAL depth;
00929   Vec3f normal;
00930   bool res;
00931 
00932   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
00933   BOOST_CHECK(res);
00934   BOOST_CHECK(std::abs(depth - 5) < 0.001);
00935   BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)) || normal.equal(Vec3f(1, 0, 0)));
00936   BOOST_CHECK(contact.equal(Vec3f(0, 0, 0)));
00937 
00938   res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
00939   BOOST_CHECK(res);
00940   BOOST_CHECK(std::abs(depth - 5) < 0.001);
00941   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))) || normal.equal(transform.getQuatRotation().transform(Vec3f(1, 0, 0))));
00942   BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0))));
00943 
00944   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal);
00945   BOOST_CHECK(res);
00946   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
00947   BOOST_CHECK(normal.equal(Vec3f(1, 0, 0)));
00948   BOOST_CHECK(contact.equal(Vec3f(2.5, 0, 0)));
00949 
00950   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal);
00951   BOOST_CHECK(res);
00952   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
00953   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(1, 0, 0))));
00954   BOOST_CHECK(contact.equal(transform.transform(Vec3f(2.5, 0, 0))));
00955   
00956   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal);
00957   BOOST_CHECK(res);
00958   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
00959   BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
00960   BOOST_CHECK(contact.equal(Vec3f(-2.5, 0, 0)));
00961 
00962   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal);
00963   BOOST_CHECK(res);
00964   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
00965   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
00966   BOOST_CHECK(contact.equal(transform.transform(Vec3f(-2.5, 0, 0))));
00967 
00968   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal);
00969   BOOST_CHECK_FALSE(res);
00970 
00971   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal);
00972   BOOST_CHECK_FALSE(res);
00973 
00974   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal);
00975   BOOST_CHECK_FALSE(res);
00976 
00977   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal);
00978   BOOST_CHECK_FALSE(res);
00979 
00980 
00981 
00982 
00983   hs = Plane(Vec3f(0, 1, 0), 0);
00984 
00985   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
00986   BOOST_CHECK(res);
00987   BOOST_CHECK(std::abs(depth - 5) < 0.001);
00988   BOOST_CHECK(normal.equal(Vec3f(0, -1, 0)) || normal.equal(Vec3f(0, 1, 0)));
00989   BOOST_CHECK(contact.equal(Vec3f(0, 0, 0)));
00990 
00991   res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
00992   BOOST_CHECK(res);
00993   BOOST_CHECK(std::abs(depth - 5) < 0.001);
00994   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))) || normal.equal(transform.getQuatRotation().transform(Vec3f(0, 1, 0))));
00995   BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0))));
00996 
00997   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal);
00998   BOOST_CHECK(res);
00999   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
01000   BOOST_CHECK(normal.equal(Vec3f(0, 1, 0)));
01001   BOOST_CHECK(contact.equal(Vec3f(0, 2.5, 0)));
01002 
01003   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal);
01004   BOOST_CHECK(res);
01005   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
01006   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 1, 0))));
01007   BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 2.5, 0))));
01008 
01009   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal);
01010   BOOST_CHECK(res);
01011   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
01012   BOOST_CHECK(normal.equal(Vec3f(0, -1, 0)));
01013   BOOST_CHECK(contact.equal(Vec3f(0, -2.5, 0)));
01014 
01015   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal);
01016   BOOST_CHECK(res);
01017   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
01018   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
01019   BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, -2.5, 0))));
01020 
01021   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal);
01022   BOOST_CHECK_FALSE(res);
01023 
01024   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal);
01025   BOOST_CHECK_FALSE(res);
01026 
01027   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal);
01028   BOOST_CHECK_FALSE(res);
01029 
01030   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal);
01031   BOOST_CHECK_FALSE(res);
01032 
01033 
01034 
01035 
01036   hs = Plane(Vec3f(0, 0, 1), 0);
01037 
01038   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
01039   BOOST_CHECK(res);
01040   BOOST_CHECK(std::abs(depth - 10) < 0.001);
01041   BOOST_CHECK(normal.equal(Vec3f(0, 0, -1)) || normal.equal(Vec3f(0, 0, 1)));
01042   BOOST_CHECK(contact.equal(Vec3f(0, 0, 0)));
01043 
01044   res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
01045   BOOST_CHECK(res);
01046   BOOST_CHECK(std::abs(depth - 10) < 0.001);
01047   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))) || normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, 1))));
01048   BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0))));
01049 
01050   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal);
01051   BOOST_CHECK(res);
01052   BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
01053   BOOST_CHECK(normal.equal(Vec3f(0, 0, 1)));
01054   BOOST_CHECK(contact.equal(Vec3f(0, 0, 2.5)));
01055 
01056   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal);
01057   BOOST_CHECK(res);
01058   BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
01059   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, 1))));
01060   BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 2.5))));
01061 
01062   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal);
01063   BOOST_CHECK(res);
01064   BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
01065   BOOST_CHECK(normal.equal(Vec3f(0, 0, -1)));
01066   BOOST_CHECK(contact.equal(Vec3f(0, 0, -2.5)));
01067 
01068   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal);
01069   BOOST_CHECK(res);
01070   BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
01071   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))));
01072   BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, -2.5))));
01073 
01074   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 10.1)), &contact, &depth, &normal);
01075   BOOST_CHECK_FALSE(res);
01076 
01077   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 10.1)), &contact, &depth, &normal);
01078   BOOST_CHECK_FALSE(res);
01079 
01080   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -10.1)), &contact, &depth, &normal);
01081   BOOST_CHECK_FALSE(res);
01082 
01083   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -10.1)), &contact, &depth, &normal);
01084   BOOST_CHECK_FALSE(res);
01085 }
01086 
01087 BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder)
01088 {
01089   Cylinder s(5, 10);
01090   Halfspace hs(Vec3f(1, 0, 0), 0);
01091 
01092   Transform3f transform;
01093   generateRandomTransform(extents, transform);
01094 
01095   Vec3f contact;
01096   FCL_REAL depth;
01097   Vec3f normal;
01098   bool res;
01099   
01100   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
01101   BOOST_CHECK(res);
01102   BOOST_CHECK(std::abs(depth - 5) < 0.001);
01103   BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
01104   BOOST_CHECK(contact.equal(Vec3f(-2.5, 0, 0)));
01105 
01106   res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
01107   BOOST_CHECK(res);
01108   BOOST_CHECK(std::abs(depth - 5) < 0.001);
01109   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
01110   BOOST_CHECK(contact.equal(transform.transform(Vec3f(-2.5, 0, 0))));
01111 
01112   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal);
01113   BOOST_CHECK(res);
01114   BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
01115   BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
01116   BOOST_CHECK(contact.equal(Vec3f(-1.25, 0, 0)));
01117 
01118   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal);
01119   BOOST_CHECK(res);
01120   BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
01121   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
01122   BOOST_CHECK(contact.equal(transform.transform(Vec3f(-1.25, 0, 0))));
01123   
01124   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal);
01125   BOOST_CHECK(res);
01126   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
01127   BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
01128   BOOST_CHECK(contact.equal(Vec3f(-3.75, 0, 0)));
01129 
01130   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal);
01131   BOOST_CHECK(res);
01132   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
01133   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
01134   BOOST_CHECK(contact.equal(transform.transform(Vec3f(-3.75, 0, 0))));
01135 
01136   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal);
01137   BOOST_CHECK(res);
01138   BOOST_CHECK(std::abs(depth - 10.1) < 0.001);
01139   BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
01140   BOOST_CHECK(contact.equal(Vec3f(0.05, 0, 0)));
01141 
01142   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal);
01143   BOOST_CHECK(res);
01144   BOOST_CHECK(std::abs(depth - 10.1) < 0.001);
01145   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
01146   BOOST_CHECK(contact.equal(transform.transform(Vec3f(0.05, 0, 0))));
01147 
01148   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal);
01149   BOOST_CHECK_FALSE(res);
01150 
01151   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal);
01152   BOOST_CHECK_FALSE(res);
01153 
01154 
01155 
01156 
01157   hs = Halfspace(Vec3f(0, 1, 0), 0);
01158 
01159   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
01160   BOOST_CHECK(res);
01161   BOOST_CHECK(std::abs(depth - 5) < 0.001);
01162   BOOST_CHECK(normal.equal(Vec3f(0, -1, 0)));
01163   BOOST_CHECK(contact.equal(Vec3f(0, -2.5, 0)));
01164 
01165   res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
01166   BOOST_CHECK(res);
01167   BOOST_CHECK(std::abs(depth - 5) < 0.001);
01168   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
01169   BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, -2.5, 0))));
01170 
01171   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal);
01172   BOOST_CHECK(res);
01173   BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
01174   BOOST_CHECK(normal.equal(Vec3f(0, -1, 0)));
01175   BOOST_CHECK(contact.equal(Vec3f(0, -1.25, 0)));
01176 
01177   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal);
01178   BOOST_CHECK(res);
01179   BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
01180   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
01181   BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, -1.25, 0))));
01182 
01183   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal);
01184   BOOST_CHECK(res);
01185   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
01186   BOOST_CHECK(normal.equal(Vec3f(0, -1, 0)));
01187   BOOST_CHECK(contact.equal(Vec3f(0, -3.75, 0)));
01188 
01189   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal);
01190   BOOST_CHECK(res);
01191   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
01192   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
01193   BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, -3.75, 0))));
01194 
01195   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal);
01196   BOOST_CHECK(res);
01197   BOOST_CHECK(std::abs(depth - 10.1) < 0.001);
01198   BOOST_CHECK(normal.equal(Vec3f(0, -1, 0)));
01199   BOOST_CHECK(contact.equal(Vec3f(0, 0.05, 0)));
01200 
01201   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal);
01202   BOOST_CHECK(res);
01203   BOOST_CHECK(std::abs(depth - 10.1) < 0.001);
01204   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
01205   BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0.05, 0))));
01206 
01207   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal);
01208   BOOST_CHECK_FALSE(res);
01209 
01210   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal);
01211   BOOST_CHECK_FALSE(res);
01212 
01213 
01214 
01215 
01216   hs = Halfspace(Vec3f(0, 0, 1), 0);
01217 
01218   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
01219   BOOST_CHECK(res);
01220   BOOST_CHECK(std::abs(depth - 5) < 0.001);
01221   BOOST_CHECK(normal.equal(Vec3f(0, 0, -1)));
01222   BOOST_CHECK(contact.equal(Vec3f(0, 0, -2.5)));
01223 
01224   res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
01225   BOOST_CHECK(res);
01226   BOOST_CHECK(std::abs(depth - 5) < 0.001);
01227   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))));
01228   BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, -2.5))));
01229 
01230   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal);
01231   BOOST_CHECK(res);
01232   BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
01233   BOOST_CHECK(normal.equal(Vec3f(0, 0, -1)));
01234   BOOST_CHECK(contact.equal(Vec3f(0, 0, -1.25)));
01235 
01236   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal);
01237   BOOST_CHECK(res);
01238   BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
01239   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))));
01240   BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, -1.25))));
01241 
01242   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal);
01243   BOOST_CHECK(res);
01244   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
01245   BOOST_CHECK(normal.equal(Vec3f(0, 0, -1)));
01246   BOOST_CHECK(contact.equal(Vec3f(0, 0, -3.75)));
01247 
01248   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal);
01249   BOOST_CHECK(res);
01250   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
01251   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))));
01252   BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, -3.75))));
01253 
01254   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 5.1)), &contact, &depth, &normal);
01255   BOOST_CHECK(res);
01256   BOOST_CHECK(std::abs(depth - 10.1) < 0.001);
01257   BOOST_CHECK(normal.equal(Vec3f(0, 0, -1)));
01258   BOOST_CHECK(contact.equal(Vec3f(0, 0, 0.05)));
01259 
01260   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 5.1)), &contact, &depth, &normal);
01261   BOOST_CHECK(res);
01262   BOOST_CHECK(std::abs(depth - 10.1) < 0.001);
01263   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))));
01264   BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0.05))));
01265 
01266   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -5.1)), &contact, &depth, &normal);
01267   BOOST_CHECK_FALSE(res);
01268 
01269   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -5.1)), &contact, &depth, &normal);
01270   BOOST_CHECK_FALSE(res);
01271 }
01272 
01273 BOOST_AUTO_TEST_CASE(shapeIntersection_planecylinder)
01274 {
01275   Cylinder s(5, 10);
01276   Plane hs(Vec3f(1, 0, 0), 0);
01277 
01278   Transform3f transform;
01279   generateRandomTransform(extents, transform);
01280 
01281   Vec3f contact;
01282   FCL_REAL depth;
01283   Vec3f normal;
01284   bool res;
01285 
01286   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
01287   BOOST_CHECK(res);
01288   BOOST_CHECK(std::abs(depth - 5) < 0.001);
01289   BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)) || normal.equal(Vec3f(1, 0, 0)));
01290   BOOST_CHECK(contact.equal(Vec3f(0, 0, 0)));
01291 
01292   res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
01293   BOOST_CHECK(res);
01294   BOOST_CHECK(std::abs(depth - 5) < 0.001);
01295   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))) || normal.equal(transform.getQuatRotation().transform(Vec3f(1, 0, 0))));
01296   BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0))));
01297 
01298   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal);
01299   BOOST_CHECK(res);
01300   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
01301   BOOST_CHECK(normal.equal(Vec3f(1, 0, 0)));
01302   BOOST_CHECK(contact.equal(Vec3f(2.5, 0, 0)));
01303 
01304   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal);
01305   BOOST_CHECK(res);
01306   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
01307   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(1, 0, 0))));
01308   BOOST_CHECK(contact.equal(transform.transform(Vec3f(2.5, 0, 0))));
01309   
01310   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal);
01311   BOOST_CHECK(res);
01312   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
01313   BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
01314   BOOST_CHECK(contact.equal(Vec3f(-2.5, 0, 0)));
01315 
01316   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal);
01317   BOOST_CHECK(res);
01318   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
01319   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
01320   BOOST_CHECK(contact.equal(transform.transform(Vec3f(-2.5, 0, 0))));
01321 
01322   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal);
01323   BOOST_CHECK_FALSE(res);
01324 
01325   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal);
01326   BOOST_CHECK_FALSE(res);
01327 
01328   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal);
01329   BOOST_CHECK_FALSE(res);
01330 
01331   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal);
01332   BOOST_CHECK_FALSE(res);
01333 
01334 
01335 
01336 
01337   hs = Plane(Vec3f(0, 1, 0), 0);
01338 
01339   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
01340   BOOST_CHECK(res);
01341   BOOST_CHECK(std::abs(depth - 5) < 0.001);
01342   BOOST_CHECK(normal.equal(Vec3f(0, -1, 0)) || normal.equal(Vec3f(0, 1, 0)));
01343   BOOST_CHECK(contact.equal(Vec3f(0, 0, 0)));
01344 
01345   res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
01346   BOOST_CHECK(res);
01347   BOOST_CHECK(std::abs(depth - 5) < 0.001);
01348   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))) || normal.equal(transform.getQuatRotation().transform(Vec3f(0, 1, 0))));
01349   BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0))));
01350 
01351   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal);
01352   BOOST_CHECK(res);
01353   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
01354   BOOST_CHECK(normal.equal(Vec3f(0, 1, 0)));
01355   BOOST_CHECK(contact.equal(Vec3f(0, 2.5, 0)));
01356 
01357   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal);
01358   BOOST_CHECK(res);
01359   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
01360   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 1, 0))));
01361   BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 2.5, 0))));
01362 
01363   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal);
01364   BOOST_CHECK(res);
01365   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
01366   BOOST_CHECK(normal.equal(Vec3f(0, -1, 0)));
01367   BOOST_CHECK(contact.equal(Vec3f(0, -2.5, 0)));
01368 
01369   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal);
01370   BOOST_CHECK(res);
01371   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
01372   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
01373   BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, -2.5, 0))));
01374 
01375   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal);
01376   BOOST_CHECK_FALSE(res);
01377 
01378   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal);
01379   BOOST_CHECK_FALSE(res);
01380 
01381   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal);
01382   BOOST_CHECK_FALSE(res);
01383 
01384   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal);
01385   BOOST_CHECK_FALSE(res);
01386 
01387 
01388 
01389 
01390   hs = Plane(Vec3f(0, 0, 1), 0);
01391 
01392   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
01393   BOOST_CHECK(res);
01394   BOOST_CHECK(std::abs(depth - 5) < 0.001);
01395   BOOST_CHECK(normal.equal(Vec3f(0, 0, -1)) || normal.equal(Vec3f(0, 0, 1)));
01396   BOOST_CHECK(contact.equal(Vec3f(0, 0, 0)));
01397 
01398   res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
01399   BOOST_CHECK(res);
01400   BOOST_CHECK(std::abs(depth - 5) < 0.001);
01401   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))) || normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, 1))));
01402   BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0))));
01403 
01404   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal);
01405   BOOST_CHECK(res);
01406   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
01407   BOOST_CHECK(normal.equal(Vec3f(0, 0, 1)));
01408   BOOST_CHECK(contact.equal(Vec3f(0, 0, 2.5)));
01409 
01410   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal);
01411   BOOST_CHECK(res);
01412   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
01413   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, 1))));
01414   BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 2.5))));
01415 
01416   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal);
01417   BOOST_CHECK(res);
01418   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
01419   BOOST_CHECK(normal.equal(Vec3f(0, 0, -1)));
01420   BOOST_CHECK(contact.equal(Vec3f(0, 0, -2.5)));
01421 
01422   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal);
01423   BOOST_CHECK(res);
01424   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
01425   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))));
01426   BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, -2.5))));
01427 
01428   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 10.1)), &contact, &depth, &normal);
01429   BOOST_CHECK_FALSE(res);
01430 
01431   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 10.1)), &contact, &depth, &normal);
01432   BOOST_CHECK_FALSE(res);
01433 
01434   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -10.1)), &contact, &depth, &normal);
01435   BOOST_CHECK_FALSE(res);
01436 
01437   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -10.1)), &contact, &depth, &normal);
01438   BOOST_CHECK_FALSE(res);
01439 }
01440 
01441 
01442 BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone)
01443 {
01444   Cone s(5, 10);
01445   Halfspace hs(Vec3f(1, 0, 0), 0);
01446 
01447   Transform3f transform;
01448   generateRandomTransform(extents, transform);
01449 
01450   Vec3f contact;
01451   FCL_REAL depth;
01452   Vec3f normal;
01453   bool res;
01454   
01455   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
01456   BOOST_CHECK(res);
01457   BOOST_CHECK(std::abs(depth - 5) < 0.001);
01458   BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
01459   BOOST_CHECK(contact.equal(Vec3f(-2.5, 0, -5)));
01460 
01461   res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
01462   BOOST_CHECK(res);
01463   BOOST_CHECK(std::abs(depth - 5) < 0.001);
01464   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
01465   BOOST_CHECK(contact.equal(transform.transform(Vec3f(-2.5, 0, -5))));
01466 
01467   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal);
01468   BOOST_CHECK(res);
01469   BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
01470   BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
01471   BOOST_CHECK(contact.equal(Vec3f(-1.25, 0, -5)));
01472 
01473   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal);
01474   BOOST_CHECK(res);
01475   BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
01476   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
01477   BOOST_CHECK(contact.equal(transform.transform(Vec3f(-1.25, 0, -5))));
01478   
01479   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal);
01480   BOOST_CHECK(res);
01481   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
01482   BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
01483   BOOST_CHECK(contact.equal(Vec3f(-3.75, 0, -5)));
01484 
01485   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal);
01486   BOOST_CHECK(res);
01487   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
01488   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
01489   BOOST_CHECK(contact.equal(transform.transform(Vec3f(-3.75, 0, -5))));
01490 
01491   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal);
01492   BOOST_CHECK(res);
01493   BOOST_CHECK(std::abs(depth - 10.1) < 0.001);
01494   BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
01495   BOOST_CHECK(contact.equal(Vec3f(0.05, 0, -5)));
01496 
01497   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal);
01498   BOOST_CHECK(res);
01499   BOOST_CHECK(std::abs(depth - 10.1) < 0.001);
01500   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
01501   BOOST_CHECK(contact.equal(transform.transform(Vec3f(0.05, 0, -5))));
01502 
01503   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal);
01504   BOOST_CHECK_FALSE(res);
01505 
01506   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal);
01507   BOOST_CHECK_FALSE(res);
01508 
01509 
01510 
01511 
01512   hs = Halfspace(Vec3f(0, 1, 0), 0);
01513 
01514   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
01515   BOOST_CHECK(res);
01516   BOOST_CHECK(std::abs(depth - 5) < 0.001);
01517   BOOST_CHECK(normal.equal(Vec3f(0, -1, 0)));
01518   BOOST_CHECK(contact.equal(Vec3f(0, -2.5, -5)));
01519 
01520   res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
01521   BOOST_CHECK(res);
01522   BOOST_CHECK(std::abs(depth - 5) < 0.001);
01523   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
01524   BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, -2.5, -5))));
01525 
01526   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal);
01527   BOOST_CHECK(res);
01528   BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
01529   BOOST_CHECK(normal.equal(Vec3f(0, -1, 0)));
01530   BOOST_CHECK(contact.equal(Vec3f(0, -1.25, -5)));
01531 
01532   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal);
01533   BOOST_CHECK(res);
01534   BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
01535   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
01536   BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, -1.25, -5))));
01537 
01538   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal);
01539   BOOST_CHECK(res);
01540   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
01541   BOOST_CHECK(normal.equal(Vec3f(0, -1, 0)));
01542   BOOST_CHECK(contact.equal(Vec3f(0, -3.75, -5)));
01543 
01544   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal);
01545   BOOST_CHECK(res);
01546   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
01547   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
01548   BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, -3.75, -5))));
01549 
01550   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal);
01551   BOOST_CHECK(res);
01552   BOOST_CHECK(std::abs(depth - 10.1) < 0.001);
01553   BOOST_CHECK(normal.equal(Vec3f(0, -1, 0)));
01554   BOOST_CHECK(contact.equal(Vec3f(0, 0.05, -5)));
01555 
01556   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal);
01557   BOOST_CHECK(res);
01558   BOOST_CHECK(std::abs(depth - 10.1) < 0.001);
01559   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
01560   BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0.05, -5))));
01561 
01562   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal);
01563   BOOST_CHECK_FALSE(res);
01564 
01565   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal);
01566   BOOST_CHECK_FALSE(res);
01567 
01568 
01569 
01570 
01571   hs = Halfspace(Vec3f(0, 0, 1), 0);
01572 
01573   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
01574   BOOST_CHECK(res);
01575   BOOST_CHECK(std::abs(depth - 5) < 0.001);
01576   BOOST_CHECK(normal.equal(Vec3f(0, 0, -1)));
01577   BOOST_CHECK(contact.equal(Vec3f(0, 0, -2.5)));
01578 
01579   res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
01580   BOOST_CHECK(res);
01581   BOOST_CHECK(std::abs(depth - 5) < 0.001);
01582   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))));
01583   BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, -2.5))));
01584 
01585   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal);
01586   BOOST_CHECK(res);
01587   BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
01588   BOOST_CHECK(normal.equal(Vec3f(0, 0, -1)));
01589   BOOST_CHECK(contact.equal(Vec3f(0, 0, -1.25)));
01590 
01591   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal);
01592   BOOST_CHECK(res);
01593   BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
01594   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))));
01595   BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, -1.25))));
01596 
01597   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal);
01598   BOOST_CHECK(res);
01599   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
01600   BOOST_CHECK(normal.equal(Vec3f(0, 0, -1)));
01601   BOOST_CHECK(contact.equal(Vec3f(0, 0, -3.75)));
01602 
01603   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal);
01604   BOOST_CHECK(res);
01605   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
01606   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))));
01607   BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, -3.75))));
01608 
01609   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 5.1)), &contact, &depth, &normal);
01610   BOOST_CHECK(res);
01611   BOOST_CHECK(std::abs(depth - 10.1) < 0.001);
01612   BOOST_CHECK(normal.equal(Vec3f(0, 0, -1)));
01613   BOOST_CHECK(contact.equal(Vec3f(0, 0, 0.05)));
01614 
01615   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 5.1)), &contact, &depth, &normal);
01616   BOOST_CHECK(res);
01617   BOOST_CHECK(std::abs(depth - 10.1) < 0.001);
01618   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))));
01619   BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0.05))));
01620 
01621   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -5.1)), &contact, &depth, &normal);
01622   BOOST_CHECK_FALSE(res);
01623 
01624   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -5.1)), &contact, &depth, &normal);
01625   BOOST_CHECK_FALSE(res);
01626 }
01627 
01628 BOOST_AUTO_TEST_CASE(shapeIntersection_planecone)
01629 {
01630   Cone s(5, 10);
01631   Plane hs(Vec3f(1, 0, 0), 0);
01632 
01633   Transform3f transform;
01634   generateRandomTransform(extents, transform);
01635 
01636   Vec3f contact;
01637   FCL_REAL depth;
01638   Vec3f normal;
01639   bool res;
01640 
01641   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
01642   BOOST_CHECK(res);
01643   BOOST_CHECK(std::abs(depth - 5) < 0.001);
01644   BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)) || normal.equal(Vec3f(1, 0, 0)));
01645   BOOST_CHECK(contact.equal(Vec3f(0, 0, 0)));
01646 
01647   res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
01648   BOOST_CHECK(res);
01649   BOOST_CHECK(std::abs(depth - 5) < 0.001);
01650   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))) || normal.equal(transform.getQuatRotation().transform(Vec3f(1, 0, 0))));
01651   BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0))));
01652 
01653   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal);
01654   BOOST_CHECK(res);
01655   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
01656   BOOST_CHECK(normal.equal(Vec3f(1, 0, 0)));
01657   BOOST_CHECK(contact.equal(Vec3f(2.5, 0, -2.5)));
01658 
01659   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal);
01660   BOOST_CHECK(res);
01661   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
01662   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(1, 0, 0))));
01663   BOOST_CHECK(contact.equal(transform.transform(Vec3f(2.5, 0, -2.5))));
01664   
01665   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal);
01666   BOOST_CHECK(res);
01667   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
01668   BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
01669   BOOST_CHECK(contact.equal(Vec3f(-2.5, 0, -2.5)));
01670 
01671   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal);
01672   BOOST_CHECK(res);
01673   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
01674   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
01675   BOOST_CHECK(contact.equal(transform.transform(Vec3f(-2.5, 0, -2.5))));
01676 
01677   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal);
01678   BOOST_CHECK_FALSE(res);
01679 
01680   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal);
01681   BOOST_CHECK_FALSE(res);
01682 
01683   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal);
01684   BOOST_CHECK_FALSE(res);
01685 
01686   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal);
01687   BOOST_CHECK_FALSE(res);
01688 
01689 
01690 
01691 
01692   hs = Plane(Vec3f(0, 1, 0), 0);
01693 
01694   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
01695   BOOST_CHECK(res);
01696   BOOST_CHECK(std::abs(depth - 5) < 0.001);
01697   BOOST_CHECK(normal.equal(Vec3f(0, -1, 0)) || normal.equal(Vec3f(0, 1, 0)));
01698   BOOST_CHECK(contact.equal(Vec3f(0, 0, 0)));
01699 
01700   res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
01701   BOOST_CHECK(res);
01702   BOOST_CHECK(std::abs(depth - 5) < 0.001);
01703   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))) || normal.equal(transform.getQuatRotation().transform(Vec3f(0, 1, 0))));
01704   BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0))));
01705 
01706   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal);
01707   BOOST_CHECK(res);
01708   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
01709   BOOST_CHECK(normal.equal(Vec3f(0, 1, 0)));
01710   BOOST_CHECK(contact.equal(Vec3f(0, 2.5, -2.5)));
01711 
01712   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal);
01713   BOOST_CHECK(res);
01714   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
01715   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 1, 0))));
01716   BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 2.5, -2.5))));
01717 
01718   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal);
01719   BOOST_CHECK(res);
01720   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
01721   BOOST_CHECK(normal.equal(Vec3f(0, -1, 0)));
01722   BOOST_CHECK(contact.equal(Vec3f(0, -2.5, -2.5)));
01723 
01724   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal);
01725   BOOST_CHECK(res);
01726   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
01727   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
01728   BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, -2.5, -2.5))));
01729 
01730   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal);
01731   BOOST_CHECK_FALSE(res);
01732 
01733   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal);
01734   BOOST_CHECK_FALSE(res);
01735 
01736   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal);
01737   BOOST_CHECK_FALSE(res);
01738 
01739   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal);
01740   BOOST_CHECK_FALSE(res);
01741 
01742 
01743 
01744 
01745   hs = Plane(Vec3f(0, 0, 1), 0);
01746 
01747   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
01748   BOOST_CHECK(res);
01749   BOOST_CHECK(std::abs(depth - 5) < 0.001);
01750   BOOST_CHECK(normal.equal(Vec3f(0, 0, -1)) || normal.equal(Vec3f(0, 0, 1)));
01751   BOOST_CHECK(contact.equal(Vec3f(0, 0, 0)));
01752 
01753   res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
01754   BOOST_CHECK(res);
01755   BOOST_CHECK(std::abs(depth - 5) < 0.001);
01756   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))) || normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, 1))));
01757   BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0))));
01758 
01759   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal);
01760   BOOST_CHECK(res);
01761   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
01762   BOOST_CHECK(normal.equal(Vec3f(0, 0, 1)));
01763   BOOST_CHECK(contact.equal(Vec3f(0, 0, 2.5)));
01764 
01765   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal);
01766   BOOST_CHECK(res);
01767   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
01768   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, 1))));
01769   BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 2.5))));
01770 
01771   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal);
01772   BOOST_CHECK(res);
01773   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
01774   BOOST_CHECK(normal.equal(Vec3f(0, 0, -1)));
01775   BOOST_CHECK(contact.equal(Vec3f(0, 0, -2.5)));
01776 
01777   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal);
01778   BOOST_CHECK(res);
01779   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
01780   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))));
01781   BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, -2.5))));
01782 
01783   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 10.1)), &contact, &depth, &normal);
01784   BOOST_CHECK_FALSE(res);
01785 
01786   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 10.1)), &contact, &depth, &normal);
01787   BOOST_CHECK_FALSE(res);
01788 
01789   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -10.1)), &contact, &depth, &normal);
01790   BOOST_CHECK_FALSE(res);
01791 
01792   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -10.1)), &contact, &depth, &normal);
01793   BOOST_CHECK_FALSE(res);
01794 }
01795 
01796 
01797 
01798 BOOST_AUTO_TEST_CASE(shapeDistance_spheresphere)
01799 {  
01800   Sphere s1(20);
01801   Sphere s2(10);
01802 
01803   Transform3f transform;
01804   generateRandomTransform(extents, transform);
01805 
01806   bool res;
01807   FCL_REAL dist = -1;
01808   
01809   res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &dist);
01810   BOOST_CHECK(fabs(dist - 10) < 0.001);
01811   BOOST_CHECK(res);
01812 
01813   res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(30.1, 0, 0)), &dist);
01814   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
01815   BOOST_CHECK(res);
01816 
01817   res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(29.9, 0, 0)), &dist);
01818   BOOST_CHECK(dist < 0);
01819   BOOST_CHECK_FALSE(res);
01820 
01821   res = solver1.shapeDistance(s1, Transform3f(Vec3f(40, 0, 0)), s2, Transform3f(), &dist);
01822   BOOST_CHECK(fabs(dist - 10) < 0.001);
01823   BOOST_CHECK(res);
01824 
01825   res = solver1.shapeDistance(s1, Transform3f(Vec3f(30.1, 0, 0)), s2, Transform3f(), &dist);
01826   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
01827   BOOST_CHECK(res);
01828 
01829   res = solver1.shapeDistance(s1, Transform3f(Vec3f(29.9, 0, 0)), s2, Transform3f(), &dist);
01830   BOOST_CHECK(dist < 0);
01831   BOOST_CHECK_FALSE(res);
01832 
01833 
01834   res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), &dist);
01835   // this is one problem: the precise is low sometimes
01836   BOOST_CHECK(fabs(dist - 10) < 0.1);
01837   BOOST_CHECK(res);
01838 
01839   res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(30.1, 0, 0)), &dist);
01840   BOOST_CHECK(fabs(dist - 0.1) < 0.06);
01841   BOOST_CHECK(res);
01842 
01843   res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(29.9, 0, 0)), &dist);
01844   BOOST_CHECK(dist < 0);
01845   BOOST_CHECK_FALSE(res);
01846 
01847   res = solver1.shapeDistance(s1, transform * Transform3f(Vec3f(40, 0, 0)), s2, transform, &dist);
01848   BOOST_CHECK(fabs(dist - 10) < 0.1);
01849   BOOST_CHECK(res);
01850 
01851   res = solver1.shapeDistance(s1, transform * Transform3f(Vec3f(30.1, 0, 0)), s2, transform, &dist);
01852   BOOST_CHECK(fabs(dist - 0.1) < 0.1);
01853   BOOST_CHECK(res);
01854 
01855   res = solver1.shapeDistance(s1, transform * Transform3f(Vec3f(29.9, 0, 0)), s2, transform, &dist);
01856   BOOST_CHECK(dist < 0);
01857   BOOST_CHECK_FALSE(res);
01858 }
01859 
01860 BOOST_AUTO_TEST_CASE(shapeDistance_boxbox)
01861 {                      
01862   Box s1(20, 40, 50);
01863   Box s2(10, 10, 10);
01864 
01865   Transform3f transform;
01866   generateRandomTransform(extents, transform);
01867 
01868   bool res;
01869   FCL_REAL dist;
01870 
01871   res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), &dist);
01872   BOOST_CHECK(dist < 0);
01873   BOOST_CHECK_FALSE(res);
01874 
01875   res = solver1.shapeDistance(s1, transform, s2, transform, &dist);
01876   BOOST_CHECK(dist < 0);
01877   BOOST_CHECK_FALSE(res);
01878 
01879   res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(15.1, 0, 0)), &dist);
01880   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
01881   BOOST_CHECK(res);
01882 
01883   res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(15.1, 0, 0)), &dist);
01884   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
01885   BOOST_CHECK(res);
01886 
01887   res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(20, 0, 0)), &dist);
01888   BOOST_CHECK(fabs(dist - 5) < 0.001);
01889   BOOST_CHECK(res);
01890 
01891   res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(20, 0, 0)), &dist);
01892   BOOST_CHECK(fabs(dist - 5) < 0.001);
01893   BOOST_CHECK(res);
01894 }
01895 
01896 BOOST_AUTO_TEST_CASE(shapeDistance_boxsphere)
01897 {
01898   Sphere s1(20);
01899   Box s2(5, 5, 5);
01900 
01901   Transform3f transform;
01902   generateRandomTransform(extents, transform);
01903 
01904   bool res;
01905   FCL_REAL dist;
01906 
01907   res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), &dist);
01908   BOOST_CHECK(dist < 0);
01909   BOOST_CHECK_FALSE(res);
01910 
01911   res = solver1.shapeDistance(s1, transform, s2, transform, &dist);
01912   BOOST_CHECK(dist < 0);
01913   BOOST_CHECK_FALSE(res);
01914 
01915   res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(22.6, 0, 0)), &dist);
01916   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
01917   BOOST_CHECK(res);
01918   
01919   res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(22.6, 0, 0)), &dist);
01920   BOOST_CHECK(fabs(dist - 0.1) < 0.05);
01921   BOOST_CHECK(res);
01922 
01923   res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &dist);
01924   BOOST_CHECK(fabs(dist - 17.5) < 0.001);
01925   BOOST_CHECK(res);
01926   
01927   res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), &dist);
01928   BOOST_CHECK(fabs(dist - 17.5) < 0.001);
01929   BOOST_CHECK(res);
01930 }
01931 
01932 BOOST_AUTO_TEST_CASE(shapeDistance_cylindercylinder)
01933 {
01934   Cylinder s1(5, 10);
01935   Cylinder s2(5, 10);
01936 
01937   Transform3f transform;
01938   generateRandomTransform(extents, transform);
01939 
01940   bool res;
01941   FCL_REAL dist;
01942 
01943   res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), &dist);
01944   BOOST_CHECK(dist < 0);
01945   BOOST_CHECK_FALSE(res);
01946 
01947   res = solver1.shapeDistance(s1, transform, s2, transform, &dist);
01948   BOOST_CHECK(dist < 0);
01949   BOOST_CHECK_FALSE(res);
01950 
01951   res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), &dist);
01952   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
01953   BOOST_CHECK(res);
01954 
01955   res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), &dist);
01956   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
01957   BOOST_CHECK(res);
01958 
01959   res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &dist);
01960   BOOST_CHECK(fabs(dist - 30) < 0.001);
01961   BOOST_CHECK(res);
01962 
01963   res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), &dist);
01964   BOOST_CHECK(fabs(dist - 30) < 0.001);
01965   BOOST_CHECK(res);
01966 }
01967 
01968 
01969 
01970 BOOST_AUTO_TEST_CASE(shapeDistance_conecone)
01971 {
01972   Cone s1(5, 10);
01973   Cone s2(5, 10);
01974 
01975   Transform3f transform;
01976   generateRandomTransform(extents, transform);
01977 
01978   bool res;
01979   FCL_REAL dist;
01980 
01981   res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), &dist);
01982   BOOST_CHECK(dist < 0);
01983   BOOST_CHECK_FALSE(res);
01984 
01985   res = solver1.shapeDistance(s1, transform, s2, transform, &dist);
01986   BOOST_CHECK(dist < 0);
01987   BOOST_CHECK_FALSE(res);
01988 
01989   res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), &dist);
01990   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
01991   BOOST_CHECK(res);
01992 
01993   res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), &dist);
01994   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
01995   BOOST_CHECK(res);
01996 
01997   res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 40)), &dist);
01998   BOOST_CHECK(fabs(dist - 30) < 1);
01999   BOOST_CHECK(res);
02000 
02001   res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 40)), &dist);
02002   BOOST_CHECK(fabs(dist - 30) < 1);
02003   BOOST_CHECK(res);
02004 }
02005 
02006 BOOST_AUTO_TEST_CASE(shapeDistance_conecylinder)
02007 {
02008   Cylinder s1(5, 10);
02009   Cone s2(5, 10);
02010 
02011   Transform3f transform;
02012   generateRandomTransform(extents, transform);
02013 
02014   bool res;
02015   FCL_REAL dist;
02016 
02017   res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), &dist);
02018   BOOST_CHECK(dist < 0);
02019   BOOST_CHECK_FALSE(res);
02020 
02021   res = solver1.shapeDistance(s1, transform, s2, transform, &dist);
02022   BOOST_CHECK(dist < 0);
02023   BOOST_CHECK_FALSE(res);
02024 
02025   res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), &dist);
02026   BOOST_CHECK(fabs(dist - 0.1) < 0.01);
02027   BOOST_CHECK(res);
02028 
02029   res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), &dist);
02030   BOOST_CHECK(fabs(dist - 0.1) < 0.02);
02031   BOOST_CHECK(res);
02032 
02033   res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &dist);
02034   BOOST_CHECK(fabs(dist - 30) < 0.01);
02035   BOOST_CHECK(res);
02036 
02037   res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), &dist);
02038   BOOST_CHECK(fabs(dist - 30) < 0.1);
02039   BOOST_CHECK(res);
02040 }
02041 
02042 
02043 
02044 BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spheresphere)
02045 {
02046   Sphere s1(20);
02047   Sphere s2(10);
02048 
02049   Transform3f transform;
02050   generateRandomTransform(extents, transform);
02051 
02052   CollisionRequest request;
02053   CollisionResult result;
02054 
02055   Vec3f contact;
02056   FCL_REAL penetration_depth;
02057   Vec3f normal;  
02058   bool res;
02059 
02060   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), NULL, NULL, NULL);
02061   BOOST_CHECK_FALSE(res); 
02062   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &contact, &penetration_depth, &normal);
02063   BOOST_CHECK_FALSE(res);
02064   result.clear();
02065   res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(40, 0, 0)), &solver2, request, result) > 0);
02066   BOOST_CHECK_FALSE(res);
02067 
02068   res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), NULL, NULL, NULL);
02069   BOOST_CHECK_FALSE(res);
02070   res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), &contact, &penetration_depth, &normal);
02071   BOOST_CHECK_FALSE(res);
02072   result.clear();
02073   res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(40, 0, 0)), &solver2, request, result) > 0);
02074   BOOST_CHECK_FALSE(res);
02075 
02076 
02077   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(30, 0, 0)), NULL, NULL, NULL);
02078   BOOST_CHECK(res);
02079   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(30, 0, 0)), &contact, &penetration_depth, &normal);
02080   BOOST_CHECK(res);
02081   result.clear();
02082   res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(30, 0, 0)), &solver2, request, result) > 0);
02083   BOOST_CHECK(res);
02084 
02085 
02086   res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(30.01, 0, 0)), NULL, NULL, NULL);
02087   BOOST_CHECK_FALSE(res);
02088   res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(30.01, 0, 0)), &contact, &penetration_depth, &normal);
02089   BOOST_CHECK_FALSE(res);
02090   result.clear();
02091   res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(30.01, 0, 0)), &solver2, request, result) > 0);
02092   BOOST_CHECK_FALSE(res);
02093 
02094 
02095   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(29.9, 0, 0)), NULL, NULL, NULL);
02096   BOOST_CHECK(res);
02097   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(29.9, 0, 0)), &contact, &penetration_depth, &normal);
02098   BOOST_CHECK(res);
02099   result.clear();
02100   res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(29.9, 0, 0)), &solver2, request, result) > 0);
02101   BOOST_CHECK(res);
02102 
02103 
02104   res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(29.9, 0, 0)), NULL, NULL, NULL);
02105   BOOST_CHECK(res);
02106   res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(29.9, 0, 0)), &contact, &penetration_depth, &normal);
02107   BOOST_CHECK(res);
02108   result.clear();
02109   res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(29.9, 0, 0)), &solver2, request, result) > 0);
02110   BOOST_CHECK(res);
02111 
02112 
02113   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
02114   BOOST_CHECK(res);
02115   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), &contact, &penetration_depth, &normal);
02116   BOOST_CHECK(res);
02117   result.clear();
02118   res = (collide(&s1, Transform3f(), &s2, Transform3f(), &solver2, request, result) > 0);
02119   BOOST_CHECK(res);
02120 
02121 
02122   res = solver2.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
02123   BOOST_CHECK(res);
02124   res = solver2.shapeIntersect(s1, transform, s2, transform, &contact, &penetration_depth, &normal);
02125   BOOST_CHECK(res);
02126   result.clear();
02127   res = (collide(&s1, transform, &s2, transform, &solver2, request, result) > 0);
02128   BOOST_CHECK(res);
02129 
02130 
02131   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(-29.9, 0, 0)), NULL, NULL, NULL);
02132   BOOST_CHECK(res);
02133   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(-29.9, 0, 0)), &contact, &penetration_depth, &normal);
02134   BOOST_CHECK(res);
02135   result.clear();
02136   res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(-29.9, 0, 0)), &solver2, request, result) > 0);
02137   BOOST_CHECK(res);
02138 
02139 
02140 
02141   res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(-29.9, 0, 0)), NULL, NULL, NULL);
02142   BOOST_CHECK(res);
02143   res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(-29.9, 0, 0)), &contact, &penetration_depth, &normal);
02144   BOOST_CHECK(res);
02145   result.clear();
02146   res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(-29.9, 0, 0)), &solver2, request, result) > 0);
02147   BOOST_CHECK(res);
02148 
02149 
02150   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(-30, 0, 0)), NULL, NULL, NULL);
02151   BOOST_CHECK(res);
02152   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(-30, 0, 0)), &contact, &penetration_depth, &normal);
02153   BOOST_CHECK(res);
02154   result.clear();
02155   res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(-30, 0, 0)), &solver2, request, result) > 0);
02156   BOOST_CHECK(res);
02157 
02158   res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(-30.01, 0, 0)), NULL, NULL, NULL);
02159   BOOST_CHECK_FALSE(res);
02160   res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(-30.01, 0, 0)), &contact, &penetration_depth, &normal);
02161   BOOST_CHECK_FALSE(res);
02162   result.clear();
02163   res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(-30.01, 0, 0)), &solver2, request, result) > 0);
02164   BOOST_CHECK_FALSE(res);
02165 }
02166 
02167 BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_boxbox)
02168 {
02169   Box s1(20, 40, 50);
02170   Box s2(10, 10, 10);
02171 
02172   Transform3f transform;
02173   generateRandomTransform(extents, transform);
02174 
02175   Vec3f contact;
02176   FCL_REAL penetration_depth;
02177   Vec3f normal;  
02178   bool res;
02179 
02180   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
02181   BOOST_CHECK(res);
02182   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), &contact, &penetration_depth, &normal);
02183   BOOST_CHECK(res);
02184 
02185   res = solver2.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
02186   BOOST_CHECK(res);
02187   res = solver2.shapeIntersect(s1, transform, s2, transform, &contact, &penetration_depth, &normal);
02188   BOOST_CHECK(res);
02189 
02190   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(15, 0, 0)), NULL, NULL, NULL);
02191   BOOST_CHECK(res);
02192   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(15, 0, 0)), &contact, &penetration_depth, &normal);
02193   BOOST_CHECK(res);
02194 
02195   res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(15.01, 0, 0)), NULL, NULL, NULL);
02196   BOOST_CHECK_FALSE(res);
02197   res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(15.01, 0, 0)), &contact, &penetration_depth, &normal);
02198   BOOST_CHECK_FALSE(res);
02199 
02200   Quaternion3f q;
02201   q.fromAxisAngle(Vec3f(0, 0, 1), (FCL_REAL)3.140 / 6);
02202   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(q), NULL, NULL, NULL);
02203   BOOST_CHECK(res);
02204   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(q), &contact, &penetration_depth, &normal);
02205   BOOST_CHECK(res);
02206   
02207   res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(q), NULL, NULL, NULL);
02208   BOOST_CHECK(res);
02209   res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(q), &contact, &penetration_depth, &normal);
02210   BOOST_CHECK(res);
02211 }
02212 
02213 BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spherebox)
02214 {
02215   Sphere s1(20);
02216   Box s2(5, 5, 5);
02217 
02218   Transform3f transform;
02219   generateRandomTransform(extents, transform);
02220 
02221   Vec3f contact;
02222   FCL_REAL penetration_depth;
02223   Vec3f normal;  
02224   bool res;
02225 
02226   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
02227   BOOST_CHECK(res);
02228   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), &contact, &penetration_depth, &normal);
02229   BOOST_CHECK(res);
02230 
02231   res = solver2.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
02232   BOOST_CHECK(res);
02233   res = solver2.shapeIntersect(s1, transform, s2, transform, &contact, &penetration_depth, &normal);
02234   BOOST_CHECK(res);
02235 
02236   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(22.5, 0, 0)), NULL, NULL, NULL);
02237   BOOST_CHECK(res);
02238   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(22.5, 0, 0)), &contact, &penetration_depth, &normal);
02239   BOOST_CHECK(res);
02240 
02241   res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(22.51, 0, 0)), NULL, NULL, NULL);
02242   BOOST_CHECK_FALSE(res);
02243   res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(22.51, 0, 0)), &contact, &penetration_depth, &normal);
02244   BOOST_CHECK_FALSE(res);
02245 
02246   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(22.4, 0, 0)), NULL, NULL, NULL);
02247   BOOST_CHECK(res);
02248   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(22.4, 0, 0)), &contact, &penetration_depth, &normal);
02249   BOOST_CHECK(res);
02250 
02251   res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(22.4, 0, 0)), NULL, NULL, NULL);
02252   BOOST_CHECK(res);
02253   res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(22.4, 0, 0)), &contact, &penetration_depth, &normal);
02254   BOOST_CHECK(res);
02255 }
02256 
02257 BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_cylindercylinder)
02258 {
02259   Cylinder s1(5, 10);
02260   Cylinder s2(5, 10);
02261 
02262   Transform3f transform;
02263   generateRandomTransform(extents, transform);
02264 
02265   Vec3f contact;
02266   FCL_REAL penetration_depth;
02267   Vec3f normal;  
02268   bool res;
02269 
02270   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
02271   BOOST_CHECK(res);
02272   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), &contact, &penetration_depth, &normal);
02273   BOOST_CHECK(res);
02274 
02275   res = solver2.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
02276   BOOST_CHECK(res);
02277   res = solver2.shapeIntersect(s1, transform, s2, transform, &contact, &penetration_depth, &normal);
02278   BOOST_CHECK(res);
02279 
02280   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
02281   BOOST_CHECK(res);
02282   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), &contact, &penetration_depth, &normal);
02283   BOOST_CHECK(res);
02284 
02285   res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
02286   BOOST_CHECK(res);
02287   res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), &contact, &penetration_depth, &normal);
02288   BOOST_CHECK(res);
02289 
02290   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10, 0, 0)), NULL, NULL, NULL);
02291   BOOST_CHECK(res);
02292   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10, 0, 0)), &contact, &penetration_depth, &normal);
02293   BOOST_CHECK(res);
02294 
02295   res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), NULL, NULL, NULL);
02296   BOOST_CHECK_FALSE(res);
02297   res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), &contact, &penetration_depth, &normal);
02298   BOOST_CHECK_FALSE(res);
02299 }
02300 
02301 BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_conecone)
02302 {
02303   Cone s1(5, 10);
02304   Cone s2(5, 10);
02305 
02306   Transform3f transform;
02307   generateRandomTransform(extents, transform);
02308 
02309   Vec3f contact;
02310   FCL_REAL penetration_depth;
02311   Vec3f normal;  
02312   bool res;
02313 
02314   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
02315   BOOST_CHECK(res);
02316   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), &contact, &penetration_depth, &normal);
02317   BOOST_CHECK(res);
02318 
02319   res = solver2.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
02320   BOOST_CHECK(res);
02321   res = solver2.shapeIntersect(s1, transform, s2, transform, &contact, &penetration_depth, &normal);
02322   BOOST_CHECK(res);
02323 
02324   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
02325   BOOST_CHECK(res);
02326   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), &contact, &penetration_depth, &normal);
02327   BOOST_CHECK(res);
02328 
02329   res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
02330   BOOST_CHECK(res);
02331   res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), &contact, &penetration_depth, &normal);
02332   BOOST_CHECK(res);
02333 
02334   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), NULL, NULL, NULL);
02335   BOOST_CHECK_FALSE(res);
02336   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), &contact, &penetration_depth, &normal);
02337   BOOST_CHECK_FALSE(res);
02338 
02339   res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), NULL, NULL, NULL);
02340   BOOST_CHECK_FALSE(res);
02341   res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), &contact, &penetration_depth, &normal);
02342   BOOST_CHECK_FALSE(res);
02343 
02344   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 9.9)), NULL, NULL, NULL);
02345   BOOST_CHECK(res);
02346   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 9.9)), &contact, &penetration_depth, &normal);
02347   BOOST_CHECK(res);
02348 
02349   res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 9.9)), NULL, NULL, NULL);
02350   BOOST_CHECK(res);
02351   res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 9.9)), &contact, &penetration_depth, &normal);
02352   BOOST_CHECK(res);
02353 }
02354 
02355 BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_conecylinder)
02356 {
02357   Cylinder s1(5, 10);
02358   Cone s2(5, 10);
02359 
02360   Transform3f transform;
02361   generateRandomTransform(extents, transform);
02362 
02363   Vec3f contact;
02364   FCL_REAL penetration_depth;
02365   Vec3f normal;  
02366   bool res;
02367 
02368   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
02369   BOOST_CHECK(res);
02370   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), &contact, &penetration_depth, &normal);
02371   BOOST_CHECK(res);
02372 
02373   res = solver2.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
02374   BOOST_CHECK(res);
02375   res = solver2.shapeIntersect(s1, transform, s2, transform, &contact, &penetration_depth, &normal);
02376   BOOST_CHECK(res);
02377 
02378   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
02379   BOOST_CHECK(res);
02380   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), &contact, &penetration_depth, &normal);
02381   BOOST_CHECK(res);
02382 
02383   res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
02384   BOOST_CHECK(res);
02385   res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), &contact, &penetration_depth, &normal);
02386   BOOST_CHECK(res);
02387 
02388   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10, 0, 0)), NULL, NULL, NULL);
02389   BOOST_CHECK(res);
02390   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10, 0, 0)), &contact, &penetration_depth, &normal);
02391   BOOST_CHECK(res);
02392 
02393   res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10, 0, 0)), NULL, NULL, NULL);
02394   BOOST_CHECK(res);
02395   res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10, 0, 0)), &contact, &penetration_depth, &normal);
02396   BOOST_CHECK(res);
02397 
02398   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 9.9)), NULL, NULL, NULL);
02399   BOOST_CHECK(res);
02400   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 9.9)), &contact, &penetration_depth, &normal);
02401   BOOST_CHECK(res);
02402 
02403   res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 9.9)), NULL, NULL, NULL);
02404   BOOST_CHECK(res);
02405   res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 9.9)), &contact, &penetration_depth, &normal);
02406   BOOST_CHECK(res);
02407 
02408   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 10)), NULL, NULL, NULL);
02409   BOOST_CHECK(res);
02410   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 10)), &contact, &penetration_depth, &normal);
02411   BOOST_CHECK(res);
02412 
02413   res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 10.1)), NULL, NULL, NULL);
02414   BOOST_CHECK_FALSE(res);
02415   res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 10.1)), &contact, &penetration_depth, &normal);
02416   BOOST_CHECK_FALSE(res);
02417 }
02418 
02419 
02420 BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spheretriangle)
02421 {
02422   Sphere s(10);
02423   Vec3f t[3];
02424   t[0].setValue(20, 0, 0);
02425   t[1].setValue(-20, 0, 0);
02426   t[2].setValue(0, 20, 0);
02427 
02428   Transform3f transform;
02429   generateRandomTransform(extents, transform);
02430 
02431   bool res;
02432 
02433   res = solver2.shapeTriangleIntersect(s, Transform3f(), t[0], t[1], t[2], NULL, NULL, NULL);
02434   BOOST_CHECK(res);
02435 
02436   res =  solver2.shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL);
02437   BOOST_CHECK(res);
02438 
02439   t[0].setValue(30, 0, 0);
02440   t[1].setValue(9.9, -20, 0);
02441   t[2].setValue(9.9, 20, 0);
02442   res = solver2.shapeTriangleIntersect(s, Transform3f(), t[0], t[1], t[2], NULL, NULL, NULL);
02443   BOOST_CHECK(res);
02444 
02445   res =  solver2.shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL);
02446   BOOST_CHECK(res);
02447 }
02448 
02449 
02450 
02451 
02452 BOOST_AUTO_TEST_CASE(spheresphere)
02453 {  
02454   Sphere s1(20);
02455   Sphere s2(10);
02456 
02457   Transform3f transform;
02458   generateRandomTransform(extents, transform);
02459 
02460   bool res;
02461   FCL_REAL dist = -1;
02462   
02463   res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &dist);
02464   BOOST_CHECK(fabs(dist - 10) < 0.001);
02465   BOOST_CHECK(res);
02466 
02467   res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(30.1, 0, 0)), &dist);
02468   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
02469   BOOST_CHECK(res);
02470 
02471   res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(29.9, 0, 0)), &dist);
02472   BOOST_CHECK(dist < 0);
02473   BOOST_CHECK_FALSE(res);
02474 
02475   res = solver2.shapeDistance(s1, Transform3f(Vec3f(40, 0, 0)), s2, Transform3f(), &dist);
02476   BOOST_CHECK(fabs(dist - 10) < 0.001);
02477   BOOST_CHECK(res);
02478 
02479   res = solver2.shapeDistance(s1, Transform3f(Vec3f(30.1, 0, 0)), s2, Transform3f(), &dist);
02480   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
02481   BOOST_CHECK(res);
02482 
02483   res = solver2.shapeDistance(s1, Transform3f(Vec3f(29.9, 0, 0)), s2, Transform3f(), &dist);
02484   BOOST_CHECK(dist < 0);
02485   BOOST_CHECK_FALSE(res);
02486 
02487 
02488   res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), &dist);
02489   BOOST_CHECK(fabs(dist - 10) < 0.001);
02490   BOOST_CHECK(res);
02491 
02492   res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(30.1, 0, 0)), &dist);
02493   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
02494   BOOST_CHECK(res);
02495 
02496   res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(29.9, 0, 0)), &dist);
02497   BOOST_CHECK(dist < 0);
02498   BOOST_CHECK_FALSE(res);
02499 
02500   res = solver2.shapeDistance(s1, transform * Transform3f(Vec3f(40, 0, 0)), s2, transform, &dist);
02501   BOOST_CHECK(fabs(dist - 10) < 0.001);
02502   BOOST_CHECK(res);
02503 
02504   res = solver2.shapeDistance(s1, transform * Transform3f(Vec3f(30.1, 0, 0)), s2, transform, &dist);
02505   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
02506   BOOST_CHECK(res);
02507 
02508   res = solver2.shapeDistance(s1, transform * Transform3f(Vec3f(29.9, 0, 0)), s2, transform, &dist);
02509   BOOST_CHECK(dist < 0);
02510   BOOST_CHECK_FALSE(res);
02511 }
02512 
02513 BOOST_AUTO_TEST_CASE(boxbox)
02514 {                      
02515   Box s1(20, 40, 50);
02516   Box s2(10, 10, 10);
02517 
02518   Transform3f transform;
02519   generateRandomTransform(extents, transform);
02520 
02521   bool res;
02522   FCL_REAL dist;
02523 
02524   res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(), &dist);
02525   BOOST_CHECK(dist < 0);
02526   BOOST_CHECK_FALSE(res);
02527 
02528   res = solver2.shapeDistance(s1, transform, s2, transform, &dist);
02529   BOOST_CHECK(dist < 0);
02530   BOOST_CHECK_FALSE(res);
02531 
02532   res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(15.1, 0, 0)), &dist);
02533   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
02534   BOOST_CHECK(res);
02535 
02536   res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(15.1, 0, 0)), &dist);
02537   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
02538   BOOST_CHECK(res);
02539 
02540   res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(20, 0, 0)), &dist);
02541   BOOST_CHECK(fabs(dist - 5) < 0.001);
02542   BOOST_CHECK(res);
02543 
02544   res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(20, 0, 0)), &dist);
02545   BOOST_CHECK(fabs(dist - 5) < 0.001);
02546   BOOST_CHECK(res);
02547 }
02548 
02549 BOOST_AUTO_TEST_CASE(boxsphere)
02550 {
02551   Sphere s1(20);
02552   Box s2(5, 5, 5);
02553 
02554   Transform3f transform;
02555   generateRandomTransform(extents, transform);
02556 
02557   bool res;
02558   FCL_REAL dist;
02559 
02560   res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(), &dist);
02561   BOOST_CHECK(dist < 0);
02562   BOOST_CHECK_FALSE(res);
02563 
02564   res = solver2.shapeDistance(s1, transform, s2, transform, &dist);
02565   BOOST_CHECK(dist < 0);
02566   BOOST_CHECK_FALSE(res);
02567 
02568   res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(22.6, 0, 0)), &dist);
02569   BOOST_CHECK(fabs(dist - 0.1) < 0.01);
02570   BOOST_CHECK(res);
02571   
02572   res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(22.6, 0, 0)), &dist);
02573   BOOST_CHECK(fabs(dist - 0.1) < 0.01);
02574   BOOST_CHECK(res);
02575 
02576   res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &dist);
02577   BOOST_CHECK(fabs(dist - 17.5) < 0.001);
02578   BOOST_CHECK(res);
02579   
02580   res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), &dist);
02581   BOOST_CHECK(fabs(dist - 17.5) < 0.001);
02582   BOOST_CHECK(res);
02583 }
02584 
02585 BOOST_AUTO_TEST_CASE(cylindercylinder)
02586 {
02587   Cylinder s1(5, 10);
02588   Cylinder s2(5, 10);
02589 
02590   Transform3f transform;
02591   generateRandomTransform(extents, transform);
02592 
02593   bool res;
02594   FCL_REAL dist;
02595 
02596   res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(), &dist);
02597   BOOST_CHECK(dist < 0);
02598   BOOST_CHECK_FALSE(res);
02599 
02600   res = solver2.shapeDistance(s1, transform, s2, transform, &dist);
02601   BOOST_CHECK(dist < 0);
02602   BOOST_CHECK_FALSE(res);
02603 
02604   res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), &dist);
02605   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
02606   BOOST_CHECK(res);
02607 
02608   res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), &dist);
02609   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
02610   BOOST_CHECK(res);
02611 
02612   res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &dist);
02613   BOOST_CHECK(fabs(dist - 30) < 0.001);
02614   BOOST_CHECK(res);
02615 
02616   res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), &dist);
02617   BOOST_CHECK(fabs(dist - 30) < 0.001);
02618   BOOST_CHECK(res);
02619 }
02620 
02621 
02622 
02623 BOOST_AUTO_TEST_CASE(conecone)
02624 {
02625   Cone s1(5, 10);
02626   Cone s2(5, 10);
02627 
02628   Transform3f transform;
02629   generateRandomTransform(extents, transform);
02630 
02631   bool res;
02632   FCL_REAL dist;
02633 
02634   res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(), &dist);
02635   BOOST_CHECK(dist < 0);
02636   BOOST_CHECK_FALSE(res);
02637 
02638   res = solver2.shapeDistance(s1, transform, s2, transform, &dist);
02639   BOOST_CHECK(dist < 0);
02640   BOOST_CHECK_FALSE(res);
02641 
02642   res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), &dist);
02643   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
02644   BOOST_CHECK(res);
02645 
02646   res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), &dist);
02647   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
02648   BOOST_CHECK(res);
02649 
02650   res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 40)), &dist);
02651   BOOST_CHECK(fabs(dist - 30) < 0.001);
02652   BOOST_CHECK(res);
02653 
02654   res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 40)), &dist);
02655   BOOST_CHECK(fabs(dist - 30) < 0.001);
02656   BOOST_CHECK(res);
02657 }
02658 
02659 BOOST_AUTO_TEST_CASE(conecylinder)
02660 {
02661   Cylinder s1(5, 10);
02662   Cone s2(5, 10);
02663 
02664   Transform3f transform;
02665   generateRandomTransform(extents, transform);
02666 
02667   bool res;
02668   FCL_REAL dist;
02669 
02670   res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(), &dist);
02671   BOOST_CHECK(dist < 0);
02672   BOOST_CHECK_FALSE(res);
02673 
02674   res = solver2.shapeDistance(s1, transform, s2, transform, &dist);
02675   BOOST_CHECK(dist < 0);
02676   BOOST_CHECK_FALSE(res);
02677 
02678   res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), &dist);
02679   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
02680   BOOST_CHECK(res);
02681 
02682   res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), &dist);
02683   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
02684   BOOST_CHECK(res);
02685 
02686   res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &dist);
02687   BOOST_CHECK(fabs(dist - 30) < 0.001);
02688   BOOST_CHECK(res);
02689 
02690   res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), &dist);
02691   BOOST_CHECK(fabs(dist - 30) < 0.001);
02692   BOOST_CHECK(res);
02693 }
02694 
02695 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines


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