36 #include "DTrackDataTypes.hpp" 45 DTrackQuaternion rot2quat(
const double rot[ 9 ] )
47 DTrackQuaternion quat;
50 tr = rot[ 0 ] + rot[ 4 ] + rot[ 8 ];
54 s = std::sqrt( 1.0 + tr );
60 quat.x = ( rot[ 5 ] - rot[ 7 ] ) * s ;
61 quat.y = ( rot[ 6 ] - rot[ 2 ] ) * s ;
62 quat.z = ( rot[ 1 ] - rot[ 3 ] ) * s ;
66 if ( ( rot[ 0 ] > rot[ 4 ] ) && ( rot[ 0 ] > rot[ 8 ] ) )
68 s = std::sqrt( 1.0 + rot[ 0 ] - rot[ 4 ] - rot[ 8 ] );
74 quat.y = ( rot[ 1 ] + rot[ 3 ] ) * s ;
75 quat.z = ( rot[ 2 ] + rot[ 6 ] ) * s ;
76 quat.w = ( rot[ 5 ] - rot[ 7 ] ) * s ;
78 else if ( rot[ 4 ] > rot[ 8 ] )
80 s = std::sqrt( 1.0 - rot[ 0 ] + rot[ 4 ] - rot[ 8 ] );
86 quat.x = ( rot[ 1 ] + rot[ 3 ] ) * s ;
87 quat.z = ( rot[ 5 ] + rot[ 7 ] ) * s ;
88 quat.w = ( rot[ 6 ] - rot[ 2 ] ) * s ;
92 s = std::sqrt( 1.0 - rot[ 0 ] - rot[ 4 ] + rot[ 8 ] );
98 quat.x = ( rot[ 2 ] + rot[ 6 ] ) * s ;
99 quat.y = ( rot[ 5 ] + rot[ 7 ] ) * s ;
100 quat.w = ( rot[ 1 ] - rot[ 3 ] ) * s ;