
/********************************************************************************************/
/*                                                                                          */
/*                                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 usefull too.                             */
/*                                                                                          */
/* This file provides tools to convert between quaternions and R^3 rotation matrices.       */
/*                                                                                          */
/********************************************************************************************/

//  (C) Copyright Hubert Holin 2001. Permission to copy, use, modify, sell and
//  distribute this software is granted provided this copyright notice appears
//  in all copies. This software is provided "as is" without express or implied
//  warranty, and with no claim as to its suitability for any purpose.

#ifndef TEST_HSO3_HPP
#define TEST_HSO3_HPP

#include <algorithm>
#include <limits>
#include <stdexcept>
#include <string>

#include <boost/math/quaternion.hpp>

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::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);
}


namespace
{
   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::quaternion<TYPE_FLOAT>   R3_rotation_to_quaternion(R3_matrix<TYPE_FLOAT> const & rot,
                                                            ::boost::quaternion<TYPE_FLOAT> const * hint = 0)
{
   using   ::std::abs;
   using   ::std::sqrt;
   
   using   ::std::numeric_limits;
   
   using   ::boost::abs;
   
   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::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::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::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::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::quaternion<TYPE_FLOAT>(0, +x, +y, +z);
         }
         else
         {
            q = ::boost::quaternion<TYPE_FLOAT>(0, -x, -y, -z);
         }
      }
   }
   
   if   ((hint != 0) && (abs(*hint+q) < abs(*hint-q)))
   {
      return(-q);
   }
   
   return(q);
}

#endif /* TEST_HSO3_HPP */
