libs/splines/math_quaternion.cpp

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