fcl: test_fcl_math.cpp Source File

Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  */
00034 
00035 
00036 #define BOOST_TEST_MODULE "FCL_MATH"
00037 #include <boost/test/unit_test.hpp>
00038 
00039 #if FCL_HAVE_SSE
00040   #include "fcl/simd/math_simd_details.h"
00041 #endif
00042 #include "fcl/math/vec_3f.h"
00043 #include "fcl/math/matrix_3f.h"
00044 #include "fcl/broadphase/morton.h"
00045 #include "fcl/config.h"
00046 
00047 using namespace fcl;
00048 
00049 BOOST_AUTO_TEST_CASE(vec_test_basic_vec32)
00050 {
00051   typedef Vec3fX<details::Vec3Data<float> > Vec3f32;
00052   Vec3f32 v1(1.0f, 2.0f, 3.0f);
00053   BOOST_CHECK(v1[0] == 1.0f);
00054   BOOST_CHECK(v1[1] == 2.0f);
00055   BOOST_CHECK(v1[2] == 3.0f);
00056 
00057   Vec3f32 v2 = v1;
00058   Vec3f32 v3(3.3f, 4.3f, 5.3f);
00059   v1 += v3;
00060   BOOST_CHECK(v1.equal(v2 + v3));
00061   v1 -= v3;
00062   BOOST_CHECK(v1.equal(v2));
00063   v1 -= v3;
00064   BOOST_CHECK(v1.equal(v2 - v3));
00065   v1 += v3;
00066 
00067   v1 *= v3;
00068   BOOST_CHECK(v1.equal(v2 * v3));
00069   v1 /= v3;
00070   BOOST_CHECK(v1.equal(v2));
00071   v1 /= v3;
00072   BOOST_CHECK(v1.equal(v2 / v3));
00073   v1 *= v3;
00074 
00075   v1 *= 2.0f;
00076   BOOST_CHECK(v1.equal(v2 * 2.0f));
00077   v1 /= 2.0f;
00078   BOOST_CHECK(v1.equal(v2));
00079   v1 /= 2.0f;
00080   BOOST_CHECK(v1.equal(v2 / 2.0f));
00081   v1 *= 2.0f;
00082 
00083   v1 += 2.0f;
00084   BOOST_CHECK(v1.equal(v2 + 2.0f));
00085   v1 -= 2.0f;
00086   BOOST_CHECK(v1.equal(v2));
00087   v1 -= 2.0f;
00088   BOOST_CHECK(v1.equal(v2 - 2.0f));
00089   v1 += 2.0f;
00090   
00091   BOOST_CHECK((-Vec3f32(1.0f, 2.0f, 3.0f)).equal(Vec3f32(-1.0f, -2.0f, -3.0f)));
00092 
00093   v1 = Vec3f32(1.0f, 2.0f, 3.0f);
00094   v2 = Vec3f32(3.0f, 4.0f, 5.0f);
00095   BOOST_CHECK((v1.cross(v2)).equal(Vec3f32(-2.0f, 4.0f, -2.0f)));
00096   BOOST_CHECK(abs(v1.dot(v2) - 26) < 1e-5);
00097 
00098   v1 = Vec3f32(3.0f, 4.0f, 5.0f);
00099   BOOST_CHECK(abs(v1.sqrLength() - 50.0) < 1e-5);
00100   BOOST_CHECK(abs(v1.length() - sqrt(50.0)) < 1e-5);
00101   BOOST_CHECK(normalize(v1).equal(v1 / v1.length()));
00102 }
00103 
00104 BOOST_AUTO_TEST_CASE(vec_test_basic_vec64)
00105 {
00106   typedef Vec3fX<details::Vec3Data<double> > Vec3f64;
00107   Vec3f64 v1(1.0, 2.0, 3.0);
00108   BOOST_CHECK(v1[0] == 1.0);
00109   BOOST_CHECK(v1[1] == 2.0);
00110   BOOST_CHECK(v1[2] == 3.0);
00111 
00112   Vec3f64 v2 = v1;
00113   Vec3f64 v3(3.3, 4.3, 5.3);
00114   v1 += v3;
00115   BOOST_CHECK(v1.equal(v2 + v3));
00116   v1 -= v3;
00117   BOOST_CHECK(v1.equal(v2));
00118   v1 -= v3;
00119   BOOST_CHECK(v1.equal(v2 - v3));
00120   v1 += v3;
00121 
00122   v1 *= v3;
00123   BOOST_CHECK(v1.equal(v2 * v3));
00124   v1 /= v3;
00125   BOOST_CHECK(v1.equal(v2));
00126   v1 /= v3;
00127   BOOST_CHECK(v1.equal(v2 / v3));
00128   v1 *= v3;
00129 
00130   v1 *= 2.0;
00131   BOOST_CHECK(v1.equal(v2 * 2.0));
00132   v1 /= 2.0;
00133   BOOST_CHECK(v1.equal(v2));
00134   v1 /= 2.0;
00135   BOOST_CHECK(v1.equal(v2 / 2.0));
00136   v1 *= 2.0;
00137 
00138   v1 += 2.0;
00139   BOOST_CHECK(v1.equal(v2 + 2.0));
00140   v1 -= 2.0;
00141   BOOST_CHECK(v1.equal(v2));
00142   v1 -= 2.0;
00143   BOOST_CHECK(v1.equal(v2 - 2.0));
00144   v1 += 2.0;
00145 
00146   BOOST_CHECK((-Vec3f64(1.0, 2.0, 3.0)).equal(Vec3f64(-1.0, -2.0, -3.0)));
00147 
00148   v1 = Vec3f64(1.0, 2.0, 3.0);
00149   v2 = Vec3f64(3.0, 4.0, 5.0);
00150   BOOST_CHECK((v1.cross(v2)).equal(Vec3f64(-2.0, 4.0, -2.0)));
00151   BOOST_CHECK(abs(v1.dot(v2) - 26) < 1e-5);
00152 
00153   v1 = Vec3f64(3.0, 4.0, 5.0);
00154   BOOST_CHECK(abs(v1.sqrLength() - 50.0) < 1e-5);
00155   BOOST_CHECK(abs(v1.length() - sqrt(50.0)) < 1e-5);
00156   BOOST_CHECK(normalize(v1).equal(v1 / v1.length()));
00157 
00158 
00159   v1 = Vec3f64(1.0, 2.0, 3.0);
00160   v2 = Vec3f64(3.0, 4.0, 5.0);
00161   BOOST_CHECK((v1.cross(v2)).equal(Vec3f64(-2.0, 4.0, -2.0)));
00162   BOOST_CHECK(v1.dot(v2) == 26);
00163 }
00164 
00165 #if FCL_HAVE_SSE
00166 
00167 BOOST_AUTO_TEST_CASE(vec_test_sse_vec32)
00168 {
00169   typedef Vec3fX<details::sse_meta_f4> Vec3f32;
00170   Vec3f32 v1(1.0f, 2.0f, 3.0f);
00171   BOOST_CHECK(v1[0] == 1.0f);
00172   BOOST_CHECK(v1[1] == 2.0f);
00173   BOOST_CHECK(v1[2] == 3.0f);
00174 
00175   Vec3f32 v2 = v1;
00176   Vec3f32 v3(3.3f, 4.3f, 5.3f);
00177   v1 += v3;
00178   BOOST_CHECK(v1.equal(v2 + v3));
00179   v1 -= v3;
00180   BOOST_CHECK(v1.equal(v2));
00181   v1 -= v3;
00182   BOOST_CHECK(v1.equal(v2 - v3));
00183   v1 += v3;
00184 
00185   v1 *= v3;
00186   BOOST_CHECK(v1.equal(v2 * v3));
00187   v1 /= v3;
00188   BOOST_CHECK(v1.equal(v2));
00189   v1 /= v3;
00190   BOOST_CHECK(v1.equal(v2 / v3));
00191   v1 *= v3;
00192 
00193   v1 *= 2.0f;
00194   BOOST_CHECK(v1.equal(v2 * 2.0f));
00195   v1 /= 2.0f;
00196   BOOST_CHECK(v1.equal(v2));
00197   v1 /= 2.0f;
00198   BOOST_CHECK(v1.equal(v2 / 2.0f));
00199   v1 *= 2.0f;
00200 
00201   v1 += 2.0f;
00202   BOOST_CHECK(v1.equal(v2 + 2.0f));
00203   v1 -= 2.0f;
00204   BOOST_CHECK(v1.equal(v2));
00205   v1 -= 2.0f;
00206   BOOST_CHECK(v1.equal(v2 - 2.0f));
00207   v1 += 2.0f;
00208   
00209   BOOST_CHECK((-Vec3f32(1.0f, 2.0f, 3.0f)).equal(Vec3f32(-1.0f, -2.0f, -3.0f)));
00210 
00211   v1 = Vec3f32(1.0f, 2.0f, 3.0f);
00212   v2 = Vec3f32(3.0f, 4.0f, 5.0f);
00213   BOOST_CHECK((v1.cross(v2)).equal(Vec3f32(-2.0f, 4.0f, -2.0f)));
00214   BOOST_CHECK(abs(v1.dot(v2) - 26) < 1e-5);
00215 
00216   v1 = Vec3f32(3.0f, 4.0f, 5.0f);
00217   BOOST_CHECK(abs(v1.sqrLength() - 50) < 1e-5);
00218   BOOST_CHECK(abs(v1.length() - sqrt(50)) < 1e-5);
00219   BOOST_CHECK(normalize(v1).equal(v1 / v1.length()));
00220 }
00221 
00222 BOOST_AUTO_TEST_CASE(vec_test_sse_vec64)
00223 {
00224   typedef Vec3fX<details::sse_meta_d4> Vec3f64;
00225   Vec3f64 v1(1.0, 2.0, 3.0);
00226   BOOST_CHECK(v1[0] == 1.0);
00227   BOOST_CHECK(v1[1] == 2.0);
00228   BOOST_CHECK(v1[2] == 3.0);
00229 
00230   Vec3f64 v2 = v1;
00231   Vec3f64 v3(3.3, 4.3, 5.3);
00232   v1 += v3;
00233   BOOST_CHECK(v1.equal(v2 + v3));
00234   v1 -= v3;
00235   BOOST_CHECK(v1.equal(v2));
00236   v1 -= v3;
00237   BOOST_CHECK(v1.equal(v2 - v3));
00238   v1 += v3;
00239 
00240   v1 *= v3;
00241   BOOST_CHECK(v1.equal(v2 * v3));
00242   v1 /= v3;
00243   BOOST_CHECK(v1.equal(v2));
00244   v1 /= v3;
00245   BOOST_CHECK(v1.equal(v2 / v3));
00246   v1 *= v3;
00247 
00248   v1 *= 2.0;
00249   BOOST_CHECK(v1.equal(v2 * 2.0));
00250   v1 /= 2.0;
00251   BOOST_CHECK(v1.equal(v2));
00252   v1 /= 2.0;
00253   BOOST_CHECK(v1.equal(v2 / 2.0));
00254   v1 *= 2.0;
00255 
00256   v1 += 2.0;
00257   BOOST_CHECK(v1.equal(v2 + 2.0));
00258   v1 -= 2.0;
00259   BOOST_CHECK(v1.equal(v2));
00260   v1 -= 2.0;
00261   BOOST_CHECK(v1.equal(v2 - 2.0));
00262   v1 += 2.0;
00263 
00264   BOOST_CHECK((-Vec3f64(1.0, 2.0, 3.0)).equal(Vec3f64(-1.0, -2.0, -3.0)));
00265 
00266   v1 = Vec3f64(1.0, 2.0, 3.0);
00267   v2 = Vec3f64(3.0, 4.0, 5.0);
00268   BOOST_CHECK((v1.cross(v2)).equal(Vec3f64(-2.0, 4.0, -2.0)));
00269   BOOST_CHECK(abs(v1.dot(v2) - 26) < 1e-5);
00270 
00271   v1 = Vec3f64(3.0, 4.0, 5.0);
00272   BOOST_CHECK(abs(v1.sqrLength() - 50) < 1e-5);
00273   BOOST_CHECK(abs(v1.length() - sqrt(50)) < 1e-5);
00274   BOOST_CHECK(normalize(v1).equal(v1 / v1.length()));
00275 
00276 
00277   v1 = Vec3f64(1.0, 2.0, 3.0);
00278   v2 = Vec3f64(3.0, 4.0, 5.0);
00279   BOOST_CHECK((v1.cross(v2)).equal(Vec3f64(-2.0, 4.0, -2.0)));
00280   BOOST_CHECK(v1.dot(v2) == 26);
00281 }
00282 
00283 BOOST_AUTO_TEST_CASE(sse_mat32_consistent)
00284 {
00285   typedef Vec3fX<details::Vec3Data<float> > Vec3f32;
00286   typedef Vec3fX<details::sse_meta_f4> Vec3f32SSE;
00287 
00288   typedef Matrix3fX<details::Matrix3Data<float> > Matrix3f32;
00289   typedef Matrix3fX<details::sse_meta_f12> Matrix3f32SSE;
00290 
00291   Vec3f32 v1(1, 2, 3);
00292   Vec3f32SSE v2(1, 2, 3);
00293 
00294   Matrix3f32 m1(-1, 3, -3, 0, -6, 6, -5, -3, 1);
00295   Matrix3f32SSE m2(-1, 3, -3, 0, -6, 6, -5, -3, 1);
00296 
00297   for(size_t i = 0; i < 3; ++i)
00298     for(size_t j = 0; j < 3; ++j)
00299       BOOST_CHECK((m1(i, j) - m2(i, j) < 1e-1));
00300   
00301   Matrix3f32 m3(transpose(m1));
00302   Matrix3f32SSE m4(transpose(m2));
00303         
00304   for(size_t i = 0; i < 3; ++i)
00305     for(size_t j = 0; j < 3; ++j)
00306       BOOST_CHECK((m3(i, j) - m4(i, j) < 1e-1));
00307 
00308   m3 = m1; m3.transpose();
00309   m4 = m2; m4.transpose();
00310 
00311   for(size_t i = 0; i < 3; ++i)
00312     for(size_t j = 0; j < 3; ++j)
00313       BOOST_CHECK((m3(i, j) - m4(i, j) < 1e-1));
00314 
00315   m3 = inverse(m1);
00316   m4 = inverse(m2);
00317   
00318   for(size_t i = 0; i < 3; ++i)
00319     for(size_t j = 0; j < 3; ++j)
00320       BOOST_CHECK((m3(i, j) - m4(i, j) < 1e-1));
00321 
00322   m3 = m1; m3.inverse();
00323   m4 = m2; m4.inverse();
00324 
00325   for(size_t i = 0; i < 3; ++i)
00326     for(size_t j = 0; j < 3; ++j)
00327       BOOST_CHECK((m3(i, j) - m4(i, j) < 1e-1));
00328 }
00329 
00330 BOOST_AUTO_TEST_CASE(vec_test_sse_vec32_consistent)
00331 {
00332   typedef Vec3fX<details::Vec3Data<float> > Vec3f32;
00333   typedef Vec3fX<details::sse_meta_f4> Vec3f32SSE;
00334 
00335   Vec3f32 v1(3.4f, 4.2f, 10.5f), v2(3.1f, 0.1f, -50.4f);
00336   Vec3f32SSE v3(3.4f, 4.2f, 10.5f), v4(3.1f, 0.1f, -50.4f);
00337   Vec3f32 v12 = v1 + v2;
00338   Vec3f32SSE v34 = v3 + v4;
00339   BOOST_CHECK(abs(v12[0] - v34[0]) < 1e-5);
00340   BOOST_CHECK(abs(v12[1] - v34[1]) < 1e-5);
00341   BOOST_CHECK(abs(v12[2] - v34[2]) < 1e-5);
00342   v12 = v1 - v2;
00343   v34 = v3 - v4;
00344   BOOST_CHECK(abs(v12[0] - v34[0]) < 1e-5);
00345   BOOST_CHECK(abs(v12[1] - v34[1]) < 1e-5);
00346   BOOST_CHECK(abs(v12[2] - v34[2]) < 1e-5);
00347   v12 = v1 * v2;
00348   v34 = v3 * v4;
00349   BOOST_CHECK(abs(v12[0] - v34[0]) < 1e-5);
00350   BOOST_CHECK(abs(v12[1] - v34[1]) < 1e-5);
00351   BOOST_CHECK(abs(v12[2] - v34[2]) < 1e-5);
00352   v12 = v1 / v2;
00353   v34 = v3 / v4;
00354   BOOST_CHECK(abs(v12[0] - v34[0]) < 1e-5);
00355   BOOST_CHECK(abs(v12[1] - v34[1]) < 1e-5);
00356   BOOST_CHECK(abs(v12[2] - v34[2]) < 1e-5);
00357   float t = 1234.433f;
00358   v12 = v1 + t;
00359   v34 = v3 + t;
00360   BOOST_CHECK(abs(v12[0] - v34[0]) < 1e-5);
00361   BOOST_CHECK(abs(v12[1] - v34[1]) < 1e-5);
00362   BOOST_CHECK(abs(v12[2] - v34[2]) < 1e-5);
00363   v12 = v1 - t;
00364   v34 = v3 - t;
00365   BOOST_CHECK(abs(v12[0] - v34[0]) < 1e-5);
00366   BOOST_CHECK(abs(v12[1] - v34[1]) < 1e-5);
00367   BOOST_CHECK(abs(v12[2] - v34[2]) < 1e-5);
00368   v12 = v1 * t;
00369   v34 = v3 * t;
00370   BOOST_CHECK(abs(v12[0] - v34[0]) < 1e-5);
00371   BOOST_CHECK(abs(v12[1] - v34[1]) < 1e-5);
00372   BOOST_CHECK(abs(v12[2] - v34[2]) < 1e-5);
00373   v12 = v1 / t;
00374   v34 = v3 / t;
00375   BOOST_CHECK(abs(v12[0] - v34[0]) < 1e-5);
00376   BOOST_CHECK(abs(v12[1] - v34[1]) < 1e-5);
00377   BOOST_CHECK(abs(v12[2] - v34[2]) < 1e-5);
00378 
00379   v12 = v1; v12 += v2;
00380   v34 = v3; v34 += v4;
00381   BOOST_CHECK(abs(v12[0] - v34[0]) < 1e-5);
00382   BOOST_CHECK(abs(v12[1] - v34[1]) < 1e-5);
00383   BOOST_CHECK(abs(v12[2] - v34[2]) < 1e-5);
00384   v12 = v1; v12 -= v2;
00385   v34 = v3; v34 -= v4;
00386   BOOST_CHECK(abs(v12[0] - v34[0]) < 1e-5);
00387   BOOST_CHECK(abs(v12[1] - v34[1]) < 1e-5);
00388   BOOST_CHECK(abs(v12[2] - v34[2]) < 1e-5);
00389   v12 = v1; v12 *= v2;
00390   v34 = v3; v34 *= v4;
00391   BOOST_CHECK(abs(v12[0] - v34[0]) < 1e-5);
00392   BOOST_CHECK(abs(v12[1] - v34[1]) < 1e-5);
00393   BOOST_CHECK(abs(v12[2] - v34[2]) < 1e-5);
00394   v12 = v1; v12 /= v2;
00395   v34 = v3; v34 /= v4;
00396   BOOST_CHECK(abs(v12[0] - v34[0]) < 1e-5);
00397   BOOST_CHECK(abs(v12[1] - v34[1]) < 1e-5);
00398   BOOST_CHECK(abs(v12[2] - v34[2]) < 1e-5);
00399   v12 = v1; v12 += t;
00400   v34 = v3; v34 += t;
00401   BOOST_CHECK(abs(v12[0] - v34[0]) < 1e-5);
00402   BOOST_CHECK(abs(v12[1] - v34[1]) < 1e-5);
00403   BOOST_CHECK(abs(v12[2] - v34[2]) < 1e-5);
00404   v12 = v1; v12 -= t;
00405   v34 = v3; v34 -= t;
00406   BOOST_CHECK(abs(v12[0] - v34[0]) < 1e-5);
00407   BOOST_CHECK(abs(v12[1] - v34[1]) < 1e-5);
00408   BOOST_CHECK(abs(v12[2] - v34[2]) < 1e-5);
00409   v12 = v1; v12 *= t;
00410   v34 = v3; v34 *= t;
00411   BOOST_CHECK(abs(v12[0] - v34[0]) < 1e-5);
00412   BOOST_CHECK(abs(v12[1] - v34[1]) < 1e-5);
00413   BOOST_CHECK(abs(v12[2] - v34[2]) < 1e-5);
00414   v12 = v1; v12 /= t;
00415   v34 = v3; v34 /= t;
00416   BOOST_CHECK(abs(v12[0] - v34[0]) < 1e-5);
00417   BOOST_CHECK(abs(v12[1] - v34[1]) < 1e-5);
00418   BOOST_CHECK(abs(v12[2] - v34[2]) < 1e-5);
00419 
00420   v12 = -v1;
00421   v34 = -v3;
00422   BOOST_CHECK(abs(v12[0] - v34[0]) < 1e-5);
00423   BOOST_CHECK(abs(v12[1] - v34[1]) < 1e-5);
00424   BOOST_CHECK(abs(v12[2] - v34[2]) < 1e-5);
00425 
00426   v12 = v1.cross(v2);
00427   v34 = v3.cross(v4);
00428   BOOST_CHECK(abs(v12[0] - v34[0]) < 1e-5);
00429   BOOST_CHECK(abs(v12[1] - v34[1]) < 1e-5);
00430   BOOST_CHECK(abs(v12[2] - v34[2]) < 1e-5);
00431 
00432   BOOST_CHECK(abs(v1.dot(v2) - v3.dot(v4)) < 1e-5);
00433 
00434   v12 = min(v1, v2);
00435   v34 = min(v3, v4);
00436   BOOST_CHECK(abs(v12[0] - v34[0]) < 1e-5);
00437   BOOST_CHECK(abs(v12[1] - v34[1]) < 1e-5);
00438   BOOST_CHECK(abs(v12[2] - v34[2]) < 1e-5);
00439   v12 = max(v1, v2);
00440   v34 = max(v3, v4);
00441   BOOST_CHECK(abs(v12[0] - v34[0]) < 1e-5);
00442   BOOST_CHECK(abs(v12[1] - v34[1]) < 1e-5);
00443   BOOST_CHECK(abs(v12[2] - v34[2]) < 1e-5);
00444 
00445   v12 = abs(v2);
00446   v34 = abs(v4);
00447   BOOST_CHECK(abs(v12[0] - v34[0]) < 1e-5);
00448   BOOST_CHECK(abs(v12[1] - v34[1]) < 1e-5);
00449   BOOST_CHECK(abs(v12[2] - v34[2]) < 1e-5);
00450 
00451   Vec3f32 delta1(1e-9f, 1e-9f, 1e-9f);
00452   Vec3f32SSE delta2(1e-9f, 1e-9f, 1e-9f);
00453   BOOST_CHECK((v1 + delta1).equal(v1));
00454   BOOST_CHECK((v3 + delta2).equal(v3));
00455 
00456   BOOST_CHECK(abs(v1.length() - v3.length()) < 1e-5);
00457   BOOST_CHECK(abs(v1.sqrLength() - v3.sqrLength()) < 1e-5);
00458  
00459   v12 = v1; v12.negate();
00460   v34 = v3; v34.negate();
00461   BOOST_CHECK(abs(v12[0] - v34[0]) < 1e-5);
00462   BOOST_CHECK(abs(v12[1] - v34[1]) < 1e-5);
00463   BOOST_CHECK(abs(v12[2] - v34[2]) < 1e-5);
00464 
00465   v12 = v1; v12.normalize();
00466   v34 = v3; v34.normalize();
00467   BOOST_CHECK(abs(v12[0] - v34[0]) < 1e-5);
00468   BOOST_CHECK(abs(v12[1] - v34[1]) < 1e-5);
00469   BOOST_CHECK(abs(v12[2] - v34[2]) < 1e-5);
00470   
00471   v12 = normalize(v1);
00472   v34 = normalize(v3);
00473   BOOST_CHECK(abs(v12[0] - v34[0]) < 1e-5);
00474   BOOST_CHECK(abs(v12[1] - v34[1]) < 1e-5);
00475   BOOST_CHECK(abs(v12[2] - v34[2]) < 1e-5);  
00476 }
00477 
00478 BOOST_AUTO_TEST_CASE(vec_test_sse_vec64_consistent)
00479 {
00480   typedef Vec3fX<details::Vec3Data<double> > Vec3f64;
00481   typedef Vec3fX<details::sse_meta_d4> Vec3f64SSE;
00482 
00483   Vec3f64 v1(3.4, 4.2, 10.5), v2(3.1, 0.1, -50.4);
00484   Vec3f64SSE v3(3.4, 4.2, 10.5), v4(3.1, 0.1, -50.4);
00485   Vec3f64 v12 = v1 + v2;
00486   Vec3f64SSE v34 = v3 + v4;
00487   BOOST_CHECK(abs(v12[0] - v34[0]) < 1e-5);
00488   BOOST_CHECK(abs(v12[1] - v34[1]) < 1e-5);
00489   BOOST_CHECK(abs(v12[2] - v34[2]) < 1e-5);
00490   v12 = v1 - v2;
00491   v34 = v3 - v4;
00492   BOOST_CHECK(abs(v12[0] - v34[0]) < 1e-5);
00493   BOOST_CHECK(abs(v12[1] - v34[1]) < 1e-5);
00494   BOOST_CHECK(abs(v12[2] - v34[2]) < 1e-5);
00495   v12 = v1 * v2;
00496   v34 = v3 * v4;
00497   BOOST_CHECK(abs(v12[0] - v34[0]) < 1e-5);
00498   BOOST_CHECK(abs(v12[1] - v34[1]) < 1e-5);
00499   BOOST_CHECK(abs(v12[2] - v34[2]) < 1e-5);
00500   v12 = v1 / v2;
00501   v34 = v3 / v4;
00502   BOOST_CHECK(abs(v12[0] - v34[0]) < 1e-5);
00503   BOOST_CHECK(abs(v12[1] - v34[1]) < 1e-5);
00504   BOOST_CHECK(abs(v12[2] - v34[2]) < 1e-5);
00505   double t = 1234.433;
00506   v12 = v1 + t;
00507   v34 = v3 + t;
00508   BOOST_CHECK(abs(v12[0] - v34[0]) < 1e-5);
00509   BOOST_CHECK(abs(v12[1] - v34[1]) < 1e-5);
00510   BOOST_CHECK(abs(v12[2] - v34[2]) < 1e-5);
00511   v12 = v1 - t;
00512   v34 = v3 - t;
00513   BOOST_CHECK(abs(v12[0] - v34[0]) < 1e-5);
00514   BOOST_CHECK(abs(v12[1] - v34[1]) < 1e-5);
00515   BOOST_CHECK(abs(v12[2] - v34[2]) < 1e-5);
00516   v12 = v1 * t;
00517   v34 = v3 * t;
00518   BOOST_CHECK(abs(v12[0] - v34[0]) < 1e-5);
00519   BOOST_CHECK(abs(v12[1] - v34[1]) < 1e-5);
00520   BOOST_CHECK(abs(v12[2] - v34[2]) < 1e-5);
00521   v12 = v1 / t;
00522   v34 = v3 / t;
00523   BOOST_CHECK(abs(v12[0] - v34[0]) < 1e-5);
00524   BOOST_CHECK(abs(v12[1] - v34[1]) < 1e-5);
00525   BOOST_CHECK(abs(v12[2] - v34[2]) < 1e-5);
00526 
00527   v12 = v1; v12 += v2;
00528   v34 = v3; v34 += v4;
00529   BOOST_CHECK(abs(v12[0] - v34[0]) < 1e-5);
00530   BOOST_CHECK(abs(v12[1] - v34[1]) < 1e-5);
00531   BOOST_CHECK(abs(v12[2] - v34[2]) < 1e-5);
00532   v12 = v1; v12 -= v2;
00533   v34 = v3; v34 -= v4;
00534   BOOST_CHECK(abs(v12[0] - v34[0]) < 1e-5);
00535   BOOST_CHECK(abs(v12[1] - v34[1]) < 1e-5);
00536   BOOST_CHECK(abs(v12[2] - v34[2]) < 1e-5);
00537   v12 = v1; v12 *= v2;
00538   v34 = v3; v34 *= v4;
00539   BOOST_CHECK(abs(v12[0] - v34[0]) < 1e-5);
00540   BOOST_CHECK(abs(v12[1] - v34[1]) < 1e-5);
00541   BOOST_CHECK(abs(v12[2] - v34[2]) < 1e-5);
00542   v12 = v1; v12 /= v2;
00543   v34 = v3; v34 /= v4;
00544   BOOST_CHECK(abs(v12[0] - v34[0]) < 1e-5);
00545   BOOST_CHECK(abs(v12[1] - v34[1]) < 1e-5);
00546   BOOST_CHECK(abs(v12[2] - v34[2]) < 1e-5);
00547   v12 = v1; v12 += t;
00548   v34 = v3; v34 += t;
00549   BOOST_CHECK(abs(v12[0] - v34[0]) < 1e-5);
00550   BOOST_CHECK(abs(v12[1] - v34[1]) < 1e-5);
00551   BOOST_CHECK(abs(v12[2] - v34[2]) < 1e-5);
00552   v12 = v1; v12 -= t;
00553   v34 = v3; v34 -= t;
00554   BOOST_CHECK(abs(v12[0] - v34[0]) < 1e-5);
00555   BOOST_CHECK(abs(v12[1] - v34[1]) < 1e-5);
00556   BOOST_CHECK(abs(v12[2] - v34[2]) < 1e-5);
00557   v12 = v1; v12 *= t;
00558   v34 = v3; v34 *= t;
00559   BOOST_CHECK(abs(v12[0] - v34[0]) < 1e-5);
00560   BOOST_CHECK(abs(v12[1] - v34[1]) < 1e-5);
00561   BOOST_CHECK(abs(v12[2] - v34[2]) < 1e-5);
00562   v12 = v1; v12 /= t;
00563   v34 = v3; v34 /= t;
00564   BOOST_CHECK(abs(v12[0] - v34[0]) < 1e-5);
00565   BOOST_CHECK(abs(v12[1] - v34[1]) < 1e-5);
00566   BOOST_CHECK(abs(v12[2] - v34[2]) < 1e-5);
00567 
00568   v12 = -v1;
00569   v34 = -v3;
00570   BOOST_CHECK(abs(v12[0] - v34[0]) < 1e-5);
00571   BOOST_CHECK(abs(v12[1] - v34[1]) < 1e-5);
00572   BOOST_CHECK(abs(v12[2] - v34[2]) < 1e-5);
00573 
00574   v12 = v1.cross(v2);
00575   v34 = v3.cross(v4);
00576   BOOST_CHECK(abs(v12[0] - v34[0]) < 1e-5);
00577   BOOST_CHECK(abs(v12[1] - v34[1]) < 1e-5);
00578   BOOST_CHECK(abs(v12[2] - v34[2]) < 1e-5);
00579 
00580   BOOST_CHECK(abs(v1.dot(v2) - v3.dot(v4)) < 1e-5);
00581 
00582   v12 = min(v1, v2);
00583   v34 = min(v3, v4);
00584   BOOST_CHECK(abs(v12[0] - v34[0]) < 1e-5);
00585   BOOST_CHECK(abs(v12[1] - v34[1]) < 1e-5);
00586   BOOST_CHECK(abs(v12[2] - v34[2]) < 1e-5);
00587   v12 = max(v1, v2);
00588   v34 = max(v3, v4);
00589   BOOST_CHECK(abs(v12[0] - v34[0]) < 1e-5);
00590   BOOST_CHECK(abs(v12[1] - v34[1]) < 1e-5);
00591   BOOST_CHECK(abs(v12[2] - v34[2]) < 1e-5);
00592 
00593   v12 = abs(v2);
00594   v34 = abs(v4);
00595   BOOST_CHECK(abs(v12[0] - v34[0]) < 1e-5);
00596   BOOST_CHECK(abs(v12[1] - v34[1]) < 1e-5);
00597   BOOST_CHECK(abs(v12[2] - v34[2]) < 1e-5);
00598 
00599   Vec3f64 delta1(1e-15, 1e-15, 1e-15);
00600   Vec3f64SSE delta2(1e-15, 1e-15, 1e-15);
00601   BOOST_CHECK((v1 + delta1).equal(v1));
00602   BOOST_CHECK((v3 + delta2).equal(v3));
00603 
00604   BOOST_CHECK(abs(v1.length() - v3.length()) < 1e-5);
00605   BOOST_CHECK(abs(v1.sqrLength() - v3.sqrLength()) < 1e-5);
00606  
00607   v12 = v1; v12.negate();
00608   v34 = v3; v34.negate();
00609   BOOST_CHECK(abs(v12[0] - v34[0]) < 1e-5);
00610   BOOST_CHECK(abs(v12[1] - v34[1]) < 1e-5);
00611   BOOST_CHECK(abs(v12[2] - v34[2]) < 1e-5);
00612 
00613   v12 = v1; v12.normalize();
00614   v34 = v3; v34.normalize();
00615   BOOST_CHECK(abs(v12[0] - v34[0]) < 1e-5);
00616   BOOST_CHECK(abs(v12[1] - v34[1]) < 1e-5);
00617   BOOST_CHECK(abs(v12[2] - v34[2]) < 1e-5);
00618   
00619   v12 = normalize(v1);
00620   v34 = normalize(v3);
00621   BOOST_CHECK(abs(v12[0] - v34[0]) < 1e-5);
00622   BOOST_CHECK(abs(v12[1] - v34[1]) < 1e-5);
00623   BOOST_CHECK(abs(v12[2] - v34[2]) < 1e-5);  
00624 }
00625 
00626 #endif
00627 
00628 BOOST_AUTO_TEST_CASE(morton)
00629 {
00630   AABB bbox(Vec3f(0, 0, 0), Vec3f(1000, 1000, 1000));
00631   morton_functor<boost::dynamic_bitset<> > F1(bbox, 10);
00632   morton_functor<boost::dynamic_bitset<> > F2(bbox, 20);
00633   morton_functor<FCL_UINT64> F3(bbox);
00634   morton_functor<FCL_UINT32> F4(bbox);
00635 
00636   Vec3f p(254, 873, 674);
00637   std::cout << std::hex << F1(p).to_ulong() << std::endl;
00638   std::cout << std::hex << F2(p).to_ulong() << std::endl;
00639   std::cout << std::hex << F3(p) << std::endl;
00640   std::cout << std::hex << F4(p) << std::endl;
00641   
00642 }