$search
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 }