1.1 --- /dev/null Thu Jan 01 00:00:00 1970 +0000
1.2 +++ b/os/ossrv/stdcpp/tsrc/Boost_test/math/quaternion/inc/HSO3.hpp Fri Jun 15 03:10:57 2012 +0200
1.3 @@ -0,0 +1,509 @@
1.4 +
1.5 +/********************************************************************************************/
1.6 +/* */
1.7 +/* HSO3.hpp header file */
1.8 +/* */
1.9 +/* This file is not currently part of the Boost library. It is simply an example of the use */
1.10 +/* quaternions can be put to. Hopefully it will be useful too. */
1.11 +/* */
1.12 +/* This file provides tools to convert between quaternions and R^3 rotation matrices. */
1.13 +/* */
1.14 +/********************************************************************************************/
1.15 +
1.16 +// (C) Copyright Hubert Holin 2001.
1.17 +// Distributed under the Boost Software License, Version 1.0. (See
1.18 +// accompanying file LICENSE_1_0.txt or copy at
1.19 +// http://www.boost.org/LICENSE_1_0.txt)
1.20 +
1.21 +#ifndef TEST_HSO3_HPP
1.22 +#define TEST_HSO3_HPP
1.23 +
1.24 +#include <algorithm>
1.25 +
1.26 +#if defined(__GNUC__) && (__GNUC__ < 3)
1.27 +#include <boost/limits.hpp>
1.28 +#else
1.29 +#include <limits>
1.30 +#endif
1.31 +
1.32 +#include <stdexcept>
1.33 +#include <string>
1.34 +
1.35 +#include <boost/math/quaternion.hpp>
1.36 +
1.37 +
1.38 +#if defined(__GNUC__) && (__GNUC__ < 3)
1.39 +// gcc 2.x ignores function scope using declarations, put them here instead:
1.40 +using namespace ::std;
1.41 +using namespace ::boost::math;
1.42 +#endif
1.43 +
1.44 +template<typename TYPE_FLOAT>
1.45 +struct R3_matrix
1.46 +{
1.47 + TYPE_FLOAT a11, a12, a13;
1.48 + TYPE_FLOAT a21, a22, a23;
1.49 + TYPE_FLOAT a31, a32, a33;
1.50 +};
1.51 +
1.52 +
1.53 +// Note: the input quaternion need not be of norm 1 for the following function
1.54 +
1.55 +template<typename TYPE_FLOAT>
1.56 +R3_matrix<TYPE_FLOAT> quaternion_to_R3_rotation(::boost::math::quaternion<TYPE_FLOAT> const & q)
1.57 +{
1.58 + using ::std::numeric_limits;
1.59 +
1.60 + TYPE_FLOAT a = q.R_component_1();
1.61 + TYPE_FLOAT b = q.R_component_2();
1.62 + TYPE_FLOAT c = q.R_component_3();
1.63 + TYPE_FLOAT d = q.R_component_4();
1.64 +
1.65 + TYPE_FLOAT aa = a*a;
1.66 + TYPE_FLOAT ab = a*b;
1.67 + TYPE_FLOAT ac = a*c;
1.68 + TYPE_FLOAT ad = a*d;
1.69 + TYPE_FLOAT bb = b*b;
1.70 + TYPE_FLOAT bc = b*c;
1.71 + TYPE_FLOAT bd = b*d;
1.72 + TYPE_FLOAT cc = c*c;
1.73 + TYPE_FLOAT cd = c*d;
1.74 + TYPE_FLOAT dd = d*d;
1.75 +
1.76 + TYPE_FLOAT norme_carre = aa+bb+cc+dd;
1.77 +
1.78 + if (norme_carre <= numeric_limits<TYPE_FLOAT>::epsilon())
1.79 + {
1.80 + ::std::string error_reporting("Argument to quaternion_to_R3_rotation is too small!");
1.81 + ::std::underflow_error bad_argument(error_reporting);
1.82 +
1.83 + throw(bad_argument);
1.84 + }
1.85 +
1.86 + R3_matrix<TYPE_FLOAT> out_matrix;
1.87 +
1.88 + out_matrix.a11 = (aa+bb-cc-dd)/norme_carre;
1.89 + out_matrix.a12 = 2*(-ad+bc)/norme_carre;
1.90 + out_matrix.a13 = 2*(ac+bd)/norme_carre;
1.91 + out_matrix.a21 = 2*(ad+bc)/norme_carre;
1.92 + out_matrix.a22 = (aa-bb+cc-dd)/norme_carre;
1.93 + out_matrix.a23 = 2*(-ab+cd)/norme_carre;
1.94 + out_matrix.a31 = 2*(-ac+bd)/norme_carre;
1.95 + out_matrix.a32 = 2*(ab+cd)/norme_carre;
1.96 + out_matrix.a33 = (aa-bb-cc+dd)/norme_carre;
1.97 +
1.98 + return(out_matrix);
1.99 +}
1.100 +
1.101 +
1.102 + template<typename TYPE_FLOAT>
1.103 + void find_invariant_vector( R3_matrix<TYPE_FLOAT> const & rot,
1.104 + TYPE_FLOAT & x,
1.105 + TYPE_FLOAT & y,
1.106 + TYPE_FLOAT & z)
1.107 + {
1.108 + using ::std::sqrt;
1.109 +
1.110 + using ::std::numeric_limits;
1.111 +
1.112 + TYPE_FLOAT b11 = rot.a11 - static_cast<TYPE_FLOAT>(1);
1.113 + TYPE_FLOAT b12 = rot.a12;
1.114 + TYPE_FLOAT b13 = rot.a13;
1.115 + TYPE_FLOAT b21 = rot.a21;
1.116 + TYPE_FLOAT b22 = rot.a22 - static_cast<TYPE_FLOAT>(1);
1.117 + TYPE_FLOAT b23 = rot.a23;
1.118 + TYPE_FLOAT b31 = rot.a31;
1.119 + TYPE_FLOAT b32 = rot.a32;
1.120 + TYPE_FLOAT b33 = rot.a33 - static_cast<TYPE_FLOAT>(1);
1.121 +
1.122 + TYPE_FLOAT minors[9] =
1.123 + {
1.124 + b11*b22-b12*b21,
1.125 + b11*b23-b13*b21,
1.126 + b12*b23-b13*b22,
1.127 + b11*b32-b12*b31,
1.128 + b11*b33-b13*b31,
1.129 + b12*b33-b13*b32,
1.130 + b21*b32-b22*b31,
1.131 + b21*b33-b23*b31,
1.132 + b22*b33-b23*b32
1.133 + };
1.134 +
1.135 + TYPE_FLOAT * where = ::std::max_element(minors, minors+9);
1.136 +
1.137 + TYPE_FLOAT det = *where;
1.138 +
1.139 + if (det <= numeric_limits<TYPE_FLOAT>::epsilon())
1.140 + {
1.141 + ::std::string error_reporting("Underflow error in find_invariant_vector!");
1.142 + ::std::underflow_error processing_error(error_reporting);
1.143 +
1.144 + throw(processing_error);
1.145 + }
1.146 +
1.147 + switch (where-minors)
1.148 + {
1.149 + case 0:
1.150 +
1.151 + z = static_cast<TYPE_FLOAT>(1);
1.152 +
1.153 + x = (-b13*b22+b12*b23)/det;
1.154 + y = (-b11*b23+b13*b21)/det;
1.155 +
1.156 + break;
1.157 +
1.158 + case 1:
1.159 +
1.160 + y = static_cast<TYPE_FLOAT>(1);
1.161 +
1.162 + x = (-b12*b23+b13*b22)/det;
1.163 + z = (-b11*b22+b12*b21)/det;
1.164 +
1.165 + break;
1.166 +
1.167 + case 2:
1.168 +
1.169 + x = static_cast<TYPE_FLOAT>(1);
1.170 +
1.171 + y = (-b11*b23+b13*b21)/det;
1.172 + z = (-b12*b21+b11*b22)/det;
1.173 +
1.174 + break;
1.175 +
1.176 + case 3:
1.177 +
1.178 + z = static_cast<TYPE_FLOAT>(1);
1.179 +
1.180 + x = (-b13*b32+b12*b33)/det;
1.181 + y = (-b11*b33+b13*b31)/det;
1.182 +
1.183 + break;
1.184 +
1.185 + case 4:
1.186 +
1.187 + y = static_cast<TYPE_FLOAT>(1);
1.188 +
1.189 + x = (-b12*b33+b13*b32)/det;
1.190 + z = (-b11*b32+b12*b31)/det;
1.191 +
1.192 + break;
1.193 +
1.194 + case 5:
1.195 +
1.196 + x = static_cast<TYPE_FLOAT>(1);
1.197 +
1.198 + y = (-b11*b33+b13*b31)/det;
1.199 + z = (-b12*b31+b11*b32)/det;
1.200 +
1.201 + break;
1.202 +
1.203 + case 6:
1.204 +
1.205 + z = static_cast<TYPE_FLOAT>(1);
1.206 +
1.207 + x = (-b23*b32+b22*b33)/det;
1.208 + y = (-b21*b33+b23*b31)/det;
1.209 +
1.210 + break;
1.211 +
1.212 + case 7:
1.213 +
1.214 + y = static_cast<TYPE_FLOAT>(1);
1.215 +
1.216 + x = (-b22*b33+b23*b32)/det;
1.217 + z = (-b21*b32+b22*b31)/det;
1.218 +
1.219 + break;
1.220 +
1.221 + case 8:
1.222 +
1.223 + x = static_cast<TYPE_FLOAT>(1);
1.224 +
1.225 + y = (-b21*b33+b23*b31)/det;
1.226 + z = (-b22*b31+b21*b32)/det;
1.227 +
1.228 + break;
1.229 +
1.230 + default:
1.231 +
1.232 + ::std::string error_reporting("Impossible condition in find_invariant_vector");
1.233 + ::std::logic_error processing_error(error_reporting);
1.234 +
1.235 + throw(processing_error);
1.236 +
1.237 + break;
1.238 + }
1.239 +
1.240 + TYPE_FLOAT vecnorm = sqrt(x*x+y*y+z*z);
1.241 +
1.242 + if (vecnorm <= numeric_limits<TYPE_FLOAT>::epsilon())
1.243 + {
1.244 + ::std::string error_reporting("Overflow error in find_invariant_vector!");
1.245 + ::std::overflow_error processing_error(error_reporting);
1.246 +
1.247 + throw(processing_error);
1.248 + }
1.249 +
1.250 + x /= vecnorm;
1.251 + y /= vecnorm;
1.252 + z /= vecnorm;
1.253 + }
1.254 +
1.255 +
1.256 + template<typename TYPE_FLOAT>
1.257 + void find_orthogonal_vector( TYPE_FLOAT x,
1.258 + TYPE_FLOAT y,
1.259 + TYPE_FLOAT z,
1.260 + TYPE_FLOAT & u,
1.261 + TYPE_FLOAT & v,
1.262 + TYPE_FLOAT & w)
1.263 + {
1.264 + using ::std::abs;
1.265 + using ::std::sqrt;
1.266 +
1.267 + using ::std::numeric_limits;
1.268 +
1.269 + TYPE_FLOAT vecnormsqr = x*x+y*y+z*z;
1.270 +
1.271 + if (vecnormsqr <= numeric_limits<TYPE_FLOAT>::epsilon())
1.272 + {
1.273 + ::std::string error_reporting("Underflow error in find_orthogonal_vector!");
1.274 + ::std::underflow_error processing_error(error_reporting);
1.275 +
1.276 + throw(processing_error);
1.277 + }
1.278 +
1.279 + TYPE_FLOAT lambda;
1.280 +
1.281 + TYPE_FLOAT components[3] =
1.282 + {
1.283 + abs(x),
1.284 + abs(y),
1.285 + abs(z)
1.286 + };
1.287 +
1.288 + TYPE_FLOAT * where = ::std::min_element(components, components+3);
1.289 +
1.290 + switch (where-components)
1.291 + {
1.292 + case 0:
1.293 +
1.294 + if (*where <= numeric_limits<TYPE_FLOAT>::epsilon())
1.295 + {
1.296 + v =
1.297 + w = static_cast<TYPE_FLOAT>(0);
1.298 + u = static_cast<TYPE_FLOAT>(1);
1.299 + }
1.300 + else
1.301 + {
1.302 + lambda = -x/vecnormsqr;
1.303 +
1.304 + u = static_cast<TYPE_FLOAT>(1) + lambda*x;
1.305 + v = lambda*y;
1.306 + w = lambda*z;
1.307 + }
1.308 +
1.309 + break;
1.310 +
1.311 + case 1:
1.312 +
1.313 + if (*where <= numeric_limits<TYPE_FLOAT>::epsilon())
1.314 + {
1.315 + u =
1.316 + w = static_cast<TYPE_FLOAT>(0);
1.317 + v = static_cast<TYPE_FLOAT>(1);
1.318 + }
1.319 + else
1.320 + {
1.321 + lambda = -y/vecnormsqr;
1.322 +
1.323 + u = lambda*x;
1.324 + v = static_cast<TYPE_FLOAT>(1) + lambda*y;
1.325 + w = lambda*z;
1.326 + }
1.327 +
1.328 + break;
1.329 +
1.330 + case 2:
1.331 +
1.332 + if (*where <= numeric_limits<TYPE_FLOAT>::epsilon())
1.333 + {
1.334 + u =
1.335 + v = static_cast<TYPE_FLOAT>(0);
1.336 + w = static_cast<TYPE_FLOAT>(1);
1.337 + }
1.338 + else
1.339 + {
1.340 + lambda = -z/vecnormsqr;
1.341 +
1.342 + u = lambda*x;
1.343 + v = lambda*y;
1.344 + w = static_cast<TYPE_FLOAT>(1) + lambda*z;
1.345 + }
1.346 +
1.347 + break;
1.348 +
1.349 + default:
1.350 +
1.351 + ::std::string error_reporting("Impossible condition in find_invariant_vector");
1.352 + ::std::logic_error processing_error(error_reporting);
1.353 +
1.354 + throw(processing_error);
1.355 +
1.356 + break;
1.357 + }
1.358 +
1.359 + TYPE_FLOAT vecnorm = sqrt(u*u+v*v+w*w);
1.360 +
1.361 + if (vecnorm <= numeric_limits<TYPE_FLOAT>::epsilon())
1.362 + {
1.363 + ::std::string error_reporting("Underflow error in find_orthogonal_vector!");
1.364 + ::std::underflow_error processing_error(error_reporting);
1.365 +
1.366 + throw(processing_error);
1.367 + }
1.368 +
1.369 + u /= vecnorm;
1.370 + v /= vecnorm;
1.371 + w /= vecnorm;
1.372 + }
1.373 +
1.374 +
1.375 + // Note: we want [[v, v, w], [r, s, t], [x, y, z]] to be a direct orthogonal basis
1.376 + // of R^3. It might not be orthonormal, however, and we do not check if the
1.377 + // two input vectors are colinear or not.
1.378 +
1.379 + template<typename TYPE_FLOAT>
1.380 + void find_vector_for_BOD(TYPE_FLOAT x,
1.381 + TYPE_FLOAT y,
1.382 + TYPE_FLOAT z,
1.383 + TYPE_FLOAT u,
1.384 + TYPE_FLOAT v,
1.385 + TYPE_FLOAT w,
1.386 + TYPE_FLOAT & r,
1.387 + TYPE_FLOAT & s,
1.388 + TYPE_FLOAT & t)
1.389 + {
1.390 + r = +y*w-z*v;
1.391 + s = -x*w+z*u;
1.392 + t = +x*v-y*u;
1.393 + }
1.394 +
1.395 +
1.396 +
1.397 +template<typename TYPE_FLOAT>
1.398 +inline bool is_R3_rotation_matrix(R3_matrix<TYPE_FLOAT> const & mat)
1.399 +{
1.400 + using ::std::abs;
1.401 +
1.402 + using ::std::numeric_limits;
1.403 +
1.404 + return (
1.405 + !(
1.406 + (abs(mat.a11*mat.a11+mat.a21*mat.a21+mat.a31*mat.a31 - static_cast<TYPE_FLOAT>(1)) > static_cast<TYPE_FLOAT>(10)*numeric_limits<TYPE_FLOAT>::epsilon())||
1.407 + (abs(mat.a11*mat.a12+mat.a21*mat.a22+mat.a31*mat.a32 - static_cast<TYPE_FLOAT>(0)) > static_cast<TYPE_FLOAT>(10)*numeric_limits<TYPE_FLOAT>::epsilon())||
1.408 + (abs(mat.a11*mat.a13+mat.a21*mat.a23+mat.a31*mat.a33 - static_cast<TYPE_FLOAT>(0)) > static_cast<TYPE_FLOAT>(10)*numeric_limits<TYPE_FLOAT>::epsilon())||
1.409 + //(abs(mat.a11*mat.a12+mat.a21*mat.a22+mat.a31*mat.a32 - static_cast<TYPE_FLOAT>(0)) > static_cast<TYPE_FLOAT>(10)*numeric_limits<TYPE_FLOAT>::epsilon())||
1.410 + (abs(mat.a12*mat.a12+mat.a22*mat.a22+mat.a32*mat.a32 - static_cast<TYPE_FLOAT>(1)) > static_cast<TYPE_FLOAT>(10)*numeric_limits<TYPE_FLOAT>::epsilon())||
1.411 + (abs(mat.a12*mat.a13+mat.a22*mat.a23+mat.a32*mat.a33 - static_cast<TYPE_FLOAT>(0)) > static_cast<TYPE_FLOAT>(10)*numeric_limits<TYPE_FLOAT>::epsilon())||
1.412 + //(abs(mat.a11*mat.a13+mat.a21*mat.a23+mat.a31*mat.a33 - static_cast<TYPE_FLOAT>(0)) > static_cast<TYPE_FLOAT>(10)*numeric_limits<TYPE_FLOAT>::epsilon())||
1.413 + //(abs(mat.a12*mat.a13+mat.a22*mat.a23+mat.a32*mat.a33 - static_cast<TYPE_FLOAT>(0)) > static_cast<TYPE_FLOAT>(10)*numeric_limits<TYPE_FLOAT>::epsilon())||
1.414 + (abs(mat.a13*mat.a13+mat.a23*mat.a23+mat.a33*mat.a33 - static_cast<TYPE_FLOAT>(1)) > static_cast<TYPE_FLOAT>(10)*numeric_limits<TYPE_FLOAT>::epsilon())
1.415 + )
1.416 + );
1.417 +}
1.418 +
1.419 +
1.420 +template<typename TYPE_FLOAT>
1.421 +::boost::math::quaternion<TYPE_FLOAT> R3_rotation_to_quaternion( R3_matrix<TYPE_FLOAT> const & rot,
1.422 + ::boost::math::quaternion<TYPE_FLOAT> const * hint = 0)
1.423 +{
1.424 + using ::boost::math::abs;
1.425 +
1.426 + using ::std::abs;
1.427 + using ::std::sqrt;
1.428 +
1.429 + using ::std::numeric_limits;
1.430 +
1.431 + if (!is_R3_rotation_matrix(rot))
1.432 + {
1.433 + ::std::string error_reporting("Argument to R3_rotation_to_quaternion is not an R^3 rotation matrix!");
1.434 + ::std::range_error bad_argument(error_reporting);
1.435 +
1.436 + throw(bad_argument);
1.437 + }
1.438 +
1.439 + ::boost::math::quaternion<TYPE_FLOAT> q;
1.440 +
1.441 + if (
1.442 + (abs(rot.a11 - static_cast<TYPE_FLOAT>(1)) <= numeric_limits<TYPE_FLOAT>::epsilon())&&
1.443 + (abs(rot.a22 - static_cast<TYPE_FLOAT>(1)) <= numeric_limits<TYPE_FLOAT>::epsilon())&&
1.444 + (abs(rot.a33 - static_cast<TYPE_FLOAT>(1)) <= numeric_limits<TYPE_FLOAT>::epsilon())
1.445 + )
1.446 + {
1.447 + q = ::boost::math::quaternion<TYPE_FLOAT>(1);
1.448 + }
1.449 + else
1.450 + {
1.451 + TYPE_FLOAT cos_theta = (rot.a11+rot.a22+rot.a33-static_cast<TYPE_FLOAT>(1))/static_cast<TYPE_FLOAT>(2);
1.452 + TYPE_FLOAT stuff = (cos_theta+static_cast<TYPE_FLOAT>(1))/static_cast<TYPE_FLOAT>(2);
1.453 + TYPE_FLOAT cos_theta_sur_2 = sqrt(stuff);
1.454 + TYPE_FLOAT sin_theta_sur_2 = sqrt(1-stuff);
1.455 +
1.456 + TYPE_FLOAT x;
1.457 + TYPE_FLOAT y;
1.458 + TYPE_FLOAT z;
1.459 +
1.460 + find_invariant_vector(rot, x, y, z);
1.461 +
1.462 + TYPE_FLOAT u;
1.463 + TYPE_FLOAT v;
1.464 + TYPE_FLOAT w;
1.465 +
1.466 + find_orthogonal_vector(x, y, z, u, v, w);
1.467 +
1.468 + TYPE_FLOAT r;
1.469 + TYPE_FLOAT s;
1.470 + TYPE_FLOAT t;
1.471 +
1.472 + find_vector_for_BOD(x, y, z, u, v, w, r, s, t);
1.473 +
1.474 + TYPE_FLOAT ru = rot.a11*u+rot.a12*v+rot.a13*w;
1.475 + TYPE_FLOAT rv = rot.a21*u+rot.a22*v+rot.a23*w;
1.476 + TYPE_FLOAT rw = rot.a31*u+rot.a32*v+rot.a33*w;
1.477 +
1.478 + TYPE_FLOAT angle_sign_determinator = r*ru+s*rv+t*rw;
1.479 +
1.480 + if (angle_sign_determinator > +numeric_limits<TYPE_FLOAT>::epsilon())
1.481 + {
1.482 + q = ::boost::math::quaternion<TYPE_FLOAT>(cos_theta_sur_2, +x*sin_theta_sur_2, +y*sin_theta_sur_2, +z*sin_theta_sur_2);
1.483 + }
1.484 + else if (angle_sign_determinator < -numeric_limits<TYPE_FLOAT>::epsilon())
1.485 + {
1.486 + q = ::boost::math::quaternion<TYPE_FLOAT>(cos_theta_sur_2, -x*sin_theta_sur_2, -y*sin_theta_sur_2, -z*sin_theta_sur_2);
1.487 + }
1.488 + else
1.489 + {
1.490 + TYPE_FLOAT desambiguator = u*ru+v*rv+w*rw;
1.491 +
1.492 + if (desambiguator >= static_cast<TYPE_FLOAT>(1))
1.493 + {
1.494 + q = ::boost::math::quaternion<TYPE_FLOAT>(0, +x, +y, +z);
1.495 + }
1.496 + else
1.497 + {
1.498 + q = ::boost::math::quaternion<TYPE_FLOAT>(0, -x, -y, -z);
1.499 + }
1.500 + }
1.501 + }
1.502 +
1.503 + if ((hint != 0) && (abs(*hint+q) < abs(*hint-q)))
1.504 + {
1.505 + return(-q);
1.506 + }
1.507 +
1.508 + return(q);
1.509 +}
1.510 +
1.511 +#endif /* TEST_HSO3_HPP */
1.512 +