remove RSA's md4.c, replace by DP's
[xonotic/netradiant.git] / libs / splines / math_quaternion.cpp
1 /*
2 Copyright (C) 1999-2006 Id Software, Inc. and contributors.
3 For a list of contributors, see the accompanying CONTRIBUTORS file.
4
5 This file is part of GtkRadiant.
6
7 GtkRadiant is free software; you can redistribute it and/or modify
8 it under the terms of the GNU General Public License as published by
9 the Free Software Foundation; either version 2 of the License, or
10 (at your option) any later version.
11
12 GtkRadiant is distributed in the hope that it will be useful,
13 but WITHOUT ANY WARRANTY; without even the implied warranty of
14 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
15 GNU General Public License for more details.
16
17 You should have received a copy of the GNU General Public License
18 along with GtkRadiant; if not, write to the Free Software
19 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
20 */
21
22 #include "math_quaternion.h"
23 #include "math_matrix.h"
24
25 void toQuat( idVec3 &src, quat_t &dst ) {
26         dst.x = src.x;
27         dst.y = src.y;
28         dst.z = src.z;
29         dst.w = 0.0f;
30 }
31
32 void toQuat( angles_t &src, quat_t &dst ) {
33         mat3_t temp;
34
35         toMatrix( src, temp );
36         toQuat( temp, dst );
37 }
38
39 void toQuat( mat3_t &src, quat_t &dst ) {
40         float           trace;
41         float           s;
42         int             i;
43         int                     j;
44         int                     k;
45
46         static int      next[ 3 ] = { 1, 2, 0 };
47
48         trace = src[ 0 ][ 0 ] + src[ 1 ][ 1 ] + src[ 2 ][ 2 ];
49         if ( trace > 0.0f ) {
50                 s = ( float )sqrt( trace + 1.0f );
51                 dst.w = s * 0.5f;
52                 s = 0.5f / s;
53     
54                 dst.x = ( src[ 2 ][ 1 ] - src[ 1 ][ 2 ] ) * s;
55                 dst.y = ( src[ 0 ][ 2 ] - src[ 2 ][ 0 ] ) * s;
56                 dst.z = ( src[ 1 ][ 0 ] - src[ 0 ][ 1 ] ) * s;
57         } else {
58                 i = 0;
59                 if ( src[ 1 ][ 1 ] > src[ 0 ][ 0 ] ) {
60                         i = 1;
61                 }
62                 if ( src[ 2 ][ 2 ] > src[ i ][ i ] ) {
63                         i = 2;
64                 }
65
66                 j = next[ i ];  
67                 k = next[ j ];
68     
69                 s = ( float )sqrt( ( src[ i ][ i ] - ( src[ j ][ j ] + src[ k ][ k ] ) ) + 1.0f );
70                 dst[ i ] = s * 0.5f;
71     
72                 s = 0.5f / s;
73     
74                 dst.w           = ( src[ k ][ j ] - src[ j ][ k ] ) * s;
75                 dst[ j ]        = ( src[ j ][ i ] + src[ i ][ j ] ) * s;
76                 dst[ k ]        = ( src[ k ][ i ] + src[ i ][ k ] ) * s;
77         }
78 }