204 | | for (double pit1 = pit1start; pit1 <= pit1stop; pit1 += pit1step) { |
205 | | for (double yaw1 = yaw1start; yaw1 <= yaw1stop; yaw1 += yaw1step) { |
206 | | for (double rol2 = rol2start; rol2 <= rol2stop; rol2 += rol2step) { |
207 | | for (double pit2 = pit2start; pit2 <= pit2stop; pit2 += pit2step) { |
208 | | for (double yaw2 = yaw2start; yaw2 <= yaw2stop; yaw2 += yaw2step) { |
209 | | // create two quats based on the roll, pitch and yaw values |
210 | | osg::Quat rot_quat1 = |
211 | | osg::Quat(osg::DegreesToRadians(rol1),osg::Vec3d(1,0,0), |
212 | | osg::DegreesToRadians(pit1),osg::Vec3d(0,1,0), |
213 | | osg::DegreesToRadians(yaw1),osg::Vec3d(0,0,1)); |
214 | | |
215 | | osg::Quat rot_quat2 = |
216 | | osg::Quat(osg::DegreesToRadians(rol2),osg::Vec3d(1,0,0), |
217 | | osg::DegreesToRadians(pit2),osg::Vec3d(0,1,0), |
218 | | osg::DegreesToRadians(yaw2),osg::Vec3d(0,0,1)); |
219 | | |
220 | | // create an output quat using quaternion math |
221 | | osg::Quat out_quat1; |
222 | | out_quat1 = rot_quat2 * rot_quat1; |
223 | | |
224 | | // create two matrices based on the input quats |
225 | | osg::Matrixd mat1,mat2; |
226 | | mat1.makeRotate(rot_quat1); |
227 | | mat2.makeRotate(rot_quat2); |
228 | | |
229 | | // create an output quat by matrix multiplication and get |
230 | | osg::Matrixd out_mat; |
231 | | out_mat = mat2 * mat1; |
232 | | osg::Quat out_quat2; |
233 | | out_quat2 = out_mat.getRotate(); |
234 | | |
235 | | // if the output quat length is not one |
236 | | // or if the component magnitudes do not match, |
237 | | // something is amiss |
238 | | if (fabs(1.0-out_quat2.length()) > eps || |
239 | | (fabs(out_quat1.x())-fabs(out_quat2.x())) > eps || |
240 | | (fabs(out_quat1.y())-fabs(out_quat2.y())) > eps || |
241 | | (fabs(out_quat1.z())-fabs(out_quat2.z())) > eps || |
242 | | (fabs(out_quat1.w())-fabs(out_quat2.w())) > eps) { |
243 | | std::cout << "problem at: r1=" << rol1 |
244 | | << " p1=" << pit1 |
245 | | << " y1=" << yaw1 |
246 | | << " r2=" << rol2 |
247 | | << " p2=" << pit2 |
248 | | << " y2=" << yaw2 << "\n"; |
249 | | std::cout << "quats: " << out_quat1 << " length: " << out_quat1.length() << "\n"; |
250 | | std::cout << "mats and get: " << out_quat2 << " length: " << out_quat2.length() << "\n\n"; |
251 | | } |
252 | | } |
253 | | } |
254 | | } |
255 | | } |
256 | | } |
257 | | } |
| 225 | for (double pit1 = pit1start; pit1 <= pit1stop; pit1 += pit1step) { |
| 226 | for (double yaw1 = yaw1start; yaw1 <= yaw1stop; yaw1 += yaw1step) { |
| 227 | for (double rol2 = rol2start; rol2 <= rol2stop; rol2 += rol2step) { |
| 228 | for (double pit2 = pit2start; pit2 <= pit2stop; pit2 += pit2step) { |
| 229 | for (double yaw2 = yaw2start; yaw2 <= yaw2stop; yaw2 += yaw2step) { |
| 230 | count++; |
| 231 | // create two quats based on the roll, pitch and yaw values |
| 232 | osg::Quat rot_quat1 = |
| 233 | osg::Quat(osg::DegreesToRadians(rol1),osg::Vec3d(1,0,0), |
| 234 | osg::DegreesToRadians(pit1),osg::Vec3d(0,1,0), |
| 235 | osg::DegreesToRadians(yaw1),osg::Vec3d(0,0,1)); |
| 236 | |
| 237 | osg::Quat rot_quat2 = |
| 238 | osg::Quat(osg::DegreesToRadians(rol2),osg::Vec3d(1,0,0), |
| 239 | osg::DegreesToRadians(pit2),osg::Vec3d(0,1,0), |
| 240 | osg::DegreesToRadians(yaw2),osg::Vec3d(0,0,1)); |
| 241 | |
| 242 | // create an output quat using quaternion math |
| 243 | osg::Quat out_quat1; |
| 244 | out_quat1 = rot_quat2 * rot_quat1; |
| 245 | |
| 246 | // create two matrices based on the input quats |
| 247 | osg::Matrixd mat1,mat2; |
| 248 | mat1.makeRotate(rot_quat1); |
| 249 | mat2.makeRotate(rot_quat2); |
| 250 | |
| 251 | // create an output quat by matrix multiplication and getRotate |
| 252 | osg::Matrixd out_mat; |
| 253 | out_mat = mat2 * mat1; |
| 254 | // add matrix scale for even more nastiness |
| 255 | out_mat = out_mat * scalemat; |
| 256 | osg::Quat out_quat2; |
| 257 | out_quat2 = out_mat.getRotate(); |
| 258 | |
| 259 | // if the output quat length is not one |
| 260 | // or if the component magnitudes do not match, |
| 261 | // something is amiss |
| 262 | if (fabs(1.0-out_quat2.length()) > eps || |
| 263 | (fabs(out_quat1.x())-fabs(out_quat2.x())) > eps || |
| 264 | (fabs(out_quat1.y())-fabs(out_quat2.y())) > eps || |
| 265 | (fabs(out_quat1.z())-fabs(out_quat2.z())) > eps || |
| 266 | (fabs(out_quat1.w())-fabs(out_quat2.w())) > eps) { |
| 267 | std::cout << __FUNCTION__ << " problem at: \n" |
| 268 | << " r1=" << rol1 |
| 269 | << " p1=" << pit1 |
| 270 | << " y1=" << yaw1 |
| 271 | << " r2=" << rol2 |
| 272 | << " p2=" << pit2 |
| 273 | << " y2=" << yaw2 << "\n"; |
| 274 | std::cout << "quats: " << out_quat1 << " length: " << out_quat1.length() << "\n"; |
| 275 | std::cout << "mats and get: " << out_quat2 << " length: " << out_quat2.length() << "\n\n"; |
| 276 | } |
| 277 | } |
| 278 | } |
| 279 | } |
| 280 | } |
| 281 | } |
| 282 | } |
| 283 | tstop = osg::Timer::instance()->tick(); |
| 284 | double duration = osg::Timer::instance()->delta_s(tstart,tstop); |
| 285 | std::cout << "Time for " << __FUNCTION__ << " with " << count << " iterations: " << duration << std::endl << std::endl; |