Morph-SynRJ

view mth_quaternion.h @ 0:22de913c2d84

find changesets by author, revision, files, or words in the commit message
morph-synrj intro
author "Cedric Pinson <cedric.pinson@alcove.fr> <mornifle@plopbyte.net>"
date Tue Nov 27 15:23:52 2007 +0100 (2007-11-27)
parents
children
line source
1 /** @file mth_quaternion.h
2 *
3 * @brief quaternion class
4 *
5 *****************************************************************************
6 *
7 * @author psc80
8 *
9 * @date Created 2001/05
10 *
11 * @version $Id: mth_quaternion.h,v 1.1 2004/01/18 07:16:45 psc80 Exp $
12 *
13 ****************************************************************************/
14
15 #ifndef MTH_QUATERNION_H
16 #define MTH_QUATERNION_H
17
18 #include <cmath>
19 #include "mth.h"
20 #include "mth_vector3.h"
21
22 namespace mth {
23
24
25 /**
26 * Class Quaternion, essentially used for rotation in some cases @see phy
27 */
28 class Quaternion_t {
29
30 float n;
31 Vector3_t v;
32
33 public:
34
35 /// do nothing
36 Quaternion_t() {}
37
38
39 /// constructor with four scalar
40 Quaternion_t(const float& _n,const float& _v0, const float& _v1, const float& _v2) {n=_n;v.Init(_v0,_v1,_v2);}
41
42
43 /// constructor with n and a vector3
44 Quaternion_t(const float& _n,const Vector3_t& _v) {n=_n;v=_v;}
45
46
47 /// Constructor with three scalar to build a rotation quaternion
48 Quaternion_t(const float& _y,const float& _p,const float& _r) { MakeFromEuler(_y,_p,_r);}
49
50
51 /// Set the quaternion to 0
52 void Init() { n=0;v.Init();}
53
54
55 /// Return the scalar part of the quaternion
56 const float& Scalar() { return n;}
57
58
59 /// Return the vector part of the quaternion
60 const Vector3_t& Vector() { return v;}
61
62
63 /// return the magnitude squared
64 float MagSqr() { return v.MagSqr()+n*n;}
65
66
67 /// return the magnitude
68 float Mag() { return Sqrt(MagSqr());}
69
70
71 /// Return a conjugate quaternion
72 Quaternion_t Conjugate() const { return Quaternion_t(n,-v);}
73
74
75 /// Normalize
76 void Normalize() { float a=1.0/Mag();n*=a;v*=a;}
77
78
79
80 /// Rotate a vector
81 Vector3_t Rotate(const Vector3_t& _v) const {
82 return ((*this)*_v*Conjugate()).Vector();}
83
84
85 /// Rotate a vector
86 //Quaternion_t Rotate(const Quaternion_t& _q) const {
87 // return ((*this)*_q*Conjugate()).Vector();}
88
89
90 /// Return the angle of rotation about the axis represented by the vector part
91 float Angle() { return ACos(n);}
92
93
94 /// Return a unit vector along the axis of rotation represented by the vector part
95 Vector3_t Axis() {
96 float m=v.Mag();
97 if (m<=(float)0.0)
98 return v;
99 return v*(1.0/m);}
100
101
102
103 void MakeFromEuler(const float& _y,const float& _p,const float& _r) {
104 Quaternion_t roll( Cos(_r*0.5), Vector3_t(Sin(_r*0.5),0,0));
105 Quaternion_t pitch( Cos(_p*0.5), Vector3_t(0,Sin(_p*0.5),0));
106 Quaternion_t yaw( Cos(_y*0.5), Vector3_t(0,0,Sin(_y*0.5)));
107 (*this)=yaw*pitch*roll;
108 }
109
110
111 /// multiplication operator
112 Quaternion_t operator * (const Vector3_t& _v) const {
113 Quaternion_t r(0,_v);
114 return (*this)*r;}
115
116
117 /// Multiplication operator
118 Quaternion_t operator * (const Quaternion_t& _q) const {
119 return Quaternion_t( n*_q.n - ( v*_q.v ),
120 n*_q.v[0] + v[0]*_q.n + v[1]*_q.v[2] - v[2]*_q.v[1],
121 n*_q.v[1] + v[1]*_q.n + v[2]*_q.v[0] - v[0]*_q.v[2],
122 n*_q.v[2] + v[2]*_q.n + v[0]*_q.v[1] - v[1]*_q.v[0]);}
123
124 /// Multiplication operator
125 Quaternion_t operator * ( const float& _v ) const {
126 return Quaternion_t(n*_v,v*_v);}
127
128
129 /// Add two quaternion
130 Quaternion_t operator + (const Quaternion_t& _q) const {
131 return Quaternion_t(n+_q.n,v+_q.v); }
132
133
134 /// Sub two quaternion
135 Quaternion_t operator - (const Quaternion_t& _q) const {
136 return Quaternion_t(n-_q.n,v-_q.v); }
137
138
139
140 /// Comparaison operator ==
141 bool operator == ( const Quaternion_t& _q ) const {
142 return n == _q.n && v==_q.v; }
143
144
145 /// Comparaison operator !=
146 bool operator != ( const Quaternion_t& _q ) const {
147 return !(*this==_q); }
148
149
150 /// += operator
151 const Quaternion_t& operator += ( const Quaternion_t& _q ) {
152 n+=_q.n; v+=_q.v; return *this;}
153
154
155 /// -= operator
156 const Quaternion_t& operator -= ( const Quaternion_t& _q ) {
157 n-=_q.n; v-=_q.v; return *this;}
158
159
160 /// *= operator
161 const Quaternion_t& operator *= ( const float& _v ) {
162 n*=_v; v*=_v; return *this;}
163
164
165 /// /= operator
166 const Quaternion_t& operator /= ( const float& _v ) {
167 float a=(float)1.0/_v;
168 n*=a; v*=a; return *this;}
169
170
171 /// Operator -
172 Quaternion_t operator - () const {
173 return Quaternion_t(-n,-v);}
174
175
176 };
177
178
179 }
180
181
182 #endif