00001 #ifndef GAME_SYOKUNIN_COM_GLSL_MATH_GLSL_MATH_H
00002
00003 #define GAME_SYOKUNIN_COM_GLSL_MATH_GLSL_MATH_H
00004
00005 #include <math.h>
00006 #include <boost/assert.hpp>
00007 #include <algorithm>
00008 #include <math.h>
00009 #include <numeric>
00010 #include <functional>
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066 namespace gslib {
00067
00069
00077 namespace glsl_math {
00078
00080
00085 template < size_t Size >
00086 struct tuple {
00087 typedef size_t size_type;
00088 typedef float value_type;
00089 typedef value_type* iterator;
00090 typedef const value_type* const_iterator;
00091
00092 static size_type size() {
00093 return Size;
00094 }
00095
00096 iterator begin() {
00097 return reinterpret_cast< iterator >( this );
00098 }
00099
00100 iterator end() {
00101 return begin() + size();
00102 }
00103
00104 const_iterator begin() const {
00105 return reinterpret_cast< const_iterator >( this );
00106 }
00107
00108 const_iterator end() const {
00109 return begin() + size();
00110 }
00111
00112 void swap( tuple& other ) {
00113 std::swap_ranges( other.begin(), other.end(), begin() );
00114 }
00115
00116 tuple() {}
00117
00118 tuple( const tuple& other ) {
00119 std::copy( other.begin(), other.end(), begin() );
00120 }
00121
00122 tuple& operator = ( const tuple& other ) {
00123 if ( this != &other ) {
00124 std::copy( other.begin(), other.end(), begin() );
00125 }
00126 return *this;
00127 }
00128
00129 float& operator [] ( size_type nth ) {
00130 return begin()[ nth ];
00131 }
00132
00133 float operator [] ( size_type nth ) const {
00134 return begin()[ nth ];
00135 }
00136
00137 float& at( size_type nth ) {
00138 return begin()[ nth ];
00139 }
00140
00141 float at( size_type nth ) const {
00142 return begin()[ nth ];
00143 }
00144 };
00145
00147 template < size_t Size >
00148 float dot( const tuple< Size >& a, const tuple< Size >& b ) {
00149 return std::inner_product( a.begin(), a.end(), b.begin(), 0.0f );
00150 }
00151
00153 template < size_t Size >
00154 float length( const tuple< Size >& a ) {
00155 return sqrtf( dot( a, a ) );
00156 }
00157
00159
00163 template < typename GenType >
00164 GenType normalize( const GenType& a ) {
00165 GenType result( a );
00166 float invLen = 1.0f / length( a );
00167 return a * invLen;
00168 }
00169
00171 template < size_t Size >
00172 float distance( const tuple< Size >& a, const tuple< Size >& b ) {
00173 float result = 0.0f;
00174 for ( size_t i = 0; i < Size; ++i ) {
00175 float diff = a[ i ] - b[ i ];
00176 result += diff * diff;
00177 }
00178 return sqrtf( result );
00179 }
00180
00182
00185 template < typename SubClass, size_t Size >
00186 struct tuple_mixin : public tuple< Size > {
00187 SubClass& operator += ( const tuple< Size >& other ) {
00188 std::transform(
00189 begin(), end(), other.begin(), begin(), std::plus< value_type >() );
00190 return *static_cast< SubClass* >( this );
00191 }
00192 SubClass operator + ( const tuple< Size >& other ) const {
00193 SubClass result( *static_cast< const SubClass* >( this ) );
00194 result += other;
00195 return result;
00196 }
00197 SubClass& operator -= ( const tuple< Size >& other ) {
00198 std::transform(
00199 begin(), end(), other.begin(), begin(), std::minus< value_type >() );
00200 return *static_cast< SubClass* >( this );
00201 }
00202 SubClass operator - ( const tuple< Size >& other ) const {
00203 SubClass result( *static_cast< const SubClass* >( this ) );
00204 result -= other;
00205 return result;
00206 }
00207 SubClass& operator *= ( value_type v ) {
00208 std::transform(
00209 begin(), end(), begin(), std::bind2nd( std::multiplies< value_type >(), v ) );
00210 return *static_cast< SubClass* >( this );
00211 }
00212 SubClass operator * ( value_type v ) const {
00213 SubClass result( *static_cast< const SubClass* >( this ) );
00214 result *= v;
00215 return result;
00216 }
00217 friend SubClass operator * ( value_type v, const tuple_mixin& other ) {
00218 return other * v;
00219 }
00220 SubClass& operator /= ( value_type v ) {
00221 return operator *= ( 1.0f / v );
00222 }
00223 SubClass operator / ( value_type v ) const {
00224 return ( *this ) * ( 1.0f / v );
00225 }
00226 SubClass& operator + () const {
00227 return *static_cast< SubClass* >( this );
00228 }
00229 SubClass operator - () const {
00230 SubClass result;
00231 std::transform( begin(), end(), result.begin(), std::negate< value_type >() );
00232 return result;
00233 }
00234 bool operator < ( const tuple< Size >& other ) const {
00235 return std::lexicographical_compare( begin(), end(), other.begin(), other.end() );
00236 }
00237 bool operator == ( const tuple< Size >& other ) const {
00238 return std::equal( begin(), end(), other.begin() );
00239 }
00240 bool operator != ( const tuple< Size >& other ) const {
00241 return !operator == ( other );
00242 }
00243 };
00244
00246 template < typename SubClass, size_t Size >
00247 struct vector_mixin : public tuple_mixin< SubClass, Size > {
00248 SubClass& operator *= ( value_type v ) {
00249 return tuple_mixin< SubClass, Size >::operator *= ( v );
00250 }
00251 SubClass operator * ( value_type v ) const {
00252 return tuple_mixin< SubClass, Size >::operator * ( v );
00253 }
00254 SubClass& operator /= ( value_type v ) {
00255 return tuple_mixin< SubClass, Size >::operator /= ( v );
00256 }
00257 SubClass operator / ( value_type v ) const {
00258 return tuple_mixin< SubClass, Size >::operator / ( v );
00259 }
00260
00261 SubClass& operator *= ( const tuple< Size >& other ) {
00262 std::transform(
00263 begin(), end(), other.begin(), begin(), std::multiplies< value_type >() );
00264 return *static_cast< SubClass* >( this );
00265 }
00266 SubClass operator * ( const tuple< Size >& other ) {
00267 SubClass result( *this );
00268 result *= other;
00269 return result;
00270 }
00271 SubClass& operator /= ( const tuple< Size >& other ) {
00272 std::transform(
00273 begin(), end(), other.begin(), begin(), std::divides< value_type >() );
00274 return *static_cast< SubClass* >( this );
00275 }
00276 SubClass operator / ( const tuple< Size >& other ) {
00277 SubClass result( *this );
00278 result /= other;
00279 return result;
00280 }
00281 };
00282
00283
00284 struct vec2;
00285 struct vec3;
00286 struct vec4;
00287
00288 struct vec2 : public vector_mixin< vec2, 2 > {
00289 union {
00290 struct {
00291 float x, y;
00292 };
00293 struct {
00294 float r, g;
00295 };
00296 struct {
00297 float s, t;
00298 };
00299 float array[ 2 ];
00300 };
00301 vec2() {}
00302 vec2( float v ) : x( v ), y( v ) {}
00303 vec2( float inX, float inY ) : x( inX ), y( inY ) {}
00304 vec2( const vec3& xy );
00305 };
00306
00307 struct vec3 : public vector_mixin< vec3, 3 > {
00308 union {
00309 struct {
00310 float x, y, z;
00311 };
00312 struct {
00313 float r, g, b;
00314 };
00315 struct {
00316 float s, t, p;
00317 };
00318 float array[ 3 ];
00319 };
00320
00321 vec3() {}
00322 vec3( float v ) : x( v ), y( v ), z( v ) {}
00323 vec3( float inX, float inY, float inZ ) : x( inX ), y( inY ), z( inZ ) {}
00324 vec3( const vec4& xyz );
00326 vec3( const vec2& xy, float inZ ) :
00327 x( xy.x ), y( xy.y ), z( inZ ) {
00328 }
00330 vec3( float inX, const vec2& yz ) :
00331 x( inX ), y( yz.x ), z( yz.y ) {
00332 }
00333
00335 vec2& xy() {
00336 return reinterpret_cast< vec2& >( array[ 0 ] );
00337 }
00339 const vec2& xy() const {
00340 return reinterpret_cast< const vec2& >( array[ 0 ] );
00341 }
00343 vec2& yz() {
00344 return reinterpret_cast< vec2& >( array[ 1 ] );
00345 }
00347 const vec2& yz() const {
00348 return reinterpret_cast< const vec2& >( array[ 1 ] );
00349 }
00350 };
00351
00352 struct vec4 : public vector_mixin< vec4, 4 > {
00353 union {
00354 struct {
00355 float x, y, z, w;
00356 };
00357 struct {
00358 float r, g, b, a;
00359 };
00360 struct {
00361 float s, t, p, q;
00362 };
00363 float array[ 4 ];
00364 };
00365
00366 vec4() {}
00367 vec4( float v ) : x( v ), y( v ), z( v ), w( v ) {}
00368 vec4( float inX, float inY, float inZ, float inW ) : x( inX ), y( inY ), z( inZ ), w( inW ) {}
00369 vec4( const vec3& xyz, float inW ) :
00370 x( xyz.x ), y( xyz.y ), z( xyz.z ), w( inW ) {
00371 }
00372 vec4( float inX, const vec3& yzw ) :
00373 x( inX ), y( yzw.x ), z( yzw.y ), w( yzw.z ) {
00374 }
00375 vec4( const vec2& xy, const vec2& zw ) :
00376 x( xy.x ), y( xy.y ), z( zw.x ), w( zw.y ) {
00377 }
00378
00380 vec2& xy() {
00381 return reinterpret_cast< vec2& >( array[ 0 ] );
00382 }
00384 const vec2& xy() const {
00385 return reinterpret_cast< const vec2& >( array[ 0 ] );
00386 }
00388 vec2& yz() {
00389 return reinterpret_cast< vec2& >( array[ 1 ] );
00390 }
00392 const vec2& yz() const {
00393 return reinterpret_cast< const vec2& >( array[ 1 ] );
00394 }
00396 vec2& zw() {
00397 return reinterpret_cast< vec2& >( array[ 2 ] );
00398 }
00400 const vec2& zw() const {
00401 return reinterpret_cast< const vec2& >( array[ 2 ] );
00402 }
00404 vec3& xyz() {
00405 return reinterpret_cast< vec3& >( array[ 0 ] );
00406 }
00408 const vec3& xyz() const {
00409 return reinterpret_cast< const vec3& >( array[ 0 ] );
00410 }
00412 vec3& yzw() {
00413 return reinterpret_cast< vec3& >( array[ 1 ] );
00414 }
00416 const vec3& yzw() const {
00417 return reinterpret_cast< const vec3& >( array[ 1 ] );
00418 }
00419 };
00420
00421 inline vec2::vec2( const vec3& xy ) : x( xy.x ), y( xy.y ) {}
00422 inline vec3::vec3( const vec4& xyz ) : x( xyz.x ), y( xyz.y ), z( xyz.z ) {}
00423
00425
00428 inline vec3 cross( const vec3& x, const vec3& y ) {
00429 return vec3(
00430 x[ 1 ] * y[ 2 ] - y[ 1 ] * x[ 2 ],
00431 x[ 2 ] * y[ 0 ] - y[ 2 ] * x[ 0 ],
00432 x[ 0 ] * y[ 1 ] - y[ 0 ] * x[ 1 ] );
00433 }
00434
00436 template < typename SubClass, typename FriendVector, size_t RowSize, size_t ColumnSize >
00437 struct matrix_mixin : public tuple_mixin< SubClass, RowSize * ColumnSize > {
00438 typedef tuple_mixin< SubClass, RowSize * ColumnSize > TupleMixin;
00439 SubClass& operator *= ( value_type v ) {
00440 return TupleMixin::operator *= ( v );
00441 }
00442 SubClass operator * ( value_type v ) const {
00443 return TupleMixin::operator * ( v );
00444 }
00445 SubClass& operator /= ( value_type v ) {
00446 return TupleMixin::operator /= ( v );
00447 }
00448 SubClass operator / ( value_type v ) const {
00449 return TupleMixin::operator / ( v );
00450 }
00451
00453 FriendVector& operator [] ( size_type nthColumn ) {
00454 BOOST_ASSERT( nthColumn < ColumnSize );
00455 return *( reinterpret_cast< FriendVector* >( begin() ) + nthColumn );
00456 }
00458 const FriendVector& operator [] ( size_type nthColumn ) const {
00459 BOOST_ASSERT( nthColumn < ColumnSize );
00460 return *( reinterpret_cast< const FriendVector* >( begin() ) + nthColumn );
00461 }
00462 SubClass operator * ( const SubClass& other ) const {
00463 #define AT( i, j ) operator [] ( i )[ j ]
00464
00465 SubClass result( 0 );
00466 for ( size_type i = 0; i < ColumnSize; ++i ) {
00467 for ( size_type j = 0; j < RowSize; ++j ) {
00468 for ( size_type k = 0; k < ColumnSize; ++k ) {
00469 result[ i ][ j ] += AT( j, k ) * other[ k ][ i ];
00470 }
00471 }
00472 }
00473
00474 #undef AT
00475
00476 return result;
00477 }
00478 SubClass& operator *= ( const SubClass& other ) {
00479 *this = ( *this ) * other;
00480 return *this;
00481 }
00483 FriendVector operator * ( const FriendVector& v ) {
00484 FriendVector result( 0 );
00485 for ( size_type i = 0; i < FriendVector::size(); ++i ) {
00486 for ( size_type j = 0; j < FriendVector::size(); ++j ) {
00487 result[ i ] += v[ j ] * operator [] ( j )[ i ];
00488 }
00489 }
00490 return result;
00491 }
00493 friend FriendVector operator * ( const FriendVector& v, const SubClass& other ) {
00494 FriendVector result;
00495 for ( size_type i = 0; i < FriendVector::size(); ++i ) {
00496 result[ i ] = dot( v, other[ i ] );
00497 }
00498 return result;
00499 }
00500
00501 static size_type columnSize() {
00502 return ColumnSize;
00503 }
00504 static size_type rowSize() {
00505 return RowSize;
00506 }
00507 };
00508
00509 struct mat2 : public matrix_mixin< mat2, vec2, 2, 2 > {
00510 value_type array[ 2 * 2 ];
00511
00512 mat2() {}
00513 mat2( value_type v ) {
00514 std::fill( begin(), end(), v );
00515 }
00516 mat2( const vec2& column0, const vec2& column1 ) {
00517 operator [] ( 0 ) = column0;
00518 operator [] ( 1 ) = column1;
00519 }
00520
00522
00530 mat2(
00531 value_type _00, value_type _10,
00532 value_type _01, value_type _11 ) {
00533
00534 array[ 0 ] = _00;
00535 array[ 1 ] = _10;
00536 array[ 2 ] = _01;
00537 array[ 3 ] = _11;
00538 }
00539
00540 static mat2 identity() {
00541 return mat2( 1, 0, 0, 1 );
00542 }
00543 };
00544
00545 struct mat3 : public matrix_mixin< mat3, vec3, 3, 3 > {
00546 value_type array[ 3 * 3 ];
00547
00548 mat3() {}
00549 mat3( value_type v ) {
00550 std::fill( begin(), end(), v );
00551 }
00552 mat3( const vec3& column0, const vec3& column1, const vec3& column2 ) {
00553 operator [] ( 0 ) = column0;
00554 operator [] ( 1 ) = column1;
00555 operator [] ( 2 ) = column2;
00556 }
00557
00558 mat3(
00559 value_type _00, value_type _10, value_type _20,
00560 value_type _01, value_type _11, value_type _21,
00561 value_type _02, value_type _12, value_type _22 ) {
00562
00563 array[ 0 ] = _00;
00564 array[ 1 ] = _10;
00565 array[ 2 ] = _20;
00566 array[ 3 ] = _01;
00567 array[ 4 ] = _11;
00568 array[ 5 ] = _21;
00569 array[ 6 ] = _02;
00570 array[ 7 ] = _12;
00571 array[ 8 ] = _22;
00572 }
00573
00574 static mat3 identity() {
00575 return mat3( 1, 0, 0, 0, 1, 0, 0, 0, 1 );
00576 }
00577 };
00578
00579 struct mat4 : public matrix_mixin< mat4, vec4, 4, 4 > {
00580 value_type array[ 4 * 4 ];
00581
00582 mat4() {}
00583 mat4( value_type v ) {
00584 std::fill( begin(), end(), v );
00585 }
00586 mat4( const vec4& column0, const vec4& column1, const vec4& column2, const vec4& column3 ) {
00587 operator [] ( 0 ) = column0;
00588 operator [] ( 1 ) = column1;
00589 operator [] ( 2 ) = column2;
00590 operator [] ( 3 ) = column3;
00591 }
00592
00593 mat4(
00594 value_type _00, value_type _10, value_type _20, value_type _30,
00595 value_type _01, value_type _11, value_type _21, value_type _31,
00596 value_type _02, value_type _12, value_type _22, value_type _32,
00597 value_type _03, value_type _13, value_type _23, value_type _33 ) {
00598
00599 array[ 0 ] = _00;
00600 array[ 1 ] = _10;
00601 array[ 2 ] = _20;
00602 array[ 3 ] = _30;
00603 array[ 4 ] = _01;
00604 array[ 5 ] = _11;
00605 array[ 6 ] = _21;
00606 array[ 7 ] = _31;
00607 array[ 8 ] = _02;
00608 array[ 9 ] = _12;
00609 array[ 10 ] = _22;
00610 array[ 11 ] = _32;
00611 array[ 12 ] = _03;
00612 array[ 13 ] = _13;
00613 array[ 14 ] = _23;
00614 array[ 15 ] = _33;
00615 }
00616
00617 static mat4 identity() {
00618 return mat4( 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 );
00619 }
00620 };
00621
00622 float det( const mat2& m ) {
00623 return m[ 0 ][ 0 ] * m[ 1 ][ 1 ] - m[ 0 ][ 1 ] * m[ 1 ][ 0 ];
00624 }
00625 float cofactor( const mat2& m, mat2::size_type col, mat2::size_type row ) {
00626 return m[ col ? 0 : 1 ][ row ? 0 : 1 ];
00627 }
00628
00629 template < typename Mat, typename SmallMat >
00630 struct CofactorCalc {
00631 static float calc( const Mat& m, typename Mat::size_type col, typename Mat::size_type row ) {
00632 SmallMat result;
00633 float* f = result.array;
00634 for ( Mat::size_type i = 0; i < Mat::columnSize(); ++i ) {
00635 if ( i != col ) {
00636 for ( Mat::size_type j = 0; j < Mat::rowSize(); ++j ) {
00637 if ( j != row ) {
00638 *f = m[ i ][ j ];
00639 ++f;
00640 }
00641 }
00642 }
00643 }
00644 return det( result );
00645 }
00646 };
00647 template < typename Mat >
00648 struct DetCalc {
00649 static float calc( const Mat& m ) {
00650 float result = 0;
00651 for ( Mat::size_type i = 0; i < Mat::columnSize(); ++i ) {
00652 result += ( ( 0 == ( i & 1 ) ) ? 1 : -1 ) * m[ 0 ][ i ] * cofactor( m, 0, i );
00653 }
00654 return result;
00655 }
00656 };
00657 template < typename Mat >
00658 struct InverseCalc {
00659 static Mat calc( const Mat& m ) {
00660 Mat result;
00661 float invDetM = 1.0f / det( m );
00662 for ( Mat::size_type i = 0; i < Mat::columnSize(); ++i ) {
00663 for ( Mat::size_type j = 0; j < Mat::rowSize(); ++j ) {
00664 result[ i ][ j ] = ( ( 0 == ( ( i + j ) & 1 ) ) ? invDetM : -invDetM ) * cofactor( m, j, i );
00665 }
00666 }
00667 return result;
00668 }
00669 };
00670
00671 mat2 inverse( const mat2& m ) {
00672 return InverseCalc< mat2 >::calc( m );
00673 }
00674
00675 float cofactor( const mat3& m, mat3::size_type col, mat3::size_type row ) {
00676 return CofactorCalc< mat3, mat2 >::calc( m, col, row );
00677 }
00678 float det( const mat3& m ) {
00679 return DetCalc< mat3 >::calc( m );
00680 }
00681 mat3 inverse( const mat3& m ) {
00682 return InverseCalc< mat3 >::calc( m );
00683 }
00684
00685 float cofactor( const mat4& m, mat3::size_type col, mat3::size_type row ) {
00686 return CofactorCalc< mat4, mat3 >::calc( m, col, row );
00687 }
00688 float det( const mat4& m ) {
00689 return DetCalc< mat4 >::calc( m );
00690 }
00691 mat4 inverse( const mat4& m ) {
00692 return InverseCalc< mat4 >::calc( m );
00693 }
00694
00695 template < typename Mat >
00696 Mat transpose( const Mat& m ) {
00697 Mat result;
00698 for ( size_type i = 0; i < m.colSize(); ++i ) {
00699 for ( size_type j = 0; j < m.rowSize(); ++j ) {
00700 result[ j ][ i ] = m[ i ][ j ];
00701 }
00702 }
00703 return result;
00704 }
00705
00706
00707
00708
00709
00710
00711
00712
00713
00714
00715
00716
00717
00718
00719
00720
00721
00722
00723
00724
00725
00726
00727 struct quat : public tuple_mixin< quat, 4 > {
00728 union {
00729 struct {
00730 float x, y, z, w;
00731 };
00732 float array[ 4 ];
00733 };
00734 quat() {}
00735 quat( value_type v ) : x( v ), y( v ), z( v ), w( v ) {}
00736 quat( value_type inX, value_type inY, value_type inZ, value_type inW ) : x( inX ), y( inY ), z( inZ ), w( inW ) {}
00737 quat( const vec3& axis, value_type theta ) {
00738 value_type halfCos = cosf( theta * 0.5f );
00739 value_type halfSin = sinf( theta * 0.5f );
00740 xyz() = halfCos * axis;
00741 w = halfSin;
00742 }
00743
00745 vec4& xyzw() {
00746 return *reinterpret_cast< vec4* >( array );
00747 }
00748
00750 const vec4& xyzw() const {
00751 return *reinterpret_cast< const vec4* >( array );
00752 }
00753
00755 vec3& xyz() {
00756 return *reinterpret_cast< vec3* >( array );
00757 }
00758
00760 const vec3& xyz() const {
00761 return *reinterpret_cast< const vec3* >( array );
00762 }
00763
00765 vec2& xy() {
00766 return *reinterpret_cast< vec2* >( array );
00767 }
00768
00770 const vec2& xy() const {
00771 return *reinterpret_cast< const vec2* >( array );
00772 }
00773
00774 typedef tuple_mixin< quat, 4 > TupleMixin;
00775 quat& operator *= ( value_type v ) {
00776 return TupleMixin::operator *= ( v );
00777 }
00778 quat operator * ( value_type v ) const {
00779 return TupleMixin::operator * ( v );
00780 }
00781 quat& operator /= ( value_type v ) {
00782 return TupleMixin::operator /= ( v );
00783 }
00784 quat operator / ( value_type v ) const {
00785 return TupleMixin::operator / ( v );
00786 }
00787 quat operator * ( const quat& q ) const {
00788 return quat(
00789 w * q.x + x * q.w + y * q.z - z * q.y,
00790 w * q.y + y * q.w + z * q.x - x * q.z,
00791 w * q.z + z * q.w + x * q.y - y * q.x,
00792 w * q.w - x * q.x - y * q.y - z * q.z
00793 );
00794 }
00795 quat& operator *= ( const quat& q ) {
00796 *this = *this * q;
00797 return *this;
00798 }
00799
00800 static quat identity() {
00801 return quat( 0, 0, 0, 1 );
00802 }
00803 };
00804
00805 inline quat conj( const quat& q ) {
00806 return quat(
00807 -q.x,
00808 -q.y,
00809 -q.z,
00810 q.w );
00811 }
00812
00813 inline quat inverse( const quat& q ) {
00814 return conj( q ) / dot( q, q );
00815 }
00816
00818 inline float radians( float deg ) {
00819 const float pi = 3.14159265358979323846264338327950288419716939937510582;
00820 return deg * ( pi / 180 );
00821 }
00822
00824 inline float degrees( float rad ) {
00825 const float pi = 3.14159265358979323846264338327950288419716939937510582;
00826 return rad * ( 180 / pi );
00827 }
00828
00830
00836 template < typename GenType >
00837 inline GenType lerp( const GenType& a, const GenType& b, float blendRate ) {
00838 BOOST_ASSERT( 0 <= blendRate && blendRate <= 1 );
00839 return ( 1 - blendRate ) * a + blendRate * b;
00840 }
00841
00843
00850 inline quat log( const quat &q ) {
00851 float a = static_cast<float>(acosf(q.w));
00852 float sina = static_cast<float>(sinf(a));
00853 quat ret;
00854 ret.w = 0;
00855 if (sina > 0) {
00856
00857
00858
00859 ret.xyz() = q.xyz() * ( a / sina );
00860 } else {
00861 ret.x=ret.y=ret.z=0;
00862 }
00863 return ret;
00864 }
00865
00867
00874 inline quat exp( const quat& q ) {
00875 float a = static_cast< float >( sqrtf( q.x*q.x + q.y*q.y + q.z*q.z ) );
00876 float sina = static_cast< float >( sinf( a ) );
00877 float cosa = static_cast< float >( cosf( a ) );
00878 quat ret;
00879 ret.w = cosa;
00880 if ( a > 0 ) {
00881 ret.xyz() = q.xyz() * ( sina / a );
00882
00883
00884
00885 } else {
00886 ret.x = ret.y = ret.z = 0;
00887 }
00888
00889 return ret;
00890 }
00891
00893
00897 inline quat slerp( const quat& q1,const quat& q2,float t ) {
00898 quat q3;
00899 float d = dot( q1, q2 );
00900
00901
00902
00903
00904
00905
00906 if ( d < 0 ) {
00907 d = -d;
00908 q3 = -q2;
00909 }
00910 else
00911 {
00912 q3 = q2;
00913 }
00914
00915
00916 if ( d < 0.95f ) {
00917 float angle = static_cast<float>(acosf(d));
00918 float sina,sinat,sinaomt;
00919 sina = static_cast<float>(sinf(angle));
00920 sinat = static_cast<float>(sinf(angle*t));
00921 sinaomt = static_cast<float>(sinf(angle*(1-t)));
00922 return (q1*sinaomt+q3*sinat)/sina;
00923 } else {
00924
00925 return lerp(q1,q3,t);
00926 }
00927 }
00928
00930
00934 inline quat slerpNoInvert(const quat &q1,const quat &q2,float t) {
00935 float d = dot( q1, q2 );
00936
00937 if (d > -0.95f && d < 0.95f)
00938 {
00939 float angle = static_cast<float>(acosf(d));
00940 float sina,sinat,sinaomt;
00941 sina = static_cast<float>(sinf(angle));
00942 sinat = static_cast<float>(sinf(angle*t));
00943 sinaomt = static_cast<float>(sinf(angle*(1-t)));
00944 return (q1*sinaomt+q2*sinat)/sina;
00945 }
00946
00947
00948
00949 else
00950 {
00951 return lerp(q1,q2,t);
00952 }
00953 }
00954
00955
00957
00961 inline quat squad(const quat &q1,const quat &q2,const quat &a,const quat &b,float t) {
00962 quat c,d;
00963 c = slerpNoInvert(q1,q2,t);
00964 d = slerpNoInvert(a,b,t);
00965 return slerpNoInvert(c,d,2*t*(1-t));
00966 }
00967
00968
00970
00974 inline quat spline(const quat &qnm1,const quat &qn,const quat &qnp1) {
00975 quat qni = conj( qn );
00976 return qn * exp((log(qni*qnm1)+log(qni*qnp1))/-4);
00977 }
00978 }
00979 }
00980
00981 #endif