| 127 | /// Exercises the Matrix.get(Quat&) function, printout is generated on problems only |
| 128 | void testGetQuatFromMatrix() { |
| 129 | // acceptable error range |
| 130 | double eps=1e-3; |
| 131 | |
| 132 | #if 1 |
| 133 | // wide range |
| 134 | double rol1start = 0.0; |
| 135 | double rol1stop = 360.0; |
| 136 | double rol1step = 30.0; |
| 137 | |
| 138 | double pit1start = 0.0; |
| 139 | double pit1stop = 90.0; |
| 140 | double pit1step = 30.0; |
| 141 | |
| 142 | double yaw1start = 0.0; |
| 143 | double yaw1stop = 360.0; |
| 144 | double yaw1step = 30.0; |
| 145 | |
| 146 | double rol2start = 0.0; |
| 147 | double rol2stop = 360.0; |
| 148 | double rol2step = 30.0; |
| 149 | |
| 150 | double pit2start = 0.0; |
| 151 | double pit2stop = 90.0; |
| 152 | double pit2step = 30.0; |
| 153 | |
| 154 | double yaw2start = 0.0; |
| 155 | double yaw2stop = 360.0; |
| 156 | double yaw2step = 30.0; |
| 157 | #else |
| 158 | // focussed range |
| 159 | double rol1start = 0.0; |
| 160 | double rol1stop = 0.0; |
| 161 | double rol1step = 0.1; |
| 162 | |
| 163 | double pit1start = 0.0; |
| 164 | double pit1stop = 5.0; |
| 165 | double pit1step = 5.0; |
| 166 | |
| 167 | double yaw1start = 89.0; |
| 168 | double yaw1stop = 91.0; |
| 169 | double yaw1step = 0.1; |
| 170 | |
| 171 | double rol2start = 0.0; |
| 172 | double rol2stop = 0.0; |
| 173 | double rol2step = 0.1; |
| 174 | |
| 175 | double pit2start = 0.0; |
| 176 | double pit2stop = 0.0; |
| 177 | double pit2step = 0.1; |
| 178 | |
| 179 | double yaw2start = 89.0; |
| 180 | double yaw2stop = 91.0; |
| 181 | double yaw2step = 0.1; |
| 182 | #endif |
| 183 | |
| 184 | for (double rol1 = rol1start; rol1 <= rol1stop; rol1 += rol1step) { |
| 185 | for (double pit1 = pit1start; pit1 <= pit1stop; pit1 += pit1step) { |
| 186 | for (double yaw1 = yaw1start; yaw1 <= yaw1stop; yaw1 += yaw1step) { |
| 187 | for (double rol2 = rol2start; rol2 <= rol2stop; rol2 += rol2step) { |
| 188 | for (double pit2 = pit2start; pit2 <= pit2stop; pit2 += pit2step) { |
| 189 | for (double yaw2 = yaw2start; yaw2 <= yaw2stop; yaw2 += yaw2step) { |
| 190 | // create two quats based on the roll, pitch and yaw values |
| 191 | osg::Quat rot_quat1 = |
| 192 | osg::Quat(osg::DegreesToRadians(rol1),osg::Vec3d(1,0,0), |
| 193 | osg::DegreesToRadians(pit1),osg::Vec3d(0,1,0), |
| 194 | osg::DegreesToRadians(yaw1),osg::Vec3d(0,0,1)); |
| 195 | |
| 196 | osg::Quat rot_quat2 = |
| 197 | osg::Quat(osg::DegreesToRadians(rol2),osg::Vec3d(1,0,0), |
| 198 | osg::DegreesToRadians(pit2),osg::Vec3d(0,1,0), |
| 199 | osg::DegreesToRadians(yaw2),osg::Vec3d(0,0,1)); |
| 200 | |
| 201 | // create an output quat using quaternion math |
| 202 | osg::Quat out_quat1; |
| 203 | out_quat1 = rot_quat2 * rot_quat1; |
| 204 | |
| 205 | // create two matrices based on the input quats |
| 206 | osg::Matrixd mat1,mat2; |
| 207 | mat1.set(rot_quat1); |
| 208 | mat2.set(rot_quat2); |
| 209 | |
| 210 | // create an output quat by matrix multiplication and get |
| 211 | osg::Matrixd out_mat; |
| 212 | out_mat = mat2 * mat1; |
| 213 | osg::Quat out_quat2; |
| 214 | out_mat.get(out_quat2); |
| 215 | |
| 216 | // if the output quat length is not one |
| 217 | // or if the component magnitudes do not match, |
| 218 | // something is amiss |
| 219 | if (fabs(1.0-out_quat2.length()) > eps || |
| 220 | (fabs(out_quat1.x())-fabs(out_quat2.x())) > eps || |
| 221 | (fabs(out_quat1.y())-fabs(out_quat2.y())) > eps || |
| 222 | (fabs(out_quat1.z())-fabs(out_quat2.z())) > eps || |
| 223 | (fabs(out_quat1.w())-fabs(out_quat2.w())) > eps) { |
| 224 | std::cout << "problem at: r1=" << rol1 |
| 225 | << " p1=" << pit1 |
| 226 | << " y1=" << yaw1 |
| 227 | << " r2=" << rol2 |
| 228 | << " p2=" << pit2 |
| 229 | << " y2=" << yaw2 << "\n"; |
| 230 | std::cout << "quats: " << out_quat1 << " length: " << out_quat1.length() << "\n"; |
| 231 | std::cout << "mats and get: " << out_quat2 << " length: " << out_quat2.length() << "\n\n"; |
| 232 | } |
| 233 | } |
| 234 | } |
| 235 | } |
| 236 | } |
| 237 | } |
| 238 | } |
| 239 | } |