Show
Ignore:
Timestamp:
06/01/07 23:38:53 (7 years ago)
Author:
robert
Message:

From J.P Delport, "attached find an updated osgunittests.cpp that allows for the testing of
the matrix.getRotate() function when a matrix contains a scale as well
as a rotation.

The scale can optionally be switched off, see the top of
testQuatFromMatrix().

As expected, all the current methods for mat to quat conversion fail
these new tests. When the scale is omitted, mk2 of getRotate with sign
instead of signOrZero passes, as well as mk1.
"

Files:
1 modified

Legend:

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

    r6627 r6810  
    44#include <osg/Vec3> 
    55#include <osg/Matrix> 
     6#include <osg/Timer> 
    67#include <osg/io_utils> 
    78 
     
    144145} 
    145146 
    146 /// Exercises the Matrix.get(Quat&) function, printout is generated on problems only 
     147/// Exercise the Matrix.getRotate function. 
     148/// Compare the output of: 
     149///  q1 * q2  
     150/// versus 
     151///  (mat(q1)*mat(q2)*scale).getRotate() 
     152/// for a range of rotations 
    147153void testGetQuatFromMatrix() { 
     154     
     155    // Options 
     156     
    148157    // acceptable error range 
    149     double eps=1e-3; 
    150  
     158    double eps=1e-6; 
     159 
     160    // scale matrix 
     161    // To not test with scale, use 1,1,1 
     162    // Not sure if 0's or negative values are acceptable 
     163    osg::Matrixd scalemat; 
     164    scalemat.makeScale(osg::Vec3d(0.9,0.5,0.1)); 
     165     
     166    // range of rotations 
    151167#if 1 
    152168    // wide range 
    153169    double rol1start = 0.0; 
    154170    double rol1stop = 360.0; 
    155     double rol1step = 30.0; 
     171    double rol1step = 20.0; 
    156172 
    157173    double pit1start = 0.0; 
    158174    double pit1stop = 90.0; 
    159     double pit1step = 30.0; 
     175    double pit1step = 20.0; 
    160176 
    161177    double yaw1start = 0.0; 
    162178    double yaw1stop = 360.0; 
    163     double yaw1step = 30.0; 
     179    double yaw1step = 20.0; 
    164180 
    165181    double rol2start = 0.0; 
    166182    double rol2stop = 360.0; 
    167     double rol2step = 30.0; 
     183    double rol2step = 20.0; 
    168184 
    169185    double pit2start = 0.0; 
    170186    double pit2stop = 90.0; 
    171     double pit2step = 30.0; 
     187    double pit2step = 20.0; 
    172188 
    173189    double yaw2start = 0.0; 
    174190    double yaw2stop = 360.0; 
    175     double yaw2step = 30.0; 
     191    double yaw2step = 20.0; 
    176192#else 
    177193    // focussed range 
     
    201217#endif 
    202218     
     219    std::cout << std::endl << "Starting " << __FUNCTION__ << ", it can take a while ..." << std::endl; 
     220 
     221    osg::Timer_t tstart, tstop; 
     222    tstart = osg::Timer::instance()->tick(); 
     223    int count=0; 
    203224    for (double rol1 = rol1start; rol1 <= rol1stop; rol1 += rol1step) { 
    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; 
    258286} 
    259287 
     
    314342    testQuatRotate(osg::Vec3d(0.0,0.0,-1.0),osg::Vec3d(0.0,0.0,1.0)); 
    315343 
     344    // Test a range of rotations 
    316345    testGetQuatFromMatrix(); 
    317346 
    318  
     347    // This is a specific test case for a matrix containing scale and rotation 
    319348    osg::Matrix matrix(0.5, 0.0, 0.0, 0.0, 
    320349                       0.0, 0.5, 0.0, 0.0, 
     
    325354    matrix.get(quat); 
    326355     
    327     osg::notify(osg::NOTICE)<<"Matrix = "<<matrix<<" rotation="<<quat<<std::endl; 
    328  
     356    osg::notify(osg::NOTICE)<<"Matrix = "<<matrix<<"rotation = "<<quat<<", expected quat = (0,0,0,1)"<<std::endl; 
    329357} 
    330358