2019-12-30 15:42:10 +01:00
/*
LA : linear algebra C + + interface library
Copyright ( C ) 2008 - 2020 Jiri Pittner < jiri . pittner @ jh - inst . cas . cz > or < jiri @ pittnerovi . com >
This program is free software : you can redistribute it and / or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation , either version 3 of the License , or
( at your option ) any later version .
This program is distributed in the hope that it will be useful ,
but WITHOUT ANY WARRANTY ; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE . See the
GNU General Public License for more details .
You should have received a copy of the GNU General Public License
along with this program . If not , see < http : //www.gnu.org/licenses/>.
*/
2019-12-31 18:49:39 +01:00
//this actually should be compilable separately from LA as well as being a part of LA
2019-12-30 15:42:10 +01:00
# ifndef _QUATERNION_H_
# define _QUATERNION_H_
# include <stdlib.h>
2019-12-30 16:41:36 +01:00
# include <iostream>
2019-12-31 18:49:39 +01:00
# include <complex>
# include <cstring>
# include <math.h>
2019-12-30 15:42:10 +01:00
template < typename T >
class Quaternion
{
public :
2019-12-30 16:15:12 +01:00
//just plain old data
2019-12-30 15:42:10 +01:00
T q [ 4 ] ;
2019-12-30 16:15:12 +01:00
//
2019-12-30 15:42:10 +01:00
Quaternion ( void ) { } ;
2019-12-31 18:49:39 +01:00
Quaternion ( const T x , const T u = 0 , const T v = 0 , const T w = 0 ) { q [ 0 ] = x ; q [ 1 ] = u ; q [ 2 ] = v ; q [ 3 ] = w ; } ; //quaternion from real(s)
Quaternion ( const std : : complex < T > & rhs ) { q [ 0 ] = rhs . real ( ) ; q [ 1 ] = rhs . imag ( ) ; q [ 2 ] = 0 ; q [ 3 ] = 0 ; } //quaternion from complex
2020-01-01 10:22:55 +01:00
explicit Quaternion ( const T * x , const int shift = 1 ) { q [ 0 ] = 0 ; memcpy ( q + shift , x , ( 4 - shift ) * sizeof ( T ) ) ; } //for shift=1 quaternion from xyz vector
2019-12-30 16:15:12 +01:00
//compiler generates default copy constructor and assignment operator
2019-12-30 16:41:36 +01:00
//formal indexing
const T operator [ ] ( const int i ) const { return this - > q [ i ] ; } ;
2019-12-31 18:49:39 +01:00
T & operator [ ] ( const int i ) { return this - > q [ i ] ; } ;
2019-12-30 16:15:12 +01:00
//operations of quaternions with scalars
Quaternion & operator = ( const T x ) { q [ 0 ] = x ; memset ( & q [ 1 ] , 0 , 3 * sizeof ( T ) ) ; return * this ; } ; //quaternion from real
Quaternion & operator + = ( const T rhs ) { this - > q [ 0 ] + = rhs ; return * this ; } ;
Quaternion & operator - = ( const T rhs ) { this - > q [ 0 ] - = rhs ; return * this ; } ;
Quaternion & operator * = ( const T rhs ) { this - > q [ 0 ] * = rhs ; this - > q [ 1 ] * = rhs ; this - > q [ 2 ] * = rhs ; this - > q [ 3 ] * = rhs ; return * this ; } ;
Quaternion & operator / = ( const T rhs ) { return * this * = ( ( T ) 1 / rhs ) ; } ;
const Quaternion operator + ( const T rhs ) const { return Quaternion ( * this ) + = rhs ; } ;
const Quaternion operator - ( const T rhs ) const { return Quaternion ( * this ) - = rhs ; } ;
const Quaternion operator * ( const T rhs ) const { return Quaternion ( * this ) * = rhs ; } ;
const Quaternion operator / ( const T rhs ) const { return Quaternion ( * this ) / = rhs ; } ;
//quaternion algebra
2019-12-31 18:49:39 +01:00
const Quaternion operator - ( ) const { Quaternion r ( * this ) ; r . q [ 0 ] = - r . q [ 0 ] ; r . q [ 1 ] = - r . q [ 1 ] ; r . q [ 2 ] = - r . q [ 2 ] ; r . q [ 3 ] = - r . q [ 3 ] ; return r ; } ; //unary minus
2019-12-30 16:15:12 +01:00
Quaternion & operator + = ( const Quaternion & rhs ) { this - > q [ 0 ] + = rhs . q [ 0 ] ; this - > q [ 1 ] + = rhs . q [ 1 ] ; this - > q [ 2 ] + = rhs . q [ 2 ] ; this - > q [ 3 ] + = rhs . q [ 3 ] ; return * this ; } ;
Quaternion & operator - = ( const Quaternion & rhs ) { this - > q [ 0 ] - = rhs . q [ 0 ] ; this - > q [ 1 ] - = rhs . q [ 1 ] ; this - > q [ 2 ] - = rhs . q [ 2 ] ; this - > q [ 3 ] - = rhs . q [ 3 ] ; return * this ; } ;
const Quaternion operator + ( const Quaternion & rhs ) const { return Quaternion ( * this ) + = rhs ; } ;
const Quaternion operator - ( const Quaternion & rhs ) const { return Quaternion ( * this ) - = rhs ; } ;
2019-12-31 18:49:39 +01:00
const Quaternion operator * ( const Quaternion & rhs ) const ;
2019-12-30 16:41:36 +01:00
Quaternion & conjugateme ( void ) { q [ 1 ] = - q [ 1 ] ; q [ 2 ] = - q [ 2 ] ; q [ 3 ] = - q [ 3 ] ; return * this ; }
Quaternion conjugate ( void ) const { return Quaternion ( * this ) . conjugateme ( ) ; }
T normsqr ( void ) const { return q [ 0 ] * q [ 0 ] + q [ 1 ] * q [ 1 ] + q [ 2 ] * q [ 2 ] + q [ 3 ] * q [ 3 ] ; } ;
2019-12-31 18:49:39 +01:00
T norm ( void ) const { return sqrt ( this - > normsqr ( ) ) ; } ;
Quaternion & normalize ( bool unique_sign = false ) { * this / = this - > norm ( ) ; if ( unique_sign & & q [ 0 ] < 0 ) * this * = ( T ) - 1 ; return * this ; } ;
2019-12-30 16:41:36 +01:00
Quaternion inverse ( void ) const { return Quaternion ( * this ) . conjugateme ( ) / this - > normsqr ( ) ; } ;
const Quaternion operator / ( const Quaternion & rhs ) const { return * this * rhs . inverse ( ) ; } ;
2020-01-01 10:58:51 +01:00
Quaternion rotateby ( const Quaternion & rhs ) ; //conjugation-rotation of this by NORMALIZED rhs
void rotate ( T * to , const T * from ) const ; //rotate xyz vector by NORMALIZED *this
2019-12-30 15:42:10 +01:00
} ;
2019-12-30 16:41:36 +01:00
2019-12-30 16:15:12 +01:00
//stream I/O
template < typename T >
std : : istream & operator > > ( std : : istream & s , Quaternion < T > & x )
{
s > > x . q [ 0 ] ;
s > > x . q [ 1 ] ;
s > > x . q [ 2 ] ;
s > > x . q [ 3 ] ;
return s ;
}
template < typename T >
std : : ostream & operator < < ( std : : ostream & s , const Quaternion < T > & x ) {
s < < x . q [ 0 ] < < " " ;
s < < x . q [ 1 ] < < " " ;
s < < x . q [ 2 ] < < " " ;
s < < x . q [ 3 ] ;
return s ;
}
2019-12-31 18:49:39 +01:00
//"euler" or Tait-Bryan angles [corresponding to meul -r -T xyz -d -t -R]
template < typename T >
2020-01-01 10:22:55 +01:00
void normquat2euler ( const Quaternion < T > & q , T * ) ;
2019-12-31 18:49:39 +01:00
2020-01-01 10:58:51 +01:00
//the following must be in .h due to the generic M type which is unspecified and can be any type providing [][], either plain C matrix or std::matrix or LA matrix
2019-12-31 18:49:39 +01:00
//conversion between quanternions and rotation matrices
//
template < typename T , typename M >
void normquat2rotmat ( const Quaternion < T > & q , M & a )
{
a [ 0 ] [ 0 ] = 2 * q [ 0 ] * q [ 0 ] - 1 + 2 * q [ 1 ] * q [ 1 ] ;
a [ 0 ] [ 1 ] = 2 * ( q [ 1 ] * q [ 2 ] + q [ 0 ] * q [ 3 ] ) ;
a [ 0 ] [ 2 ] = 2 * ( q [ 1 ] * q [ 3 ] - q [ 0 ] * q [ 2 ] ) ;
a [ 1 ] [ 0 ] = 2 * ( q [ 1 ] * q [ 2 ] - q [ 0 ] * q [ 3 ] ) ;
a [ 1 ] [ 1 ] = 2 * q [ 0 ] * q [ 0 ] - 1 + 2 * q [ 2 ] * q [ 2 ] ;
a [ 1 ] [ 2 ] = 2 * ( q [ 2 ] * q [ 3 ] + q [ 0 ] * q [ 1 ] ) ;
a [ 2 ] [ 0 ] = 2 * ( q [ 1 ] * q [ 3 ] + q [ 0 ] * q [ 2 ] ) ;
a [ 2 ] [ 1 ] = 2 * ( q [ 2 ] * q [ 3 ] - q [ 0 ] * q [ 1 ] ) ;
a [ 2 ] [ 2 ] = 2 * q [ 0 ] * q [ 0 ] - 1 + 2 * q [ 3 ] * q [ 3 ] ;
}
template < typename T , typename M >
void quat2rotmat ( Quaternion < T > q , M & a , const bool already_normalized = false )
{
if ( ! already_normalized ) q . normalize ( ) ;
normquat2rotmat ( q , a ) ;
}
//normalized quaternion from rotation matrix
//convention compatible with the paper on MEMS sensors by Sebastian O.H. Madgwick
//the rotation matrix correcponds to transpose of (4) in Sarabandi and Thomas paper
//where the method is described
template < typename T , typename M >
void rotmat2normquat ( const M & a , Quaternion < T > & q )
{
T tr = a [ 0 ] [ 0 ] + a [ 1 ] [ 1 ] + a [ 2 ] [ 2 ] ;
if ( tr > = 0 )
{
q [ 0 ] = ( T ) .5 * sqrt ( ( T ) 1. + tr ) ;
q [ 1 ] = ( T ) .5 * sqrt ( ( T ) 1. + a [ 0 ] [ 0 ] - a [ 1 ] [ 1 ] - a [ 2 ] [ 2 ] ) ;
q [ 2 ] = ( T ) .5 * sqrt ( ( T ) 1. - a [ 0 ] [ 0 ] + a [ 1 ] [ 1 ] - a [ 2 ] [ 2 ] ) ;
q [ 3 ] = ( T ) .5 * sqrt ( ( T ) 1. - a [ 0 ] [ 0 ] - a [ 1 ] [ 1 ] + a [ 2 ] [ 2 ] ) ;
}
else
{
T a12p = a [ 0 ] [ 1 ] + a [ 1 ] [ 0 ] ;
T a12m = a [ 0 ] [ 1 ] - a [ 1 ] [ 0 ] ;
T a13p = a [ 0 ] [ 2 ] + a [ 2 ] [ 0 ] ;
T a13m = a [ 0 ] [ 2 ] - a [ 2 ] [ 0 ] ;
T a23p = a [ 1 ] [ 2 ] + a [ 2 ] [ 1 ] ;
T a23m = a [ 1 ] [ 2 ] - a [ 2 ] [ 1 ] ;
q [ 0 ] = ( T ) .5 * sqrt ( ( a23m * a23m + a13m * a13m + a12m * a12m ) / ( ( T ) 3. - tr ) ) ;
q [ 1 ] = ( T ) .5 * sqrt ( ( a23m * a23m + a12p * a12p + a13p * a13p ) / ( ( T ) 3. - a [ 0 ] [ 0 ] + a [ 1 ] [ 1 ] + a [ 2 ] [ 2 ] ) ) ;
q [ 2 ] = ( T ) .5 * sqrt ( ( a13m * a13m + a12p * a12p + a23p * a23p ) / ( ( T ) 3. + a [ 0 ] [ 0 ] - a [ 1 ] [ 1 ] + a [ 2 ] [ 2 ] ) ) ;
q [ 3 ] = ( T ) .5 * sqrt ( ( a12m * a12m + a13p * a13p + a23p * a23p ) / ( ( T ) 3. + a [ 0 ] [ 0 ] + a [ 1 ] [ 1 ] - a [ 2 ] [ 2 ] ) ) ;
}
if ( a [ 1 ] [ 2 ] - a [ 2 ] [ 1 ] < 0 ) q [ 1 ] = - q [ 1 ] ;
if ( a [ 2 ] [ 0 ] - a [ 0 ] [ 2 ] < 0 ) q [ 2 ] = - q [ 2 ] ;
if ( a [ 0 ] [ 1 ] - a [ 1 ] [ 0 ] < 0 ) q [ 3 ] = - q [ 3 ] ;
}
2020-01-01 10:58:51 +01:00
//rotation about unit vector axis (given by pointer for simplicity) through an angle to a normalized quaternion
2020-01-01 10:22:55 +01:00
# ifndef AVOID_GONIOM_FUNC
template < typename T >
void axis2normquat ( const T * axis , const T & angle , Quaternion < T > & q ) ;
template < typename T >
void normquat2axis ( const Quaternion < T > & q , T * axis , T & angle ) ;
# endif
2019-12-30 15:42:10 +01:00
# endif /* _QUATERNION_H_ */