247 | | for (double pit1 = pit1start; pit1 <= pit1stop; pit1 += pit1step) { |
248 | | for (double yaw1 = yaw1start; yaw1 <= yaw1stop; yaw1 += yaw1step) { |
249 | | for (double rol2 = rol2start; rol2 <= rol2stop; rol2 += rol2step) { |
250 | | for (double pit2 = pit2start; pit2 <= pit2stop; pit2 += pit2step) { |
251 | | for (double yaw2 = yaw2start; yaw2 <= yaw2stop; yaw2 += yaw2step) { |
252 | | count++; |
253 | | // create two quats based on the roll, pitch and yaw values |
254 | | osg::Quat rot_quat1 = |
255 | | osg::Quat(osg::DegreesToRadians(rol1),osg::Vec3d(1,0,0), |
256 | | osg::DegreesToRadians(pit1),osg::Vec3d(0,1,0), |
257 | | osg::DegreesToRadians(yaw1),osg::Vec3d(0,0,1)); |
258 | | |
259 | | osg::Quat rot_quat2 = |
260 | | osg::Quat(osg::DegreesToRadians(rol2),osg::Vec3d(1,0,0), |
261 | | osg::DegreesToRadians(pit2),osg::Vec3d(0,1,0), |
262 | | osg::DegreesToRadians(yaw2),osg::Vec3d(0,0,1)); |
263 | | |
264 | | // create an output quat using quaternion math |
265 | | osg::Quat out_quat1; |
266 | | out_quat1 = rot_quat2 * rot_quat1; |
267 | | |
268 | | // create two matrices based on the input quats |
269 | | osg::Matrixd mat1,mat2; |
270 | | mat1.makeRotate(rot_quat1); |
271 | | mat2.makeRotate(rot_quat2); |
272 | | |
273 | | // create an output quat by matrix multiplication and getRotate |
274 | | osg::Matrixd out_mat; |
275 | | out_mat = mat2 * mat1; |
276 | | // add matrix scale for even more nastiness |
277 | | out_mat = out_mat * scalemat; |
278 | | osg::Quat out_quat2; |
279 | | out_quat2 = out_mat.getRotate(); |
280 | | |
281 | | // If the quaternion W is <0, then we should reflect |
282 | | // to get it into the positive W |
283 | | if(out_quat1.w()<0) out_quat1 = out_quat1 * -1.0; |
284 | | if(out_quat2.w()<0) out_quat2 = out_quat2 * -1.0; |
285 | | |
286 | | |
287 | | // if the output quat length is not one |
288 | | // or if the components do not match, |
289 | | // something is amiss |
290 | | if (fabs(1.0-out_quat2.length()) > eps || |
291 | | (fabs(out_quat1.x()-out_quat2.x())) > eps || |
292 | | (fabs(out_quat1.y()-out_quat2.y())) > eps || |
293 | | (fabs(out_quat1.z()-out_quat2.z())) > eps || |
294 | | (fabs(out_quat1.w()-out_quat2.w())) > eps) { |
295 | | std::cout << "testGetQuatFromMatrix problem at: \n" |
296 | | << " r1=" << rol1 |
297 | | << " p1=" << pit1 |
298 | | << " y1=" << yaw1 |
299 | | << " r2=" << rol2 |
300 | | << " p2=" << pit2 |
301 | | << " y2=" << yaw2 << "\n"; |
302 | | std::cout << "quats: " << out_quat1 << " length: " << out_quat1.length() << "\n"; |
303 | | std::cout << "mats and get: " << out_quat2 << " length: " << out_quat2.length() << "\n\n"; |
| 247 | for (double pit1 = pit1start; pit1 <= pit1stop; pit1 += pit1step) { |
| 248 | for (double yaw1 = yaw1start; yaw1 <= yaw1stop; yaw1 += yaw1step) { |
| 249 | for (double rol2 = rol2start; rol2 <= rol2stop; rol2 += rol2step) { |
| 250 | for (double pit2 = pit2start; pit2 <= pit2stop; pit2 += pit2step) { |
| 251 | for (double yaw2 = yaw2start; yaw2 <= yaw2stop; yaw2 += yaw2step) |
| 252 | { |
| 253 | count++; |
| 254 | // create two quats based on the roll, pitch and yaw values |
| 255 | osg::Quat rot_quat1 = |
| 256 | osg::Quat(osg::DegreesToRadians(rol1),osg::Vec3d(1,0,0), |
| 257 | osg::DegreesToRadians(pit1),osg::Vec3d(0,1,0), |
| 258 | osg::DegreesToRadians(yaw1),osg::Vec3d(0,0,1)); |
| 259 | |
| 260 | osg::Quat rot_quat2 = |
| 261 | osg::Quat(osg::DegreesToRadians(rol2),osg::Vec3d(1,0,0), |
| 262 | osg::DegreesToRadians(pit2),osg::Vec3d(0,1,0), |
| 263 | osg::DegreesToRadians(yaw2),osg::Vec3d(0,0,1)); |
| 264 | |
| 265 | // create an output quat using quaternion math |
| 266 | osg::Quat out_quat1; |
| 267 | out_quat1 = rot_quat2 * rot_quat1; |
| 268 | |
| 269 | // create two matrices based on the input quats |
| 270 | osg::Matrixd mat1,mat2; |
| 271 | mat1.makeRotate(rot_quat1); |
| 272 | mat2.makeRotate(rot_quat2); |
| 273 | |
| 274 | // create an output quat by matrix multiplication and getRotate |
| 275 | osg::Matrixd out_mat; |
| 276 | out_mat = mat2 * mat1; |
| 277 | // add matrix scale for even more nastiness |
| 278 | out_mat = out_mat * scalemat; |
| 279 | osg::Quat out_quat2; |
| 280 | out_quat2 = out_mat.getRotate(); |
| 281 | |
| 282 | // If the quaternion W is <0, then we should reflect |
| 283 | // to get it into the positive W. |
| 284 | // Unfortunately, when W is very small (close to 0), the sign |
| 285 | // does not really make sense because of precision problems |
| 286 | // and the reflection might not work. |
| 287 | if(out_quat1.w()<0) out_quat1 = out_quat1 * -1.0; |
| 288 | if(out_quat2.w()<0) out_quat2 = out_quat2 * -1.0; |
| 289 | |
| 290 | // if the output quat length is not one |
| 291 | // or if the components do not match, |
| 292 | // something is amiss |
| 293 | |
| 294 | bool componentsOK = false; |
| 295 | if ( ((fabs(out_quat1.x()-out_quat2.x())) < eps) && |
| 296 | ((fabs(out_quat1.y()-out_quat2.y())) < eps) && |
| 297 | ((fabs(out_quat1.z()-out_quat2.z())) < eps) && |
| 298 | ((fabs(out_quat1.w()-out_quat2.w())) < eps) ) |
| 299 | { |
| 300 | componentsOK = true; |
| 301 | } |
| 302 | // We should also test for q = -q which is valid, so reflect |
| 303 | // one quat. |
| 304 | out_quat2 = out_quat2 * -1.0; |
| 305 | if ( ((fabs(out_quat1.x()-out_quat2.x())) < eps) && |
| 306 | ((fabs(out_quat1.y()-out_quat2.y())) < eps) && |
| 307 | ((fabs(out_quat1.z()-out_quat2.z())) < eps) && |
| 308 | ((fabs(out_quat1.w()-out_quat2.w())) < eps) ) |
| 309 | { |
| 310 | componentsOK = true; |
| 311 | } |
| 312 | |
| 313 | bool lengthOK = false; |
| 314 | if (fabs(1.0-out_quat2.length()) < eps) |
| 315 | { |
| 316 | lengthOK = true; |
| 317 | } |
| 318 | |
| 319 | if (!lengthOK || !componentsOK) |
| 320 | { |
| 321 | std::cout << "testGetQuatFromMatrix problem at: \n" |
| 322 | << " r1=" << rol1 |
| 323 | << " p1=" << pit1 |
| 324 | << " y1=" << yaw1 |
| 325 | << " r2=" << rol2 |
| 326 | << " p2=" << pit2 |
| 327 | << " y2=" << yaw2 << "\n"; |
| 328 | std::cout << "quats: " << out_quat1 << " length: " << out_quat1.length() << "\n"; |
| 329 | std::cout << "mats and get: " << out_quat2 << " length: " << out_quat2.length() << "\n\n"; |
| 330 | } |
| 331 | } |
| 332 | } |