/********************************************************************************************/

/*                                                                                          */

/*                                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 */