bullet_unittest.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  * 
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  * 
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  * 
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include <gtest/gtest.h>
00031 #include <sys/time.h>
00032 #include "LinearMath/btTransform.h"
00033 #include "LinearMath/btTransformUtil.h"
00034 #include "LinearMath/btVector3.h"
00035 
00036 void seed_rand()
00037 {
00038   //Seed random number generator with current microseond count
00039   timeval temp_time_struct;
00040   gettimeofday(&temp_time_struct,NULL);
00041   srand(temp_time_struct.tv_usec);
00042 };
00043 
00044 
00045 TEST(Bullet, QuaternionAngleScaling)
00046 {
00047   double epsilon = 1e-6;
00048 
00049   btQuaternion id(0,0,0,1);
00050   btQuaternion ninty(0,0,sqrt(2.0)/2.0,sqrt(2.0)/2.0);
00051   EXPECT_NEAR(id.angle(ninty), SIMD_PI/4, epsilon); //These two quaternions are 90 degrees apart half angle is 45
00052 
00053   btQuaternion id2(0,0,0);
00054   btQuaternion ninty2(SIMD_PI/2,0,0);
00055   EXPECT_NEAR(id2.angle(ninty2), SIMD_PI/4, epsilon); //These two quaternions are 90 degrees apart half angle is 45
00056 }
00057 TEST(Bullet, QuaternionAngleShortestPath)
00058 {
00059   btQuaternion q1 (SIMD_PI/4,0,0);
00060   btQuaternion q2(-q1.x(), -q1.y(), -q1.z()-.0001, -q1.w()+.0001);
00061   q2.normalize();
00062   //  printf("%f %f %f %f,%f %f %f %f\n", q2.x(), q2.y(), q2.z(), q2.w(), q3.x(), q3.y(), q3.z(), q3.w());
00063 
00064   EXPECT_NEAR(q2.angleShortestPath(q1), 0, 0.01); //These two quaternions are basically the same but expressed on the other side of quaternion space
00065 }
00066 
00067 TEST(Bullet, QuaternionAngleIdentityShortestPath)
00068 {
00069   btQuaternion q1 (SIMD_PI/4,0,0);
00070   btQuaternion q2(-q1.x(), -q1.y(), -q1.z(), -q1.w());
00071   q2.normalize();
00072   //  printf("%f %f %f %f,%f %f %f %f\n", q2.x(), q2.y(), q2.z(), q2.w(), q3.x(), q3.y(), q3.z(), q3.w());
00073 
00074   EXPECT_NEAR(q2.angleShortestPath(q1), 0, 0.01); //These two quaternions are basically the same but expressed on the other side of quaternion space
00075   EXPECT_NEAR((q2*(q1.inverse())).getAngleShortestPath(), 0, 0.01); //These two quaternions are basically the same but expressed on the other side of quaternion space
00076   btQuaternion q3(-q1.x(), -q1.y(), -q1.z(), -q1.w());
00077   q3.normalize();
00078   //  printf("%f %f %f %f,%f %f %f %f\n", q2.x(), q2.y(), q2.z(), q2.w(), q3.x(), q3.y(), q3.z(), q3.w());
00079 
00080   EXPECT_NEAR(q3.angleShortestPath(q1), 0, 0.01); //These two quaternions are basically the same but expressed on the other side of quaternion space
00081   EXPECT_NEAR((q3*(q1.inverse())).getAngleShortestPath(), 0, 0.01); //These two quaternions are basically the same but expressed on the other side of quaternion space
00082   
00083 }
00084 
00085 TEST(Bullet, AngleQuaternionQuaternionShortestPath)
00086 {
00087   btQuaternion q1 (SIMD_PI/4,0,0);
00088   btQuaternion q2(-q1.x(), -q1.y(), -q1.z()-.0001, -q1.w()+.0001);
00089   q2.normalize();
00090 
00091   //  printf("%f %f %f %f,%f %f %f %f\n", q2.x(), q2.y(), q2.z(), q2.w(), q3.x(), q3.y(), q3.z(), q3.w());
00092 
00093   EXPECT_NEAR(angleShortestPath(q1, q2), 0, 0.01); //These two quaternions are basically the same but expressed on the other side of quaternion space
00094 }
00095 
00096 TEST(Bullet, EulerConventionsYPR)
00097 {
00098   double epsilon = 1e-6;
00099   double yaw, pitch, roll;
00100 
00114   //Yaw by 90 
00115   yaw = M_PI/2;
00116   pitch = 0;
00117   roll = 0;
00118   
00119   btQuaternion q(0,0,0,1);
00120   btVector3 v(1,0,0);
00121   q.setEulerZYX(yaw, pitch,roll);
00122   btTransform tr(q, btVector3(0,0,0));
00123   
00124   btVector3 v2 = tr * v;
00125 
00126   printf("%f, %f, %f: yaw by %f -> %f, %f, %f\n", v.x(), v.y(), v.z(), yaw, v2.x(), v2.y(), v2.z());
00127   EXPECT_NEAR(v2.x(), 0,epsilon); 
00128   EXPECT_NEAR(v2.y(), 1,epsilon); 
00129   EXPECT_NEAR(v2.z(), 0,epsilon); 
00130 
00131   btMatrix3x3 mat;
00132   mat.setEulerZYX(yaw, pitch,roll);
00133   tr = btTransform(mat, btVector3(0,0,0));
00134   v2 = tr * v;
00135 
00136   printf("%f, %f, %f: yaw by %f -> %f, %f, %f\n", v.x(), v.y(), v.z(), yaw, v2.x(), v2.y(), v2.z());
00137   EXPECT_NEAR(v2.x(), 0,epsilon); 
00138   EXPECT_NEAR(v2.y(), 1,epsilon); 
00139   EXPECT_NEAR(v2.z(), 0,epsilon); 
00140                  
00141   mat.setEulerYPR(yaw, pitch,roll);
00142   tr = btTransform(mat, btVector3(0,0,0));
00143   v2 = tr * v;
00144 
00145   printf("%f, %f, %f: yaw by %f -> %f, %f, %f\n", v.x(), v.y(), v.z(), yaw, v2.x(), v2.y(), v2.z());
00146   EXPECT_NEAR(v2.x(), 0,epsilon); 
00147   EXPECT_NEAR(v2.y(), 1,epsilon); 
00148   EXPECT_NEAR(v2.z(), 0,epsilon); 
00149                  
00150   //Pitch by 90
00151   yaw = 0;
00152   pitch = M_PI/2;
00153   roll = 0;
00154   
00155   q = btQuaternion(0,0,0);
00156   v = btVector3(1,0,0);
00157   tr = btTransform(btQuaternion(yaw, pitch,roll), btVector3(0,0,0));
00158   
00159   v2 = tr * v;
00160 
00161   printf("%f, %f, %f: pitch by %f -> %f, %f, %f\n", v.x(), v.y(), v.z(), pitch, v2.x(), v2.y(), v2.z());
00162   EXPECT_NEAR(v2.x(), 0,epsilon); 
00163   EXPECT_NEAR(v2.y(), 0,epsilon); 
00164   EXPECT_NEAR(v2.z(), -1,epsilon); 
00165                  
00166   //Roll by 90
00167   yaw = 0;
00168   pitch = 0;
00169   roll = M_PI/2;
00170   
00171   q = btQuaternion(0,0,0);
00172   v = btVector3(0,1,0);
00173   tr = btTransform(btQuaternion(yaw, pitch,roll), btVector3(0,0,0));
00174   
00175   v2 = tr * v;
00176 
00177   printf("%f, %f, %f: roll by %f -> %f, %f, %f\n", v.x(), v.y(), v.z(), roll, v2.x(), v2.y(), v2.z());
00178   EXPECT_NEAR(v2.x(), 0,epsilon); 
00179   EXPECT_NEAR(v2.y(), 0,epsilon); 
00180   EXPECT_NEAR(v2.z(), 1,epsilon); 
00181                  
00182   //Yaw and Roll by 90
00183   yaw = M_PI/2;
00184   pitch = 0;
00185   roll = M_PI/2;
00186   
00187   q = btQuaternion(0,0,0);
00188   v = btVector3(1,0,0);
00189   tr = btTransform(btQuaternion(yaw, pitch,roll), btVector3(0,0,0));
00190   
00191   v2 = tr * v;
00192 
00193   printf("%f, %f, %f: yaw and roll by %f -> %f, %f, %f\n", v.x(), v.y(), v.z(), roll, v2.x(), v2.y(), v2.z());
00194   EXPECT_NEAR(v2.x(), 0,epsilon); 
00195   EXPECT_NEAR(v2.y(), 1,epsilon); 
00196   EXPECT_NEAR(v2.z(), 0,epsilon); 
00197 
00198   q = btQuaternion(0,0,0);
00199   v = btVector3(0,1,0);
00200   tr = btTransform(btQuaternion(yaw, pitch,roll), btVector3(0,0,0));
00201   
00202   v2 = tr * v;
00203 
00204   printf("%f, %f, %f: yaw and roll by %f -> %f, %f, %f\n", v.x(), v.y(), v.z(), roll, v2.x(), v2.y(), v2.z());
00205   EXPECT_NEAR(v2.x(), 0,epsilon); 
00206   EXPECT_NEAR(v2.y(), 0,epsilon); 
00207   EXPECT_NEAR(v2.z(), 1,epsilon); 
00208 
00209   q = btQuaternion(0,0,0);
00210   v = btVector3(0,0,1);
00211   tr = btTransform(btQuaternion(yaw, pitch,roll), btVector3(0,0,0));
00212   
00213   v2 = tr * v;
00214 
00215   printf("%f, %f, %f: yaw and roll by %f -> %f, %f, %f\n", v.x(), v.y(), v.z(), roll, v2.x(), v2.y(), v2.z());
00216   EXPECT_NEAR(v2.x(), 1,epsilon); 
00217   EXPECT_NEAR(v2.y(), 0,epsilon); 
00218   EXPECT_NEAR(v2.z(), 0,epsilon); 
00219 
00220   //Yaw and Pitch
00221   yaw = M_PI/2;
00222   pitch = M_PI/2;
00223   roll = 0;
00224   
00225   q = btQuaternion(0,0,0);
00226   v = btVector3(1,0,0);
00227   tr = btTransform(btQuaternion(yaw, pitch,roll), btVector3(0,0,0));
00228   
00229   v2 = tr * v;
00230 
00231   printf("%f, %f, %f: yaw and pitch by %f -> %f, %f, %f\n", v.x(), v.y(), v.z(), pitch, v2.x(), v2.y(), v2.z());
00232   EXPECT_NEAR(v2.x(), 0,epsilon); 
00233   EXPECT_NEAR(v2.y(), 0,epsilon); 
00234   EXPECT_NEAR(v2.z(), -1,epsilon); 
00235 
00236   q = btQuaternion(0,0,0);
00237   v = btVector3(0,1,0);
00238   tr = btTransform(btQuaternion(yaw, pitch,roll), btVector3(0,0,0));
00239   
00240   v2 = tr * v;
00241 
00242   printf("%f, %f, %f: yaw and pitch by %f -> %f, %f, %f\n", v.x(), v.y(), v.z(), pitch, v2.x(), v2.y(), v2.z());
00243   EXPECT_NEAR(v2.x(), -1,epsilon); 
00244   EXPECT_NEAR(v2.y(), 0,epsilon); 
00245   EXPECT_NEAR(v2.z(), 0,epsilon); 
00246 
00247   q = btQuaternion(0,0,0);
00248   v = btVector3(0,0,1);
00249   tr = btTransform(btQuaternion(yaw, pitch,roll), btVector3(0,0,0));
00250   
00251   v2 = tr * v;
00252 
00253   printf("%f, %f, %f: yaw and pitch by %f -> %f, %f, %f\n", v.x(), v.y(), v.z(), pitch, v2.x(), v2.y(), v2.z());
00254   EXPECT_NEAR(v2.x(), 0,epsilon); 
00255   EXPECT_NEAR(v2.y(), 1,epsilon); 
00256   EXPECT_NEAR(v2.z(), 0,epsilon); 
00257 
00258   //Pitch and Roll
00259   yaw = 0;
00260   pitch = M_PI/2;
00261   roll = M_PI/2;
00262   
00263   q = btQuaternion(0,0,0);
00264   v = btVector3(1,0,0);
00265   tr = btTransform(btQuaternion(yaw, pitch,roll), btVector3(0,0,0));
00266   
00267   v2 = tr * v;
00268 
00269   printf("%f, %f, %f: pitch and roll by %f -> %f, %f, %f\n", v.x(), v.y(), v.z(), pitch, v2.x(), v2.y(), v2.z());
00270   EXPECT_NEAR(v2.x(), 0,epsilon); 
00271   EXPECT_NEAR(v2.y(), 0,epsilon); 
00272   EXPECT_NEAR(v2.z(), -1,epsilon); 
00273 
00274   q = btQuaternion(0,0,0);
00275   v = btVector3(0,1,0);
00276   tr = btTransform(btQuaternion(yaw, pitch,roll), btVector3(0,0,0));
00277   
00278   v2 = tr * v;
00279 
00280   printf("%f, %f, %f: pitch and roll by %f -> %f, %f, %f\n", v.x(), v.y(), v.z(), pitch, v2.x(), v2.y(), v2.z());
00281   EXPECT_NEAR(v2.x(), 1,epsilon); 
00282   EXPECT_NEAR(v2.y(), 0,epsilon); 
00283   EXPECT_NEAR(v2.z(), 0,epsilon); 
00284 
00285   q = btQuaternion(0,0,0);
00286   v = btVector3(0,0,1);
00287   tr = btTransform(btQuaternion(yaw, pitch,roll), btVector3(0,0,0));
00288   
00289   v2 = tr * v;
00290 
00291   printf("%f, %f, %f: pitch and roll by %f -> %f, %f, %f\n", v.x(), v.y(), v.z(), pitch, v2.x(), v2.y(), v2.z());
00292   EXPECT_NEAR(v2.x(), 0,epsilon); 
00293   EXPECT_NEAR(v2.y(), -1,epsilon); 
00294   EXPECT_NEAR(v2.z(), 0,epsilon); 
00295 };
00296 
00297 TEST(Bullet, EulerConventionsConstructors)
00298 {
00299   double epsilon = 1e-6;
00300   double yaw, pitch, roll;
00301 
00315   //Yaw by 90 
00316   yaw = M_PI/2;
00317   pitch = 0;
00318   roll = 0;
00319   
00320   btQuaternion q(0,0,0);
00321   btVector3 v(1,0,0);
00322   btTransform tr(btQuaternion(yaw, pitch,roll), btVector3(0,0,0));
00323   
00324   btVector3 v2 = tr * v;
00325 
00326   printf("%f, %f, %f: yaw by %f -> %f, %f, %f\n", v.x(), v.y(), v.z(), yaw, v2.x(), v2.y(), v2.z());
00327   EXPECT_NEAR(v2.x(), 0,epsilon); 
00328   EXPECT_NEAR(v2.y(), 1,epsilon); 
00329   EXPECT_NEAR(v2.z(), 0,epsilon); 
00330                  
00331   //Pitch by 90
00332   yaw = 0;
00333   pitch = M_PI/2;
00334   roll = 0;
00335   
00336   q = btQuaternion(0,0,0);
00337   v = btVector3(1,0,0);
00338   tr = btTransform(btQuaternion(yaw, pitch,roll), btVector3(0,0,0));
00339   
00340   v2 = tr * v;
00341 
00342   printf("%f, %f, %f: pitch by %f -> %f, %f, %f\n", v.x(), v.y(), v.z(), pitch, v2.x(), v2.y(), v2.z());
00343   EXPECT_NEAR(v2.x(), 0,epsilon); 
00344   EXPECT_NEAR(v2.y(), 0,epsilon); 
00345   EXPECT_NEAR(v2.z(), -1,epsilon); 
00346                  
00347   //Roll by 90
00348   yaw = 0;
00349   pitch = 0;
00350   roll = M_PI/2;
00351   
00352   q = btQuaternion(0,0,0);
00353   v = btVector3(0,1,0);
00354   tr = btTransform(btQuaternion(yaw, pitch,roll), btVector3(0,0,0));
00355   
00356   v2 = tr * v;
00357 
00358   printf("%f, %f, %f: roll by %f -> %f, %f, %f\n", v.x(), v.y(), v.z(), roll, v2.x(), v2.y(), v2.z());
00359   EXPECT_NEAR(v2.x(), 0,epsilon); 
00360   EXPECT_NEAR(v2.y(), 0,epsilon); 
00361   EXPECT_NEAR(v2.z(), 1,epsilon); 
00362                  
00363   //Yaw and Roll by 90
00364   yaw = M_PI/2;
00365   pitch = 0;
00366   roll = M_PI/2;
00367   
00368   q = btQuaternion(0,0,0);
00369   v = btVector3(1,0,0);
00370   tr = btTransform(btQuaternion(yaw, pitch,roll), btVector3(0,0,0));
00371   
00372   v2 = tr * v;
00373 
00374   printf("%f, %f, %f: yaw and roll by %f -> %f, %f, %f\n", v.x(), v.y(), v.z(), roll, v2.x(), v2.y(), v2.z());
00375   EXPECT_NEAR(v2.x(), 0,epsilon); 
00376   EXPECT_NEAR(v2.y(), 1,epsilon); 
00377   EXPECT_NEAR(v2.z(), 0,epsilon); 
00378 
00379   q = btQuaternion(0,0,0);
00380   v = btVector3(0,1,0);
00381   tr = btTransform(btQuaternion(yaw, pitch,roll), btVector3(0,0,0));
00382   
00383   v2 = tr * v;
00384 
00385   printf("%f, %f, %f: yaw and roll by %f -> %f, %f, %f\n", v.x(), v.y(), v.z(), roll, v2.x(), v2.y(), v2.z());
00386   EXPECT_NEAR(v2.x(), 0,epsilon); 
00387   EXPECT_NEAR(v2.y(), 0,epsilon); 
00388   EXPECT_NEAR(v2.z(), 1,epsilon); 
00389 
00390   q = btQuaternion(0,0,0);
00391   v = btVector3(0,0,1);
00392   tr = btTransform(btQuaternion(yaw, pitch,roll), btVector3(0,0,0));
00393   
00394   v2 = tr * v;
00395 
00396   printf("%f, %f, %f: yaw and roll by %f -> %f, %f, %f\n", v.x(), v.y(), v.z(), roll, v2.x(), v2.y(), v2.z());
00397   EXPECT_NEAR(v2.x(), 1,epsilon); 
00398   EXPECT_NEAR(v2.y(), 0,epsilon); 
00399   EXPECT_NEAR(v2.z(), 0,epsilon); 
00400 
00401   //Yaw and Pitch
00402   yaw = M_PI/2;
00403   pitch = M_PI/2;
00404   roll = 0;
00405   
00406   q = btQuaternion(0,0,0);
00407   v = btVector3(1,0,0);
00408   tr = btTransform(btQuaternion(yaw, pitch,roll), btVector3(0,0,0));
00409   
00410   v2 = tr * v;
00411 
00412   printf("%f, %f, %f: yaw and pitch by %f -> %f, %f, %f\n", v.x(), v.y(), v.z(), pitch, v2.x(), v2.y(), v2.z());
00413   EXPECT_NEAR(v2.x(), 0,epsilon); 
00414   EXPECT_NEAR(v2.y(), 0,epsilon); 
00415   EXPECT_NEAR(v2.z(), -1,epsilon); 
00416 
00417   q = btQuaternion(0,0,0);
00418   v = btVector3(0,1,0);
00419   tr = btTransform(btQuaternion(yaw, pitch,roll), btVector3(0,0,0));
00420   
00421   v2 = tr * v;
00422 
00423   printf("%f, %f, %f: yaw and pitch by %f -> %f, %f, %f\n", v.x(), v.y(), v.z(), pitch, v2.x(), v2.y(), v2.z());
00424   EXPECT_NEAR(v2.x(), -1,epsilon); 
00425   EXPECT_NEAR(v2.y(), 0,epsilon); 
00426   EXPECT_NEAR(v2.z(), 0,epsilon); 
00427 
00428   q = btQuaternion(0,0,0);
00429   v = btVector3(0,0,1);
00430   tr = btTransform(btQuaternion(yaw, pitch,roll), btVector3(0,0,0));
00431   
00432   v2 = tr * v;
00433 
00434   printf("%f, %f, %f: yaw and pitch by %f -> %f, %f, %f\n", v.x(), v.y(), v.z(), pitch, v2.x(), v2.y(), v2.z());
00435   EXPECT_NEAR(v2.x(), 0,epsilon); 
00436   EXPECT_NEAR(v2.y(), 1,epsilon); 
00437   EXPECT_NEAR(v2.z(), 0,epsilon); 
00438 
00439   //Pitch and Roll
00440   yaw = 0;
00441   pitch = M_PI/2;
00442   roll = M_PI/2;
00443   
00444   q = btQuaternion(0,0,0);
00445   v = btVector3(1,0,0);
00446   tr = btTransform(btQuaternion(yaw, pitch,roll), btVector3(0,0,0));
00447   
00448   v2 = tr * v;
00449 
00450   printf("%f, %f, %f: pitch and roll by %f -> %f, %f, %f\n", v.x(), v.y(), v.z(), pitch, v2.x(), v2.y(), v2.z());
00451   EXPECT_NEAR(v2.x(), 0,epsilon); 
00452   EXPECT_NEAR(v2.y(), 0,epsilon); 
00453   EXPECT_NEAR(v2.z(), -1,epsilon); 
00454 
00455   q = btQuaternion(0,0,0);
00456   v = btVector3(0,1,0);
00457   tr = btTransform(btQuaternion(yaw, pitch,roll), btVector3(0,0,0));
00458   
00459   v2 = tr * v;
00460 
00461   printf("%f, %f, %f: pitch and roll by %f -> %f, %f, %f\n", v.x(), v.y(), v.z(), pitch, v2.x(), v2.y(), v2.z());
00462   EXPECT_NEAR(v2.x(), 1,epsilon); 
00463   EXPECT_NEAR(v2.y(), 0,epsilon); 
00464   EXPECT_NEAR(v2.z(), 0,epsilon); 
00465 
00466   q = btQuaternion(0,0,0);
00467   v = btVector3(0,0,1);
00468   tr = btTransform(btQuaternion(yaw, pitch,roll), btVector3(0,0,0));
00469   
00470   v2 = tr * v;
00471 
00472   printf("%f, %f, %f: pitch and roll by %f -> %f, %f, %f\n", v.x(), v.y(), v.z(), pitch, v2.x(), v2.y(), v2.z());
00473   EXPECT_NEAR(v2.x(), 0,epsilon); 
00474   EXPECT_NEAR(v2.y(), -1,epsilon); 
00475   EXPECT_NEAR(v2.z(), 0,epsilon); 
00476 };
00477 
00478 TEST(Bullet, Angle)
00479 {
00480   seed_rand();
00481   unsigned int num_steps = 1000;
00482   double epsilon = 1e-3;
00483   btQuaternion q1(0, 0, 0, 1);
00484   for(unsigned int i = 0; i < num_steps; ++i){
00485     double q1_angle = 1.0 * i / num_steps * 2. * M_PI;
00486     q1.setRPY(1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX, 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX, q1_angle);
00487     for(unsigned int j = 0; j < num_steps; j++){
00488       btQuaternion q2;
00489       double q2_angle = 1.0 * j / num_steps * 2. * M_PI;
00490       q2.setRPY(1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX, 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX, q2_angle);
00491       //EXPECT_NEAR(q2.angle(q1), fabs(q2_angle - q1_angle), epsilon);
00492 
00493       double ratio = (double) rand() / (double) RAND_MAX;
00494       EXPECT_GE(ratio, 0.0);
00495       EXPECT_LE(ratio, 1.0);
00496 
00497       btQuaternion q3 = q2.slerp(q1, ratio);
00498       EXPECT_NEAR(angleShortestPath(q3, q2), angleShortestPath(q2, q1) * ratio, epsilon);
00499       EXPECT_NEAR(angleShortestPath(q3, q1), angleShortestPath(q2, q1) * (1 - ratio), epsilon);
00500 
00501       btQuaternion q4 = q1.slerp(q2, ratio);
00502       EXPECT_NEAR(angleShortestPath(q4, q1), angleShortestPath(q2, q1) * ratio, epsilon);
00503       EXPECT_NEAR(angleShortestPath(q4, q2), angleShortestPath(q2, q1) * (1  - ratio), epsilon);
00504 
00505       /*
00506       printf("Ratio: %f, q1(%f,%f,%f,%f) q2(%f,%f,%f,%f) q3(%f,%f,%f,%f) q1_norm: %f, q2_norm: %f, q3_norm: %f, q4_norm: %f\n", 
00507              ratio, q1.x(), q1.y(), q1.z(), q1.w(), q2.x(), q2.y(), q2.z(), q2.w(), q3.x(), q3.y(), q3.z(), q3.w(), q1.length2(), q2.length2(), q3.length2(), q4.length());
00508       */
00509 
00510 
00511       q2.setX(-1.0 * q2.x());
00512       q2.setY(-1.0 * q2.y());
00513       q2.setZ(-1.0 * q2.z());
00514       q2.setW(-1.0 * q2.w());
00515 
00516       //EXPECT_NEAR(q2.angle(q1), fabs(q2_angle - q1_angle), epsilon);
00517       q3 = q2.slerp(q1, ratio);
00518       EXPECT_NEAR(angleShortestPath(q3, q2), angleShortestPath(q2, q1) * ratio, epsilon);
00519       EXPECT_NEAR(angleShortestPath(q3, q1), angleShortestPath(q2, q1) * (1 - ratio), epsilon);
00520 
00521       q4 = q1.slerp(q2, ratio);
00522       EXPECT_NEAR(angleShortestPath(q4, q1), angleShortestPath(q2, q1) * ratio, epsilon);
00523       EXPECT_NEAR(angleShortestPath(q4, q2), angleShortestPath(q2, q1) * (1  - ratio), epsilon);
00524 
00525       EXPECT_LT(angleShortestPath(q1, q2), M_PI);
00526     }
00527   }
00528 
00529   btQuaternion q5(-0.00285953665482, 0.0134168786627, 0.871697390887, -0.489852497327);
00530   btQuaternion q6(0.00256516236927, -0.0136650510447, -0.865612832372, 0.500520839481);
00531 
00532   btQuaternion q7 = q6.slerp(q5, 0.5);
00533   EXPECT_NEAR(angleShortestPath(q7, q5), angleShortestPath(q5, q6) / 2, epsilon);
00534   EXPECT_NEAR(angleShortestPath(q7, q6), angleShortestPath(q5, q6) / 2, epsilon);
00535 
00536   btQuaternion q8 = q5.slerp(q6, 0.5);
00537   EXPECT_NEAR(angleShortestPath(q8, q5), angleShortestPath(q5, q6) / 2, epsilon);
00538   EXPECT_NEAR(angleShortestPath(q8, q6), angleShortestPath(q5, q6) / 2, epsilon);
00539 }
00540 
00541 
00542 TEST(Bullet, Slerp)
00543 {
00544 
00545   unsigned int runs = 100;
00546   seed_rand();
00547 
00548   btQuaternion q1, q2;
00549   q1.setEuler(0,0,0);
00550   
00551   for (unsigned int i = 0 ; i < runs ; i++)
00552   {
00553     q2.setEuler(1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX,
00554                 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX,
00555                 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX);
00556     
00557     
00558     btQuaternion q3 = slerp(q1,q2,0.5);
00559     
00560     EXPECT_NEAR(q3.angle(q1), q2.angle(q3), 1e-5);
00561   }
00562 
00563 }
00564 
00565 TEST(Bullet, QuaternionMultiplication)
00566 {
00567   btQuaternion q1 (btVector3(1,0,0), M_PI/2.0);
00568   btQuaternion q2 (btVector3(0,0,1), M_PI/2.0);
00569 
00570   btQuaternion q3 = q1*q2;
00571   printf("(%f,%f,%f,%f)*(%f,%f,%f,%f)=(%f,%f,%f,%f)\n",q1.x(), q1.y(), q1.z(), q1.getAngle(), q2.x(), q2.y(), q2.z(), q2.getAngle(), q3.x(), q3.y(), q3.z(), q3.getAngle());
00572 
00573   btMatrix3x3 m3(q3);
00574 
00575   btVector3 xout = m3*btVector3(1,0,0);
00576   btVector3 yout = m3*btVector3(0,1,0);
00577   btVector3 zout = m3*btVector3(0,0,1);
00578   printf("(%f, %f, %f), (%f, %f, %f), (%f, %f, %f)\n", 
00579          xout.x(), xout.y(), xout.z() ,
00580          yout.x(), yout.y(), yout.z() ,
00581          zout.x(), zout.y(), zout.z());
00582 
00583   q3 = q2*q1;
00584   printf("(%f,%f,%f,%f)*(%f,%f,%f,%f)=(%f,%f,%f,%f)\n",q2.x(), q2.y(), q2.z(), q2.getAngle(), q1.x(), q1.y(), q1.z(), q1.getAngle(), q3.x(), q3.y(), q3.z(), q3.getAngle());
00585 
00586   m3.setRotation(q3);
00587 
00588   xout = m3*btVector3(1,0,0);
00589   yout = m3*btVector3(0,1,0);
00590   zout = m3*btVector3(0,0,1);
00591   printf("(%f, %f, %f), (%f, %f, %f), (%f, %f, %f)\n", 
00592          xout.x(), xout.y(), xout.z() ,
00593          yout.x(), yout.y(), yout.z() ,
00594          zout.x(), zout.y(), zout.z());
00595 }
00596 TEST(Bullet, QuaternionTimesEqual)
00597 {
00598   btQuaternion q1 (btVector3(1,0,0), M_PI/2.0);
00599   btQuaternion q2 (btVector3(0,0,1), M_PI/2.0);
00600 
00601   btQuaternion q3 (q1);
00602   q3*=q2;
00603   printf("(%f,%f,%f,%f)*(%f,%f,%f,%f)=(%f,%f,%f,%f)\n",q1.x(), q1.y(), q1.z(), q1.getAngle(), q2.x(), q2.y(), q2.z(), q2.getAngle(), q3.x(), q3.y(), q3.z(), q3.getAngle());
00604 
00605   btMatrix3x3 m3(q3);
00606 
00607   btVector3 xout = m3*btVector3(1,0,0);
00608   btVector3 yout = m3*btVector3(0,1,0);
00609   btVector3 zout = m3*btVector3(0,0,1);
00610   printf("(%f, %f, %f), (%f, %f, %f), (%f, %f, %f)\n", 
00611          xout.x(), xout.y(), xout.z() ,
00612          yout.x(), yout.y(), yout.z() ,
00613          zout.x(), zout.y(), zout.z());
00614   q3 = q1;
00615   q3*=q1;
00616   printf("(%f,%f,%f,%f)*(%f,%f,%f,%f)=(%f,%f,%f,%f)\n",q2.x(), q2.y(), q2.z(), q2.getAngle(), q1.x(), q1.y(), q1.z(), q1.getAngle(), q3.x(), q3.y(), q3.z(), q3.getAngle());
00617 
00618   m3.setRotation(q3);
00619 
00620   xout = m3*btVector3(1,0,0);
00621   yout = m3*btVector3(0,1,0);
00622   zout = m3*btVector3(0,0,1);
00623   printf("(%f, %f, %f), (%f, %f, %f), (%f, %f, %f)\n", 
00624          xout.x(), xout.y(), xout.z() ,
00625          yout.x(), yout.y(), yout.z() ,
00626          zout.x(), zout.y(), zout.z());
00627 }
00628 
00629 TEST (Bullet, downcast)
00630 {
00631   btVector3 v (1,2,3);
00632   btVector3 v2(v); 
00633   
00634 }
00635 
00636 TEST(Bullet, TransformOrder )
00637 {
00638   btTransform tf(btQuaternion(1,0,0));
00639   btTransform tf2(btQuaternion(0,0,0),btVector3(100,0,0));
00640 
00641   btTransform tf3 = tf * tf2;
00642 
00643   tf*= tf2;
00644   EXPECT_TRUE(tf3  == tf);
00645 
00646   tf = btTransform(btQuaternion(1,0,0));
00647   tf3 = tf;
00648 
00649   EXPECT_TRUE(tf.inverse() * tf2 == tf.inverseTimes(tf2));
00650 
00651   
00652 }
00653 
00654 
00655 TEST(Bullet, SlerpZeroDistanceOppositeSigns)
00656 {
00657   btQuaternion q1 (M_PI/4,0,0);
00658   btQuaternion q2(-q1.x(), -q1.y(), -q1.z()-.0001, -q1.w()+.0001);
00659   q2.normalize();
00660   btQuaternion q3 = q2.slerp(q1, .5);
00661 
00662   //  printf("%f %f %f %f,%f %f %f %f\n", q2.x(), q2.y(), q2.z(), q2.w(), q3.x(), q3.y(), q3.z(), q3.w());
00663 
00664   EXPECT_NEAR(q1.angleShortestPath(q2), 0, 0.01);
00665   EXPECT_NEAR(q2.angleShortestPath(q2), 0, 0.01);
00666   EXPECT_NEAR(q1.angleShortestPath(q3), 0, 0.01);
00667 }
00668 
00669 TEST(Bullet, SetEulerZYX)
00670 {
00671   btMatrix3x3 mat;
00672   mat.setEulerZYX(M_PI/2, 0, 0);
00673   double yaw, pitch, roll;
00674   mat.getEulerZYX(yaw, pitch, roll);
00675   EXPECT_NEAR(yaw, M_PI/2, 0.1);
00676   EXPECT_NEAR(pitch, 0, 0.1);
00677   EXPECT_NEAR(roll, 0, 0.1);
00678 //  printf("%f %f %f\n", yaw, pitch, roll);
00679   btQuaternion q;
00680   mat.getRotation(q);
00681   EXPECT_NEAR(q.z(), sqrt(2)/2, 0.1);
00682   EXPECT_NEAR(q.y(), 0, 0.1);
00683   EXPECT_NEAR(q.x(), 0, 0.1);
00684   EXPECT_NEAR(q.w(), sqrt(2)/2, 0.1);
00685   //  printf("%f %f %f %f\n", q.x(), q.y(), q.z(), q.w());
00686 }
00687 
00688 
00689 
00690 TEST(Bullet, calculateDiffAxisAngleQuaternion)
00691 {
00692   btVector3 vec;
00693   btScalar ang;
00694   for (unsigned int i = 1 ; i < 1000 ; i++)
00695   {
00696     btQuaternion q1(M_PI*2 *(double) i / 1000, 0, 0);
00697     btQuaternion q2(M_PI/2*0, 0,0);
00698     btTransformUtil::calculateDiffAxisAngleQuaternion(q1, q2, vec, ang);
00699     //    printf("%f %f %f, ang %f\n", vec.x(), vec.y(), vec.z(), ang);
00700     EXPECT_NEAR(std::min(M_PI*2 *(double) i / 1000, 2 * M_PI - M_PI*2 *(double) i / 1000), ang, 0.001);
00701     btTransformUtil::calculateDiffAxisAngleQuaternion(q2, q1, vec, ang);
00702     //printf("%f %f %f, ang %f %d\n", vec.x(), vec.y(), vec.z(), ang, i);
00703     if (i <= 500)
00704       EXPECT_NEAR( vec.z(), 1.0, 0.001);
00705     else
00706       EXPECT_NEAR( vec.z(), -1.0, 0.001);
00707 
00708     EXPECT_NEAR(std::min(M_PI*2 *(double) i / 1000, 2 * M_PI - M_PI*2 *(double) i / 1000), ang, 0.001);
00709     if (i <= 500)
00710       EXPECT_NEAR( vec.z(), 1.0, 0.001);
00711     else
00712       EXPECT_NEAR( vec.z(), -1.0, 0.001);
00713   }
00714   for (unsigned int i = 1 ; i < 1000 ; i++)
00715   {
00716     btQuaternion q1(0, M_PI*2 *(double) i / 1000,1);
00717     btQuaternion q2(0, 0, 1);
00718     btTransformUtil::calculateDiffAxisAngleQuaternion(q1, q2, vec, ang);
00719     //printf("%f %f %f, ang %f %d\n", vec.x(), vec.y(), vec.z(), ang, i);
00720     EXPECT_NEAR(std::min(M_PI*2 *(double) i / 1000, 2 * M_PI - M_PI*2 *(double) i / 1000), ang, 0.001);
00721     if (i < 500)
00722       EXPECT_NEAR( vec.y(), -1.0, 0.001);
00723     else if (i > 501)
00724       EXPECT_NEAR( vec.y(), 1.0, 0.001);
00725     else
00726       EXPECT_NEAR( fabs(vec.y()), 1.0, 0.001);
00727     btTransformUtil::calculateDiffAxisAngleQuaternion(q2, q1, vec, ang);
00728     //printf("%f %f %f, ang %f\n", vec.x(), vec.y(), vec.z(), ang); 
00729     EXPECT_NEAR(std::min(M_PI*2 *(double) i / 1000, 2 * M_PI - M_PI*2 *(double) i / 1000), ang, 0.001);
00730     if (i < 500)
00731       EXPECT_NEAR( vec.y(), 1.0, 0.001);
00732     else if (i > 500)
00733       EXPECT_NEAR( vec.y(), -1.0, 0.001);
00734     else
00735       EXPECT_NEAR( fabs(vec.y()), 1.0, 0.001);
00736   }
00737   for (unsigned int i = 1 ; i < 1000 ; i++)
00738   {
00739     btQuaternion q1(0, 0, M_PI*2 *(double) i / 1000);
00740     btQuaternion q2(0, 0,0);
00741     btTransformUtil::calculateDiffAxisAngleQuaternion(q1, q2, vec, ang);
00742     //    printf("%f %f %f, ang %f\n", vec.x(), vec.y(), vec.z(), ang);
00743     EXPECT_NEAR(std::min(M_PI*2 *(double) i / 1000, 2 * M_PI - M_PI*2 *(double) i / 1000), ang, 0.001);
00744     if (i <= 500)
00745       EXPECT_NEAR( vec.x(), -1.0, 0.001);
00746     else
00747       EXPECT_NEAR( vec.x(), 1.0, 0.001);
00748     btTransformUtil::calculateDiffAxisAngleQuaternion(q2, q1, vec, ang);
00749     //    printf("%f %f %f, ang %f\n", vec.x(), vec.y(), vec.z(), ang);
00750     EXPECT_NEAR(std::min(M_PI*2 *(double) i / 1000, 2 * M_PI - M_PI*2 *(double) i / 1000), ang, 0.001);
00751     if (i <= 500)
00752       EXPECT_NEAR( vec.x(), 1.0, 0.001);
00753     else
00754       EXPECT_NEAR( vec.x(), -1.0, 0.001);
00755   }
00756 }
00757 
00758 
00759 int main(int argc, char **argv){
00760   testing::InitGoogleTest(&argc, argv);
00761   return RUN_ALL_TESTS();
00762 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Friends


bullet
Author(s): Erwin Coumans, ROS package maintained by Tully Foote
autogenerated on Wed Aug 14 2013 10:05:26