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         }
  58         else {
  59                 i = 0;
  60                 if ( src[ 1 ][ 1 ] > src[ 0 ][ 0 ] ) {
  61                         i = 1;
  62                 }
  63                 if ( src[ 2 ][ 2 ] > src[ i ][ i ] ) {
  64                         i = 2;
  65                 }
  66
  67                 j = next[ i ];
  68                 k = next[ j ];
  69
  70                 s = ( float )sqrt( ( src[ i ][ i ] - ( src[ j ][ j ] + src[ k ][ k ] ) ) + 1.0f );
  71                 dst[ i ] = s * 0.5f;
  72
  73                 s = 0.5f / s;
  74
  75                 dst.w       = ( src[ k ][ j ] - src[ j ][ k ] ) * s;
  76                 dst[ j ]    = ( src[ j ][ i ] + src[ i ][ j ] ) * s;
  77                 dst[ k ]    = ( src[ k ][ i ] + src[ i ][ k ] ) * s;
  78         }
  79 }