quaternion.cpp
上传用户:gb3593
上传日期:2022-01-07
资源大小:3028k
文件大小:6k
源码类别:

游戏引擎

开发平台:

Visual C++

  1. /***********************************************************************
  2.   quaternion.cpp - A quaternion class
  3.   -------------------------------------------------------------------
  4.   GLUI User Interface Toolkit (LGPL)
  5.   Copyright (c) 1998 Paul Rademacher
  6.   WWW:    http://sourceforge.net/projects/glui/
  7.   Forums: http://sourceforge.net/forum/?group_id=92496
  8.   This library is free software; you can redistribute it and/or
  9.   modify it under the terms of the GNU Lesser General Public
  10.   License as published by the Free Software Foundation; either
  11.   version 2.1 of the License, or (at your option) any later version.
  12.   This library 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 GNU
  15.   Lesser General Public License for more details.
  16.   You should have received a copy of the GNU Lesser General Public
  17.   License along with this library; if not, write to the Free Software
  18.   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  19. ************************************************************************
  20.   Feb 1998, Paul Rademacher (rademach@cs.unc.edu)
  21.   Oct 2003, Nigel Stewart - GLUI Code Cleaning
  22.   
  23. ************************************************************************/
  24. #include "quaternion.h"
  25. #include <cmath>
  26. #include "glui_internal.h"
  27. /******************************************* constructors **************/
  28. quat::quat()
  29. {
  30.     *this = quat_identity();
  31. }
  32. quat::quat(const float x, const float y, const float z, const float w)
  33. {
  34.     v.set( x, y, z );
  35.     s = w;
  36. }
  37. quat::quat(const vec3 &_v, const float _s)
  38. {
  39.     set( _v, _s );
  40. }
  41. quat::quat(const float _s, const vec3 &_v)
  42. {
  43.     set( _v, _s );
  44. }
  45. quat::quat(const float *d)
  46. {
  47.     v[0] = d[0];
  48.     v[1] = d[1];
  49.     v[2] = d[2];
  50.     s    = d[3];
  51. }
  52. quat::quat(const double *d)
  53. {
  54.     v[0] = (float) d[0];
  55.     v[1] = (float) d[1];
  56.     v[2] = (float) d[2];
  57.     s    = (float) d[3];
  58. }
  59. quat::quat(const quat &q)
  60. {
  61.     v = q.v;
  62.     s = q.s;
  63. }
  64. void quat::set(const vec3 &_v, const float _s)
  65. {
  66.     v = _v;
  67.     s = _s;
  68. }
  69. quat &quat::operator=(const quat &q)
  70.     v = q.v;  
  71.     s = q.s; 
  72.     return *this; 
  73. }
  74. /******** quat friends ************/
  75. quat operator + (const quat &a, const quat &b)
  76. {
  77.     return quat( a.s+b.s, a.v+b.v );
  78. }
  79. quat operator - (const quat &a, const quat &b)
  80. {
  81.     return quat( a.s-b.s, a.v-b.v );
  82. }
  83. quat operator - (const quat &a )
  84. {
  85.     return quat( -a.s, -a.v );
  86. }
  87. quat operator * ( const quat &a, const quat &b)
  88. {
  89.     return quat( a.s*b.s - a.v*b.v, a.s*b.v + b.s*a.v + a.v^b.v );
  90. }
  91. quat operator * ( const quat &a, const float t)
  92. {
  93.     return quat( a.v * t, a.s * t );
  94. }
  95. quat operator * ( const float t, const quat &a )
  96. {
  97.     return quat( a.v * t, a.s * t );
  98. }
  99. mat4 quat::to_mat4() const
  100. {
  101.     float xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz;
  102.     float t  = 2.0f / (v*v + s*s);
  103.     xs = v[VX]*t;   ys = v[VY]*t;   zs = v[VZ]*t;
  104.     wx = s*xs;      wy = s*ys;      wz = s*zs;
  105.     xx = v[VX]*xs;  xy = v[VX]*ys;  xz = v[VX]*zs;
  106.     yy = v[VY]*ys;  yz = v[VY]*zs;  zz = v[VZ]*zs;
  107.     mat4 matrix( 
  108.            1.0f-(yy+zz), xy+wz,        xz-wy,        0.0f,
  109.            xy-wz,        1.0f-(xx+zz), yz+wx,        0.0f,
  110.            xz+wy,        yz-wx,        1.0f-(xx+yy), 0.0f,
  111.            0.0f,         0.0f,         0.0f,         1.0f );
  112.     return matrix;
  113. }
  114. /************************************************* quat_identity() *****/
  115. /* Returns quaternion identity element                                 */
  116. quat quat_identity() 
  117. {
  118.     return quat( vec3( 0.0, 0.0, 0.0 ), 1.0 );
  119. }
  120. /************************************************ quat_slerp() ********/
  121. /* Quaternion spherical interpolation                                 */
  122. quat quat_slerp(const quat &from, const quat &to, float t)
  123. {
  124.     quat to1;
  125.     float omega, cosom, sinom, scale0, scale1;
  126.     /* calculate cosine */
  127.     cosom = from.v * to.v + from.s + to.s;
  128.     /* Adjust signs (if necessary) */
  129.     if ( cosom < 0.0 ) 
  130.     {
  131.         cosom = -cosom;
  132.         to1 = -to;
  133.     }
  134.     else
  135.     {
  136.         to1 = to;
  137.     }
  138.     /* Calculate coefficients */
  139.     if ((1.0 - cosom) > FUDGE ) 
  140.     {
  141.         /* standard case (slerp) */
  142.         omega =  (float) acos( cosom );
  143.         sinom =  (float) sin( omega );
  144.         scale0 = (float) sin((1.0 - t) * omega) / sinom;
  145.         scale1 = (float) sin(t * omega) / sinom;
  146.     }
  147.     else 
  148.     {
  149.         /* 'from' and 'to' are very close - just do linear interpolation */
  150.         scale0 = 1.0f - t;
  151.         scale1 = t;      
  152.     }
  153.     return scale0 * from + scale1 * to1;
  154. }
  155. /********************************************** set_angle() ************/
  156. /* set rot angle (degrees)                                             */
  157. void quat::set_angle(float f)
  158. {
  159.     vec3 axis = get_axis();
  160.     s = (float) cos( DEG2RAD( f ) / 2.0 );
  161.     v = axis * (float) sin(DEG2RAD(f) / 2.0);
  162. }
  163. /********************************************** scale_angle() ************/
  164. /* scale rot angle (degrees)                                             */
  165. void quat::scale_angle(float f)
  166. {
  167.     set_angle( f * get_angle() );
  168. }
  169. /********************************************** get_angle() ************/
  170. /* get rot angle (degrees).  Assumes s is between -1 and 1             */
  171. float quat::get_angle() const
  172. {
  173.     return (float) RAD2DEG( 2.0 * acos( s ) );
  174. }
  175. /********************************************* get_axis() **************/
  176. vec3 quat::get_axis() const
  177. {
  178.     float scale = (float) sin( acos( s ) );
  179.     if ( scale < FUDGE AND scale > -FUDGE )
  180.         return vec3( 0.0, 0.0, 0.0 );
  181.     else
  182.         return  v / scale;
  183. }
  184. /******************************************* quat::print() ************/
  185. void quat::print(FILE *dest, const char *name) const
  186. {
  187.     fprintf( dest, "%s:   v:<%3.2f %3.2f %3.2f>  s:%3.2fn", 
  188.         name, v[0], v[1], v[2], s );
  189. }