| | 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 | } |
