summaryrefslogtreecommitdiffstats
path: root/src/boost/libs/math/example/HSO3.hpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/boost/libs/math/example/HSO3.hpp')
-rw-r--r--src/boost/libs/math/example/HSO3.hpp509
1 files changed, 509 insertions, 0 deletions
diff --git a/src/boost/libs/math/example/HSO3.hpp b/src/boost/libs/math/example/HSO3.hpp
new file mode 100644
index 00000000..4e4ead7a
--- /dev/null
+++ b/src/boost/libs/math/example/HSO3.hpp
@@ -0,0 +1,509 @@
+
+/********************************************************************************************/
+/* */
+/* HSO3.hpp header file */
+/* */
+/* This file is not currently part of the Boost library. It is simply an example of the use */
+/* quaternions can be put to. Hopefully it will be useful too. */
+/* */
+/* This file provides tools to convert between quaternions and R^3 rotation matrices. */
+/* */
+/********************************************************************************************/
+
+// (C) Copyright Hubert Holin 2001.
+// Distributed under the Boost Software License, Version 1.0. (See
+// accompanying file LICENSE_1_0.txt or copy at
+// http://www.boost.org/LICENSE_1_0.txt)
+
+#ifndef TEST_HSO3_HPP
+#define TEST_HSO3_HPP
+
+#include <algorithm>
+
+#if defined(__GNUC__) && (__GNUC__ < 3)
+#include <boost/limits.hpp>
+#else
+#include <limits>
+#endif
+
+#include <stdexcept>
+#include <string>
+
+#include <boost/math/quaternion.hpp>
+
+
+#if defined(__GNUC__) && (__GNUC__ < 3)
+// gcc 2.x ignores function scope using declarations, put them here instead:
+using namespace ::std;
+using namespace ::boost::math;
+#endif
+
+template<typename TYPE_FLOAT>
+struct R3_matrix
+{
+ TYPE_FLOAT a11, a12, a13;
+ TYPE_FLOAT a21, a22, a23;
+ TYPE_FLOAT a31, a32, a33;
+};
+
+
+// Note: the input quaternion need not be of norm 1 for the following function
+
+template<typename TYPE_FLOAT>
+R3_matrix<TYPE_FLOAT> quaternion_to_R3_rotation(::boost::math::quaternion<TYPE_FLOAT> const & q)
+{
+ using ::std::numeric_limits;
+
+ TYPE_FLOAT a = q.R_component_1();
+ TYPE_FLOAT b = q.R_component_2();
+ TYPE_FLOAT c = q.R_component_3();
+ TYPE_FLOAT d = q.R_component_4();
+
+ TYPE_FLOAT aa = a*a;
+ TYPE_FLOAT ab = a*b;
+ TYPE_FLOAT ac = a*c;
+ TYPE_FLOAT ad = a*d;
+ TYPE_FLOAT bb = b*b;
+ TYPE_FLOAT bc = b*c;
+ TYPE_FLOAT bd = b*d;
+ TYPE_FLOAT cc = c*c;
+ TYPE_FLOAT cd = c*d;
+ TYPE_FLOAT dd = d*d;
+
+ TYPE_FLOAT norme_carre = aa+bb+cc+dd;
+
+ if (norme_carre <= numeric_limits<TYPE_FLOAT>::epsilon())
+ {
+ ::std::string error_reporting("Argument to quaternion_to_R3_rotation is too small!");
+ ::std::underflow_error bad_argument(error_reporting);
+
+ throw(bad_argument);
+ }
+
+ R3_matrix<TYPE_FLOAT> out_matrix;
+
+ out_matrix.a11 = (aa+bb-cc-dd)/norme_carre;
+ out_matrix.a12 = 2*(-ad+bc)/norme_carre;
+ out_matrix.a13 = 2*(ac+bd)/norme_carre;
+ out_matrix.a21 = 2*(ad+bc)/norme_carre;
+ out_matrix.a22 = (aa-bb+cc-dd)/norme_carre;
+ out_matrix.a23 = 2*(-ab+cd)/norme_carre;
+ out_matrix.a31 = 2*(-ac+bd)/norme_carre;
+ out_matrix.a32 = 2*(ab+cd)/norme_carre;
+ out_matrix.a33 = (aa-bb-cc+dd)/norme_carre;
+
+ return(out_matrix);
+}
+
+
+ template<typename TYPE_FLOAT>
+ void find_invariant_vector( R3_matrix<TYPE_FLOAT> const & rot,
+ TYPE_FLOAT & x,
+ TYPE_FLOAT & y,
+ TYPE_FLOAT & z)
+ {
+ using ::std::sqrt;
+
+ using ::std::numeric_limits;
+
+ TYPE_FLOAT b11 = rot.a11 - static_cast<TYPE_FLOAT>(1);
+ TYPE_FLOAT b12 = rot.a12;
+ TYPE_FLOAT b13 = rot.a13;
+ TYPE_FLOAT b21 = rot.a21;
+ TYPE_FLOAT b22 = rot.a22 - static_cast<TYPE_FLOAT>(1);
+ TYPE_FLOAT b23 = rot.a23;
+ TYPE_FLOAT b31 = rot.a31;
+ TYPE_FLOAT b32 = rot.a32;
+ TYPE_FLOAT b33 = rot.a33 - static_cast<TYPE_FLOAT>(1);
+
+ TYPE_FLOAT minors[9] =
+ {
+ b11*b22-b12*b21,
+ b11*b23-b13*b21,
+ b12*b23-b13*b22,
+ b11*b32-b12*b31,
+ b11*b33-b13*b31,
+ b12*b33-b13*b32,
+ b21*b32-b22*b31,
+ b21*b33-b23*b31,
+ b22*b33-b23*b32
+ };
+
+ TYPE_FLOAT * where = ::std::max_element(minors, minors+9);
+
+ TYPE_FLOAT det = *where;
+
+ if (det <= numeric_limits<TYPE_FLOAT>::epsilon())
+ {
+ ::std::string error_reporting("Underflow error in find_invariant_vector!");
+ ::std::underflow_error processing_error(error_reporting);
+
+ throw(processing_error);
+ }
+
+ switch (where-minors)
+ {
+ case 0:
+
+ z = static_cast<TYPE_FLOAT>(1);
+
+ x = (-b13*b22+b12*b23)/det;
+ y = (-b11*b23+b13*b21)/det;
+
+ break;
+
+ case 1:
+
+ y = static_cast<TYPE_FLOAT>(1);
+
+ x = (-b12*b23+b13*b22)/det;
+ z = (-b11*b22+b12*b21)/det;
+
+ break;
+
+ case 2:
+
+ x = static_cast<TYPE_FLOAT>(1);
+
+ y = (-b11*b23+b13*b21)/det;
+ z = (-b12*b21+b11*b22)/det;
+
+ break;
+
+ case 3:
+
+ z = static_cast<TYPE_FLOAT>(1);
+
+ x = (-b13*b32+b12*b33)/det;
+ y = (-b11*b33+b13*b31)/det;
+
+ break;
+
+ case 4:
+
+ y = static_cast<TYPE_FLOAT>(1);
+
+ x = (-b12*b33+b13*b32)/det;
+ z = (-b11*b32+b12*b31)/det;
+
+ break;
+
+ case 5:
+
+ x = static_cast<TYPE_FLOAT>(1);
+
+ y = (-b11*b33+b13*b31)/det;
+ z = (-b12*b31+b11*b32)/det;
+
+ break;
+
+ case 6:
+
+ z = static_cast<TYPE_FLOAT>(1);
+
+ x = (-b23*b32+b22*b33)/det;
+ y = (-b21*b33+b23*b31)/det;
+
+ break;
+
+ case 7:
+
+ y = static_cast<TYPE_FLOAT>(1);
+
+ x = (-b22*b33+b23*b32)/det;
+ z = (-b21*b32+b22*b31)/det;
+
+ break;
+
+ case 8:
+
+ x = static_cast<TYPE_FLOAT>(1);
+
+ y = (-b21*b33+b23*b31)/det;
+ z = (-b22*b31+b21*b32)/det;
+
+ break;
+
+ default:
+
+ ::std::string error_reporting("Impossible condition in find_invariant_vector");
+ ::std::logic_error processing_error(error_reporting);
+
+ throw(processing_error);
+
+ break;
+ }
+
+ TYPE_FLOAT vecnorm = sqrt(x*x+y*y+z*z);
+
+ if (vecnorm <= numeric_limits<TYPE_FLOAT>::epsilon())
+ {
+ ::std::string error_reporting("Overflow error in find_invariant_vector!");
+ ::std::overflow_error processing_error(error_reporting);
+
+ throw(processing_error);
+ }
+
+ x /= vecnorm;
+ y /= vecnorm;
+ z /= vecnorm;
+ }
+
+
+ template<typename TYPE_FLOAT>
+ void find_orthogonal_vector( TYPE_FLOAT x,
+ TYPE_FLOAT y,
+ TYPE_FLOAT z,
+ TYPE_FLOAT & u,
+ TYPE_FLOAT & v,
+ TYPE_FLOAT & w)
+ {
+ using ::std::abs;
+ using ::std::sqrt;
+
+ using ::std::numeric_limits;
+
+ TYPE_FLOAT vecnormsqr = x*x+y*y+z*z;
+
+ if (vecnormsqr <= numeric_limits<TYPE_FLOAT>::epsilon())
+ {
+ ::std::string error_reporting("Underflow error in find_orthogonal_vector!");
+ ::std::underflow_error processing_error(error_reporting);
+
+ throw(processing_error);
+ }
+
+ TYPE_FLOAT lambda;
+
+ TYPE_FLOAT components[3] =
+ {
+ abs(x),
+ abs(y),
+ abs(z)
+ };
+
+ TYPE_FLOAT * where = ::std::min_element(components, components+3);
+
+ switch (where-components)
+ {
+ case 0:
+
+ if (*where <= numeric_limits<TYPE_FLOAT>::epsilon())
+ {
+ v =
+ w = static_cast<TYPE_FLOAT>(0);
+ u = static_cast<TYPE_FLOAT>(1);
+ }
+ else
+ {
+ lambda = -x/vecnormsqr;
+
+ u = static_cast<TYPE_FLOAT>(1) + lambda*x;
+ v = lambda*y;
+ w = lambda*z;
+ }
+
+ break;
+
+ case 1:
+
+ if (*where <= numeric_limits<TYPE_FLOAT>::epsilon())
+ {
+ u =
+ w = static_cast<TYPE_FLOAT>(0);
+ v = static_cast<TYPE_FLOAT>(1);
+ }
+ else
+ {
+ lambda = -y/vecnormsqr;
+
+ u = lambda*x;
+ v = static_cast<TYPE_FLOAT>(1) + lambda*y;
+ w = lambda*z;
+ }
+
+ break;
+
+ case 2:
+
+ if (*where <= numeric_limits<TYPE_FLOAT>::epsilon())
+ {
+ u =
+ v = static_cast<TYPE_FLOAT>(0);
+ w = static_cast<TYPE_FLOAT>(1);
+ }
+ else
+ {
+ lambda = -z/vecnormsqr;
+
+ u = lambda*x;
+ v = lambda*y;
+ w = static_cast<TYPE_FLOAT>(1) + lambda*z;
+ }
+
+ break;
+
+ default:
+
+ ::std::string error_reporting("Impossible condition in find_invariant_vector");
+ ::std::logic_error processing_error(error_reporting);
+
+ throw(processing_error);
+
+ break;
+ }
+
+ TYPE_FLOAT vecnorm = sqrt(u*u+v*v+w*w);
+
+ if (vecnorm <= numeric_limits<TYPE_FLOAT>::epsilon())
+ {
+ ::std::string error_reporting("Underflow error in find_orthogonal_vector!");
+ ::std::underflow_error processing_error(error_reporting);
+
+ throw(processing_error);
+ }
+
+ u /= vecnorm;
+ v /= vecnorm;
+ w /= vecnorm;
+ }
+
+
+ // Note: we want [[v, v, w], [r, s, t], [x, y, z]] to be a direct orthogonal basis
+ // of R^3. It might not be orthonormal, however, and we do not check if the
+ // two input vectors are colinear or not.
+
+ template<typename TYPE_FLOAT>
+ void find_vector_for_BOD(TYPE_FLOAT x,
+ TYPE_FLOAT y,
+ TYPE_FLOAT z,
+ TYPE_FLOAT u,
+ TYPE_FLOAT v,
+ TYPE_FLOAT w,
+ TYPE_FLOAT & r,
+ TYPE_FLOAT & s,
+ TYPE_FLOAT & t)
+ {
+ r = +y*w-z*v;
+ s = -x*w+z*u;
+ t = +x*v-y*u;
+ }
+
+
+
+template<typename TYPE_FLOAT>
+inline bool is_R3_rotation_matrix(R3_matrix<TYPE_FLOAT> const & mat)
+{
+ using ::std::abs;
+
+ using ::std::numeric_limits;
+
+ return (
+ !(
+ (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())||
+ (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())||
+ (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())||
+ //(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())||
+ (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())||
+ (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())||
+ //(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())||
+ //(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())||
+ (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())
+ )
+ );
+}
+
+
+template<typename TYPE_FLOAT>
+::boost::math::quaternion<TYPE_FLOAT> R3_rotation_to_quaternion( R3_matrix<TYPE_FLOAT> const & rot,
+ ::boost::math::quaternion<TYPE_FLOAT> const * hint = 0)
+{
+ using ::boost::math::abs;
+
+ using ::std::abs;
+ using ::std::sqrt;
+
+ using ::std::numeric_limits;
+
+ if (!is_R3_rotation_matrix(rot))
+ {
+ ::std::string error_reporting("Argument to R3_rotation_to_quaternion is not an R^3 rotation matrix!");
+ ::std::range_error bad_argument(error_reporting);
+
+ throw(bad_argument);
+ }
+
+ ::boost::math::quaternion<TYPE_FLOAT> q;
+
+ if (
+ (abs(rot.a11 - static_cast<TYPE_FLOAT>(1)) <= numeric_limits<TYPE_FLOAT>::epsilon())&&
+ (abs(rot.a22 - static_cast<TYPE_FLOAT>(1)) <= numeric_limits<TYPE_FLOAT>::epsilon())&&
+ (abs(rot.a33 - static_cast<TYPE_FLOAT>(1)) <= numeric_limits<TYPE_FLOAT>::epsilon())
+ )
+ {
+ q = ::boost::math::quaternion<TYPE_FLOAT>(1);
+ }
+ else
+ {
+ TYPE_FLOAT cos_theta = (rot.a11+rot.a22+rot.a33-static_cast<TYPE_FLOAT>(1))/static_cast<TYPE_FLOAT>(2);
+ TYPE_FLOAT stuff = (cos_theta+static_cast<TYPE_FLOAT>(1))/static_cast<TYPE_FLOAT>(2);
+ TYPE_FLOAT cos_theta_sur_2 = sqrt(stuff);
+ TYPE_FLOAT sin_theta_sur_2 = sqrt(1-stuff);
+
+ TYPE_FLOAT x;
+ TYPE_FLOAT y;
+ TYPE_FLOAT z;
+
+ find_invariant_vector(rot, x, y, z);
+
+ TYPE_FLOAT u;
+ TYPE_FLOAT v;
+ TYPE_FLOAT w;
+
+ find_orthogonal_vector(x, y, z, u, v, w);
+
+ TYPE_FLOAT r;
+ TYPE_FLOAT s;
+ TYPE_FLOAT t;
+
+ find_vector_for_BOD(x, y, z, u, v, w, r, s, t);
+
+ TYPE_FLOAT ru = rot.a11*u+rot.a12*v+rot.a13*w;
+ TYPE_FLOAT rv = rot.a21*u+rot.a22*v+rot.a23*w;
+ TYPE_FLOAT rw = rot.a31*u+rot.a32*v+rot.a33*w;
+
+ TYPE_FLOAT angle_sign_determinator = r*ru+s*rv+t*rw;
+
+ if (angle_sign_determinator > +numeric_limits<TYPE_FLOAT>::epsilon())
+ {
+ 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);
+ }
+ else if (angle_sign_determinator < -numeric_limits<TYPE_FLOAT>::epsilon())
+ {
+ 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);
+ }
+ else
+ {
+ TYPE_FLOAT desambiguator = u*ru+v*rv+w*rw;
+
+ if (desambiguator >= static_cast<TYPE_FLOAT>(1))
+ {
+ q = ::boost::math::quaternion<TYPE_FLOAT>(0, +x, +y, +z);
+ }
+ else
+ {
+ q = ::boost::math::quaternion<TYPE_FLOAT>(0, -x, -y, -z);
+ }
+ }
+ }
+
+ if ((hint != 0) && (abs(*hint+q) < abs(*hint-q)))
+ {
+ return(-q);
+ }
+
+ return(q);
+}
+
+#endif /* TEST_HSO3_HPP */
+
generated by cgit v1.2.3 (git 2.39.1) at 2024-12-30 19:08:40 +0000