- メインページ
- モジュール
- ネームスペース
- データ構造
- ファイル

- ファイル一覧
- グローバル

quaternion.h
説明を見る。
1 //
2 // Copyright (c) 2003-2011, MIST Project, Nagoya University
3 // All rights reserved.
4 //
5 // Redistribution and use in source and binary forms, with or without modification,
6 // are permitted provided that the following conditions are met:
7 //
8 // 1. Redistributions of source code must retain the above copyright notice,
9 // this list of conditions and the following disclaimer.
10 //
11 // 2. Redistributions in binary form must reproduce the above copyright notice,
12 // this list of conditions and the following disclaimer in the documentation
13 // and/or other materials provided with the distribution.
14 //
15 // 3. Neither the name of the Nagoya University nor the names of its contributors
16 // may be used to endorse or promote products derived from this software
17 // without specific prior written permission.
18 //
19 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
20 // IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
21 // FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
22 // CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
23 // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
24 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
25 // IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
26 // THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 //
28
33 #ifndef __INCLUDE_MIST_QUATERNION__
34 #define __INCLUDE_MIST_QUATERNION__
35
36
37 #ifndef __INCLUDE_MIST_CONF_H__
38 #include "config/mist_conf.h"
39 #endif
40
41 #ifndef __INCLUDE_MIST_TYPE_TRAIT_H__
42 #include "config/type_trait.h"
43 #endif
44
45 #ifndef __INCLUDE_MIST_VECTOR__
46 #include "vector.h"
47 #endif
48
49
50 #include <cmath>
51
52 // mist名前空間の始まり
53 _MIST_BEGIN
54
55
60 template < class T >
61 class quaternion
62 {
63 public:
64 typedef T value_type;
65 typedef size_t size_type;
66 typedef ptrdiff_t difference_type;
67 typedef typename float_type< T >::value_type float_type;
68
69 public:
70 value_type w;
71 value_type x;
72 value_type y;
73 value_type z;
74
76 quaternion( ) : w( 0 ), x( 0 ), y( 0 ), z( 0 ){ }
77
79 quaternion( const value_type &ww, const value_type &xx, const value_type &yy, const value_type &zz ) : w( ww ), x( xx ), y( yy ), z( zz ){ }
80
82 explicit quaternion( const value_type &ww ) : w( ww ), x( 0 ), y( 0 ), z( 0 ){ }
83
85 template < class TT >
86 quaternion( const quaternion< TT > &q ) : w( static_cast< value_type >( q.w ) ), x( static_cast< value_type >( q.x ) ), y( static_cast< value_type >( q.y ) ), z( static_cast< value_type >( q.z ) ){ }
87
89 quaternion( const quaternion< T > &q ) : w( q.w ), x( q.x ), y( q.y ), z( q.z ){ }
90
91
93 template < class TT >
94 quaternion( value_type ww, const vector3< TT > &v ) : w( ww ), x( static_cast< value_type >( v.x ) ), y( static_cast< value_type >( v.y ) ), z( static_cast< value_type >( v.z ) ){ }
95
96
105 template < class TT >
106 quaternion( const vector3< TT > &axis, value_type theta )
107 {
108 double t = theta * 3.1415926535897932384626433832795 / 180.0 / 2.0;
109 double c = std::cos( t );
110 double s = std::sin( t );
111 w = static_cast< value_type >( c );
112 x = static_cast< value_type >( s * axis.x );
113 y = static_cast< value_type >( s * axis.y );
114 z = static_cast< value_type >( s * axis.z );
115 }
116
117
123 template < class TT >
124 quaternion( vector3< TT > dir, vector3< TT > up )
125 {
126 // ワールド座標の単位ベクトル
127 vector3< TT > e2( 0, 1, 0 );
128 vector3< TT > e3( 0, 0, 1 );
129
130 // 単位ベクトルにする
131 dir = dir.unit( );
132 up = up.unit( );
133
134 // 視線方向を合わせるクォータニオンを作成
135 quaternion q1 = quaternion::rotate( e3, dir );
136
137 // Y軸を回転させる
138 e2 = q1.rotate( e2 );
139
140 // 上向き方向を合わせるクォータニオンを作成
141 quaternion q2 = quaternion::rotate( e2, up, dir );
142
143 // 回転を合成する
144 operator =( q2 * q1 );
145 }
146
147
149 template < class TT >
150 const quaternion &operator =( const quaternion< TT > &q )
151 {
152 w = static_cast< value_type >( q.w );
153 x = static_cast< value_type >( q.x );
154 y = static_cast< value_type >( q.y );
155 z = static_cast< value_type >( q.z );
156 return ( *this );
157 }
158
160 const quaternion &operator =( const quaternion< T > &q )
161 {
162 if( &q != this )
163 {
164 w = q.w;
165 x = q.x;
166 y = q.y;
167 z = q.z;
168 }
169 return ( *this );
170 }
171
173 quaternion operator -( ) const { return ( quaternion( -w, -x, -y, -z ) ); }
174
183 template< class TT >
184 const quaternion &operator +=( const quaternion< TT > &q )
185 {
186 w = static_cast< value_type >( w + q.w );
187 x = static_cast< value_type >( x + q.x );
188 y = static_cast< value_type >( y + q.y );
189 z = static_cast< value_type >( z + q.z );
190 return( *this );
191 }
192
201 #if defined( __MIST_MSVC__ ) && __MIST_MSVC__ < 7
202 const quaternion &operator +=( const double &a )
203 #else
204 template < class TT >
205 const quaternion &operator +=( const TT &a )
206 #endif
207 {
208 w = static_cast< value_type >( w + a );
209 return( *this );
210 }
211
220 template< class TT >
221 const quaternion &operator -=( const quaternion< TT > &q )
222 {
223 w = static_cast< value_type >( w - q.w );
224 x = static_cast< value_type >( x - q.x );
225 y = static_cast< value_type >( y - q.y );
226 z = static_cast< value_type >( z - q.z );
227 return( *this );
228 }
229
238 #if defined( __MIST_MSVC__ ) && __MIST_MSVC__ < 7
239 const quaternion &operator -=( const double &a )
240 #else
241 template < class TT >
242 const quaternion &operator -=( const TT &a )
243 #endif
244 {
245 w = static_cast< value_type >( w - a );
246 return( *this );
247 }
248
265 template < class TT >
266 const quaternion &operator *=( const quaternion< TT > &q )
267 {
268 value_type ww = static_cast< value_type >( w * q.w - x * q.x - y * q.y - z * q.z );
269 value_type xx = static_cast< value_type >( w * q.x + x * q.w + y * q.z - z * q.y );
270 value_type yy = static_cast< value_type >( w * q.y + y * q.w + z * q.x - x * q.z );
271 value_type zz = static_cast< value_type >( w * q.z + z * q.w + x * q.y - y * q.x );
272 w = ww;
273 x = xx;
274 y = yy;
275 z = zz;
276 return( *this );
277 }
278
287 #if defined( __MIST_MSVC__ ) && __MIST_MSVC__ < 7
288 const quaternion &operator *=( const double &a )
289 #else
290 template < class TT >
291 const quaternion &operator *=( const TT &a )
292 #endif
293 {
294 w = static_cast< value_type >( w * a );
295 x = static_cast< value_type >( x * a );
296 y = static_cast< value_type >( y * a );
297 z = static_cast< value_type >( z * a );
298 return( *this );
299 }
300
309 template < class TT >
310 const quaternion &operator /=( const quaternion< TT > &q )
311 {
312 return( this->operator *=( q.inv( ) ) );
313 }
314
323 #if defined( __MIST_MSVC__ ) && __MIST_MSVC__ < 7
324 const quaternion &operator /=( const double &a )
325 #else
326 template < class TT >
327 const quaternion &operator /=( const TT &a )
328 #endif
329 {
330 w = static_cast< value_type >( w / a );
331 x = static_cast< value_type >( x / a );
332 y = static_cast< value_type >( y / a );
333 z = static_cast< value_type >( z / a );
334 return( *this );
335 }
336
337
347 bool operator ==( const quaternion &q ) const { return( w == q.w && x == q.x && y == q.y && z == q.z ); }
348
358 bool operator !=( const quaternion &q ) const { return( !( *this == q ) ); }
359
369 bool operator < ( const quaternion &q ) const
370 {
371 if( w == q.w )
372 {
373 if( x == q.x )
374 {
375 if( y == q.y )
376 {
377 return( z < q.z );
378 }
379 else
380 {
381 return( y < q.y );
382 }
383 }
384 else
385 {
386 return( x < q.x );
387 }
388 }
389 else
390 {
391 return( w < q.w );
392 }
393 }
394
404 bool operator <=( const quaternion &q ) const { return( q >= *this ); }
405
415 bool operator > ( const quaternion &q ) const { return( q < *this ); }
416
426 bool operator >=( const quaternion &q ) const { return( !( *this < q ) ); }
427
428
429 public: // その他の関数
430
435 const quaternion conjugate( ) const
436 {
437 return( quaternion( w, -x, -y, -z ) );
438 }
439
444 const quaternion inv( ) const
445 {
446 float_type length_ = length( );
447 return( conjugate( ) / ( length_ * length_ ) );
448 }
449
454 const quaternion unit( ) const
455 {
456 float_type length_ = length( );
457 if( length_ > 0 )
458 {
459 return( quaternion( static_cast< value_type >( w / length_ ), static_cast< value_type >( x / length_ ), static_cast< value_type >( y / length_ ), static_cast< value_type >( z / length_ ) ) );
460 }
461 else
462 {
463 return( *this );
464 }
465 }
466
473 template < class TT >
474 value_type inner( const quaternion< TT > &q ) const
475 {
476 return( w * q.w + x * q.x + y * q.y + z * q.z );
477 }
478
483 float_type length( ) const { return ( static_cast< float_type >( std::sqrt( static_cast< double >( w * w + x * x + y * y + z * z ) ) ) ); }
484
485
492 template < class TT >
493 const vector3< TT > rotate( const vector3< TT > &v ) const
494 {
495 quaternion q = ( *this ) * quaternion( 0, static_cast< value_type >( v.x ), static_cast< value_type >( v.y ), static_cast< value_type >( v.z ) ) * inv( );
496 return( vector3< TT >( static_cast< TT >( q.x ), static_cast< TT >( q.y ), static_cast< TT >( q.z ) ) );
497 }
498
499
500
508 template < class TT >
509 static quaternion rotate( vector3< TT > v1, vector3< TT > v2 )
510 {
511 // 単位ベクトルにする
512 v1 = v1.unit( );
513 v2 = v2.unit( );
514
515 // 回転角度を計算する
516 double dot = v1.inner( v2 );
517 if( dot < -1.0 )
518 {
519 return( quaternion( -1, 0, 0, 0 ) );
520 }
521
522 double c = std::sqrt( ( dot + 1.0 ) * 0.5 );
523
524 if( std::abs( c - 1.0 ) < 1.0e-6 || c > 1.0 )
525 {
526 return( quaternion( 1, 0, 0, 0 ) );
527 }
528 else if( std::abs( c + 1.0 ) < 1.0e-6 || c < -1.0 )
529 {
530 return( quaternion( -1, 0, 0, 0 ) );
531 }
532
533 return( quaternion( value_type( c ), std::sqrt( 1.0 - c * c ) * v1.outer( v2 ).unit( ) ) );
534 }
535
544 template < class TT >
545 static quaternion rotate( vector3< TT > v1, vector3< TT > v2, const vector3< TT > &axis )
546 {
547 // 単位ベクトルにする
548 v1 = v1.unit( );
549 v2 = v2.unit( );
550
551 // 回転角度を計算する
552 double dot = v1.inner( v2 );
553 if( dot < -1.0 )
554 {
555 return( quaternion( -1, 0, 0, 0 ) );
556 }
557
558 double c = std::sqrt( ( dot + 1.0 ) * 0.5 );
559
560 if( std::abs( c - 1.0 ) < 1.0e-6 || c > 1.0 )
561 {
562 return( quaternion( 1, 0, 0, 0 ) );
563 }
564 else if( std::abs( c + 1.0 ) < 1.0e-6 || c < -1.0 )
565 {
566 return( quaternion( -1, 0, 0, 0 ) );
567 }
568
569 double s = std::sqrt( 1.0 - c * c );
570
571 if( axis.inner( v1.outer( v2 ) ) < 0.0 )
572 {
573 s = -s;
574 }
575
576 return( quaternion( value_type( c ), s * axis ) );
577 }
578 };
579
580 // 型の昇格を行う演算の定義
581
583 DEFINE_PROMOTE_BIND_OPERATOR1( quaternion, + )
584
585
586 DEFINE_PROMOTE_BIND_OPERATOR2( quaternion, + )
587
589 DEFINE_PROMOTE_BIND_OPERATOR3( quaternion, + )
590
592 DEFINE_PROMOTE_BIND_OPERATOR1( quaternion, - )
593
595 DEFINE_PROMOTE_BIND_OPERATOR2( quaternion, - )
596
598 DEFINE_PROMOTE_BIND_OPERATOR4( quaternion, - )
599
601 DEFINE_PROMOTE_BIND_OPERATOR1( quaternion, * )
602
604 DEFINE_PROMOTE_BIND_OPERATOR2( quaternion, * )
605
607 DEFINE_PROMOTE_BIND_OPERATOR3( quaternion, * )
608
610 DEFINE_PROMOTE_BIND_OPERATOR1( quaternion, / )
611
613 DEFINE_PROMOTE_BIND_OPERATOR2( quaternion, / )
614
615
616
617
618
619
631 template < class T > inline std::ostream &operator <<( std::ostream &out, const quaternion< T > &q )
632 {
633 out << "( ";
634 out << q.w << ", ";
635 out << q.x << ", ";
636 out << q.y << ", ";
637 out << q.z << " )";
638 return( out );
639 }
640
641
642
651 template < class T1, class T2 >
652 const quaternion< double > interpolate( const quaternion< T1 > &q1, const quaternion< T2 > &q2, double t )
653 {
654 typedef quaternion< double > quaternion_type;
655
656 quaternion_type Q1( q1.unit( ) );
657 quaternion_type Q2( q2.unit( ) );
658
659 double dot = Q1.inner( Q2 );
660
661 if( std::abs( dot ) < 1.0e-6 )
662 {
663 return( Q1 );
664 }
665 else if( dot < 0.0 )
666 {
667 double theta = std::acos( dot );
668
669 // 球面線形補間を行う
670 return( quaternion_type( Q1 * std::sin( theta * ( 1.0 - t ) ) - Q2 * std::sin( theta * t ) ).unit( ) );
671 }
672 else
673 {
674 double theta = std::acos( dot );
675
676 // 球面線形補間を行う
677 return( quaternion_type( Q1 * std::sin( theta * ( 1.0 - t ) ) + Q2 * std::sin( theta * t ) ).unit( ) );
678 }
679 }
680
681
682 // クォータニオンから行列へ変換する
683 //template < class T >
684 //matrix< T > rotate_matrix( const quaternion< T > &q )
685 //{
686 // matrix< T > mat( 4, 4 );
687 // mat( 0, 0 ) = q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z;
688 // mat( 0, 1 ) = 2 * ( q.x * q.y - q.w * q.z );
689 // mat( 0, 2 ) = 2 * ( q.x * q.z + q.w * q.y );
690 // mat( 0, 3 ) = 0;
691 // mat( 1, 0 ) = 2 * ( q.x * q.y + q.w * q.z );
692 // mat( 1, 1 ) = q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z;
693 // mat( 1, 2 ) = 2 * ( q.y * q.z - q.w * q.x );
694 // mat( 1, 3 ) = 0;
695 // mat( 2, 0 ) = 2 * ( q.x * q.z - q.w * q.y );
696 // mat( 2, 1 ) = 2 * ( q.y * q.z + q.w * q.x );
697 // mat( 2, 2 ) = q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z;
698 // mat( 2, 3 ) = 0;
699 // mat( 3, 0 ) = 0;
700 // mat( 3, 1 ) = 0;
701 // mat( 3, 2 ) = 0;
702 // mat( 3, 3 ) = 1;
703 // return( mat );
704 //}
705
706 //
707 //
708 //
709 //
710
711
739 template < class T >
740 const quaternion< T > track_ball( const vector2< T > &p1, const vector2< T > &p2, const vector3< T > &axisX, const vector3< T > axisY, const vector3< T > axisZ, const typename vector3< T >::value_type &trackball_size )
741 {
742 typedef typename quaternion< T >::value_type value_type;
743
744 if( p1 == p2 )
745 {
746 return( quaternion< T >( 1, 0, 0, 0 ) );
747 }
748
749 vector3< T > sp1( p1.x, p1.y, 0 ), sp2( p2.x, p2.y, 0 );
750 value_type l, _2 = std::sqrt( value_type( 2.0 ) );
751
752 // 点1の座標を仮想トラックボール上に投影
753 l = p1.length( );
754 if( l < trackball_size / _2 )
755 {
756 sp1.z = - std::sqrt( trackball_size * trackball_size - l * l );
757 }
758 else
759 {
760 sp1.z = - trackball_size * trackball_size / 2.0 / l;
761 }
762
763 // 点2の座標を仮想トラックボール上に投影
764 l = p2.length( );
765 if( l < trackball_size / _2 )
766 {
767 sp2.z = - std::sqrt( trackball_size * trackball_size - l * l );
768 }
769 else
770 {
771 sp2.z = - trackball_size * trackball_size / 2.0 / l;
772 }
773
774 // sp1 = sp1.unit();
775 // sp2 = sp2.unit();
776
777 // 右手系と左手系でここの外積の向きを反転させる
778 // Vector3<double> axis = (sp2 * sp1).unit();
779 vector3< T > axis = ( sp1 * sp2 ).unit( );
780 axis = ( axis.x * axisX + axis.y * axisY + axis.z * axisZ ).unit( );
781
782 l = ( sp2 - sp1 ).length( ) / ( 2 * trackball_size );
783 // l = (l < -1.0)? -1.0: l;
784 l = l > 1 ? 1: l;
785
786 double phi = std::asin( l );
787 // fprintf(stdout, "axis(%.1f, %.1f, %.1f) theta = %.1f\n", axis.x, axis.y, axis.z, phi * 180 / PAI);
788 // printf("%.1f\n", phi * 180 / PAI);
789 return( quaternion< T >( std::cos( phi ), std::sin( phi ) * axis ) );
790 }
791
818 template < class T >
819 inline const quaternion< T > track_ball( const vector2< T > &p1, const vector2< T > &p2, const vector3< T > &axisX, const vector3< T > axisY, const vector3< T > axisZ )
820 {
821 return( track_ball( p1, p2, axisX, axisY, axisZ, 0.8 ) );
822 }
823
824
840 template < class T >
841 const quaternion< T > track_ball( const typename vector3< T >::value_type &x1, const typename vector3< T >::value_type &y1, const typename vector3< T >::value_type &x2, const typename vector3< T >::value_type &y2,
842 const vector3< T > &axisX, const vector3< T > &axisY, const vector3< T > &axisZ, const typename vector3< T >::value_type &trackball_size )
843 {
844 return( track_ball( vector2< T >( x1, y1 ), vector2< T >( x2, y2 ), axisX, axisY, axisZ, trackball_size ) );
845 }
846
847
862 template < class T >
863 const quaternion< T > track_ball( const typename vector3< T >::value_type &x1, const typename vector3< T >::value_type &y1, const typename vector3< T >::value_type &x2,
864 const typename vector3< T >::value_type &y2, const vector3< T > &axisX, const vector3< T > &axisY, const vector3< T > &axisZ )
865 {
866 return( track_ball( vector2< T >( x1, y1 ), vector2< T >( x2, y2 ), axisX, axisY, axisZ, 0.8 ) );
867 }
868
869
870 // mist名前空間の終わり
871 _MIST_END
872
873 #endif // __INCLUDE_MIST_QUATERNION__
874