Show
Ignore:
Timestamp:
03/13/08 17:38:05 (6 years ago)
Author:
robert
Message:

From Jose Delport, "attached is a version of osgunittests that does not give false alarms
for the case where q1 = -q2. The output of 'osgunittests quat' is now
much cleaner.
"

Files:
1 modified

Legend:

Unmodified
Added
Removed
  • OpenSceneGraph/trunk/examples/osgunittests/osgunittests.cpp

    r7648 r7939  
    245245    int count=0; 
    246246    for (double rol1 = rol1start; rol1 <= rol1stop; rol1 += rol1step) { 
    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                    } 
    304333                } 
    305334            } 
    306             } 
    307335        } 
    308         } 
    309     } 
    310336    } 
    311337    tstop = osg::Timer::instance()->tick();