NCBI C++ ToolKit
quat.hpp
Go to the documentation of this file.

Go to the SVN repository for this file.

1 #ifndef GUI_MATH___QUAT___HPP
2 #define GUI_MATH___QUAT___HPP
3 
4 /* $Id: quat.hpp 14973 2007-09-14 12:11:32Z dicuccio $
5  * ===========================================================================
6  *
7  * PUBLIC DOMAIN NOTICE
8  * National Center for Biotechnology Information
9  *
10  * This software / database is a "United States Government Work" under the
11  * terms of the United States Copyright Act. It was written as part of
12  * the author's official duties as a United States Government employee and
13  * thus cannot be copyrighted. This software / database is freely available
14  * to the public for use. The National Library of Medicine and the U.S.
15  * Government have not placed any restriction on its use or reproduction.
16  *
17  * Although all reasonable efforts have been taken to ensure the accuracy
18  * and reliability of the software and data, the NLM and the U.S.
19  * Government do not and cannot warrant the performance or results that
20  * may be obtained by using this software or data. The NLM and the U.S.
21  * Government disclaim all warranties, express or implied, including
22  * warranties of performance, merchantability or fitness for any particular
23  * purpose.
24  *
25  * Please cite the author in any work or product based on this material.
26  *
27  * ===========================================================================
28  *
29  * Authors: Mike DiCuccio
30  *
31  * File Description:
32  *
33  */
34 
35 #include <corelib/ncbistd.hpp>
36 
37 #include <math.h>
38 
39 #include <gui/utils/vect3.hpp>
40 #include <gui/utils/matrix4.hpp>
41 
42 /** @addtogroup GUI_MATH
43  *
44  * @{
45  */
46 
48 
49 template <class T>
50 class CQuat
51 {
52 public:
53  // default ctor
54  CQuat(void);
55  // conversion ctor
56  CQuat(T x, T y, T z, T w);
57  // conversion ctor: rotation of degrees about axis
58  CQuat(const CVect3<T>& axis, T degrees);
59 
60  // operator: index
61  T& operator[] (int i) { return m_Xyzw[i]; }
62  const T& operator[] (int i) const { return m_Xyzw[i]; }
63 
64  // operator: *=
66  // operator: +=
68  // operator: -=
70 
71  // access data through named functions
72  T& X(void) { return m_Xyzw[0]; }
73  const T& X(void) const { return m_Xyzw[0]; }
74 
75  T& Y(void) { return m_Xyzw[1]; }
76  const T& Y(void) const { return m_Xyzw[1]; }
77 
78  T& Z(void) { return m_Xyzw[2]; }
79  const T& Z(void) const { return m_Xyzw[2]; }
80 
81  T& W(void) { return m_Xyzw[3]; }
82  const T& W(void) const { return m_Xyzw[3]; }
83 
84  //
85  // other named functions
86 
87  // create a rotation matric from a quaternion
88  void ToMatrix(CMatrix4<T>&) const;
89  CMatrix4<T> ToMatrix(void) const;
90 
91  // get the conjugate of a quaternion
92  CQuat<T> Conjugate(void) const;
93 
94  // return the magnitude of a quaternion
95  T Length(void) const;
96 
97  // return the magnitude squared of a quaternion
98  T Length2(void) const;
99 
100  // inverse of a quaterion(1 / q) = q_conj / (q * q_conj)
101  CQuat<T> Inverse(void) const;
102 
103  // rotate a vector using the current quaternion
104  void Rotate(CVect3<float>& point) const;
105 
106 protected: // data
107  // the data is stored as a 4 - element array
108  T m_Xyzw[4];
109 
110 protected: // internals
111 
112 };
113 
114 //
115 // global operators
116 //
117 
118 //
119 // operator: compare - equals
120 template <class T>
121 inline bool
122 operator== (const CQuat<T>& first, const CQuat<T>& second)
123 {
124  return (first[0] == second[0] &&
125  first[1] == second[1] &&
126  first[2] == second[2] &&
127  first[3] == second[3]);
128 }
129 
130 //
131 // operator: compare - not - equals
132 template <class T>
133 inline bool
134 operator!= (const CQuat<T>& first, const CQuat<T>& second)
135 {
136  return !(first == second);
137 }
138 
139 //
140 // operator: multiply
141 template <class T>
142 inline CQuat<T>
143 operator* (const CQuat<T>& first, const CQuat<T>& second)
144 {
145  return (CQuat<T> (first) *= second);
146 }
147 
148 //
149 // operator: multiply
150 template <class T>
151 inline CQuat<T>
152 operator* (const CQuat<T>& first, T second)
153 {
154  return (CQuat<T> (first.X() * second,
155  first.Y() * second,
156  first.Z() * second,
157  first.W() * second));
158 }
159 
160 //
161 // operator: multiply
162 template <class T>
163 inline CQuat<T>
164 operator* (T first, const CQuat<T>& second)
165 {
166  return (CQuat<T> (second.X() * first,
167  second.Y() * first,
168  second.Z() * first,
169  second.W() * first));
170 }
171 
172 //
173 // operator: divide
174 template <class T>
175 inline CQuat<T>
176 operator/ (const CQuat<T>& first, T second)
177 {
178  return (CQuat<T> (first.X() / second,
179  first.Y() / second,
180  first.Z() / second,
181  first.W() / second));
182 }
183 
184 //
185 // operator: add
186 template <class T>
187 inline CQuat<T>
188 operator+ (const CQuat<T>& first, const CQuat<T>& second)
189 {
190  return (CQuat<T> (first) += second);
191 }
192 
193 //
194 // operator: subtract
195 template <class T>
196 inline CQuat<T>
197 operator- (const CQuat<T>& first, const CQuat<T>& second)
198 {
199  return (CQuat<T> (first) -= second);
200 }
201 
202 //
203 // default ctor
204 template <class T>
205 inline
207 {
208  m_Xyzw[0] = T(0);
209  m_Xyzw[1] = T(0);
210  m_Xyzw[2] = T(0);
211  m_Xyzw[3] = T(0);
212 }
213 
214 
215 //
216 // conversion ctor
217 template <class T>
218 inline
219 CQuat<T>::CQuat(T x, T y, T z, T w)
220 {
221  m_Xyzw[0] = x;
222  m_Xyzw[1] = y;
223  m_Xyzw[2] = z;
224  m_Xyzw[3] = w;
225 }
226 
227 //
228 // conversion ctor: rotation quat
229 // we want to create a rotation quat about an axis
230 template <class T>
231 inline
232 CQuat<T>::CQuat(const CVect3<T>& axis, T theta)
233 {
234  theta *= math::pi / 180.0f;
235  double cos_theta = cos(theta / 2.0);
236  double sin_theta = sin(theta / 2.0);
237 
238  m_Xyzw[0] = T(axis[0]*sin_theta);
239  m_Xyzw[1] = T(axis[1]*sin_theta);
240  m_Xyzw[2] = T(axis[2]*sin_theta);
241  m_Xyzw[3] = T(cos_theta);
242 }
243 
244 //
245 // operator: *=
246 template <class T>
247 inline CQuat<T>&
249 {
250  T new_x = W() * q.X() + X() * q.W() + ( Y() * q.Z() - Z() * q.Y() );
251  T new_y = W() * q.Y() + Y() * q.W() + ( Z() * q.X() - X() * q.Z() );
252  T new_z = W() * q.Z() + Z() * q.W() + ( X() * q.Y() - Y() * q.X() );
253  T new_w = W() * q.W() - (X() * q.X() + Y() * q.Y() + Z() * q.Z());
254 
255  m_Xyzw[0] = new_x;
256  m_Xyzw[1] = new_y;
257  m_Xyzw[2] = new_z;
258  m_Xyzw[3] = new_w;
259 
260  return *this;
261 }
262 
263 //
264 // operator: -=
265 template <class T>
266 inline CQuat<T>&
268 {
269  X() -= q.X();
270  Y() -= q.Y();
271  Z() -= q.Z();
272  W() -= q.W();
273 
274  return *this;
275 }
276 
277 //
278 // operator: +=
279 template <class T>
280 inline CQuat<T>&
282 {
283  X() += q.X();
284  Y() += q.Y();
285  Z() += q.Z();
286  W() += q.W();
287 
288  return *this;
289 }
290 
291 
292 //
293 // create a rotation matrix from a quaternion
294 template <class T>
295 inline CMatrix4<T>
297 {
298  CMatrix4<T> m;
299  ToMatrix(m);
300  return m;
301 }
302 
303 
304 template <class T>
305 inline void
307 {
308  T Nq = m_Xyzw[0]*m_Xyzw[0] +
309  m_Xyzw[1]*m_Xyzw[1] +
310  m_Xyzw[2]*m_Xyzw[2] +
311  m_Xyzw[3]*m_Xyzw[3];
312  T s = (Nq > 0) ? (T(2.0) / Nq) : 0;
313  T xs = m_Xyzw[0]*s;
314  T ys = m_Xyzw[1]*s;
315  T zs = m_Xyzw[2]*s;
316  T wx = m_Xyzw[3]*xs;
317  T wy = m_Xyzw[3]*ys;
318  T wz = m_Xyzw[3]*zs;
319  T xx = m_Xyzw[0]*xs;
320  T xy = m_Xyzw[0]*ys;
321  T xz = m_Xyzw[0]*zs;
322  T yy = m_Xyzw[1]*ys;
323  T yz = m_Xyzw[1]*zs;
324  T zz = m_Xyzw[2]*zs;
325 
326  m(0,0) = T(1.0) - (yy + zz);
327  m(1,0) = xy + wz;
328  m(2,0) = xz - wy;
329  m(0,1) = xy - wz;
330  m(1,1) = T(1.0) - (xx + zz);
331  m(2,1) = yz + wx;
332  m(0,2) = xz + wy;
333  m(1,2) = yz - wx;
334  m(2,2) = T(1.0) - (xx + yy);
335  m(0,3) = T(0.0);
336  m(1,3) = T(0.0);
337  m(2,3) = T(0.0);
338  m(3,0) = T(0.0);
339  m(3,1) = T(0.0);
340  m(3,2) = T(0.0);
341  m(3,3) = T(1.0);
342 }
343 
344 //
345 // return the conjugate of a quaternion
346 template <class T>
347 inline CQuat<T>
349 {
350  CQuat<T> q(*this);
351  q.X() = -q.X();
352  q.Y() = -q.Y();
353  q.Z() = -q.Z();
354  return q;
355 }
356 
357 //
358 // get the length(magnitude) of a quaternion
359 template <class T>
360 inline T
362 {
363  return sqrt(Length2());
364 }
365 
366 //
367 // get the length(magnitude) squared of a quaternion
368 template <class T>
369 inline T
371 {
372  return (m_Xyzw[0]*m_Xyzw[0] +
373  m_Xyzw[1]*m_Xyzw[1] +
374  m_Xyzw[2]*m_Xyzw[2] +
375  m_Xyzw[3]*m_Xyzw[3]);
376 }
377 
378 //
379 // get the inverse of a quaternion
380 template <class T>
381 inline CQuat<T>
383 {
384  return (Conjugate() / Length2());
385 }
386 
387 //
388 // rotate a point using a quaternion
389 template <class T>
390 inline void
392 {
393  // formula is qPq', where
394  // P = (0,p)
395  CQuat<T> p(point.X(), point.Y(), point.Z(), 0);
396 
397  // there's a little extra math done in here
398  // W() is 0, so we could eliminate this
399  p = *this * p;
400  p *= Inverse();
401 
402  point.X() = p.X();
403  point.Y() = p.Y();
404  point.Z() = p.Z();
405  // w remains 0 throughout
406 }
407 
408 //
409 // streams - based output
410 template <class T>
411 ostream&
412 operator<< (ostream& o, const CQuat<T>& quat)
413 {
414  o << "[(" << quat.X() << ", " << quat.Y() << ", " << quat.Z()
415  << "), " << quat.W() << "]";
416 
417  return o;
418 }
419 
420 
422 
423 /* @} */
424 
425 #endif // GUI_MATH___QUAT___HPP
Definition: quat.hpp:51
Definition: vect3.hpp:48
Include a standard set of the NCBI C++ Toolkit most basic headers.
#define T(s)
Definition: common.h:230
static DLIST_TYPE *DLIST_NAME() first(DLIST_LIST_TYPE *list)
Definition: dlist.tmpl.h:46
void ToMatrix(CMatrix4< T > &) const
Definition: quat.hpp:306
CQuat< T > & operator+=(const CQuat< T > &)
Definition: quat.hpp:281
T & W(void)
Definition: quat.hpp:81
CQuat< T > Conjugate(void) const
Definition: quat.hpp:348
T & Z(void)
Definition: quat.hpp:78
CQuat< T > Inverse(void) const
Definition: quat.hpp:382
const T & Y(void) const
Definition: quat.hpp:76
T & Z()
Definition: vect3.hpp:114
CQuat< T > operator-(const CQuat< T > &first, const CQuat< T > &second)
Definition: quat.hpp:197
CQuat(void)
Definition: quat.hpp:206
ostream & operator<<(ostream &o, const CQuat< T > &quat)
Definition: quat.hpp:412
T & Y(void)
Definition: quat.hpp:75
const T & W(void) const
Definition: quat.hpp:82
T & X(void)
Definition: quat.hpp:72
T & X()
Definition: vect3.hpp:110
CMatrix4< T > ToMatrix(void) const
Definition: quat.hpp:296
const T & X(void) const
Definition: quat.hpp:73
CQuat< T > operator*(const CQuat< T > &first, const CQuat< T > &second)
Definition: quat.hpp:143
CQuat(const CVect3< T > &axis, T degrees)
Definition: quat.hpp:232
const T & Z(void) const
Definition: quat.hpp:79
void Rotate(CVect3< float > &point) const
Definition: quat.hpp:391
CQuat< T > operator+(const CQuat< T > &first, const CQuat< T > &second)
Definition: quat.hpp:188
CQuat(T x, T y, T z, T w)
Definition: quat.hpp:219
T & Y()
Definition: vect3.hpp:112
const float pi
Definition: math.hpp:54
CQuat< T > operator/(const CQuat< T > &first, T second)
Definition: quat.hpp:176
T Length(void) const
Definition: quat.hpp:361
T Length2(void) const
Definition: quat.hpp:370
T m_Xyzw[4]
Definition: quat.hpp:108
T & operator[](int i)
Definition: quat.hpp:61
bool operator!=(const CQuat< T > &first, const CQuat< T > &second)
Definition: quat.hpp:134
bool operator==(const CQuat< T > &first, const CQuat< T > &second)
Definition: quat.hpp:122
CQuat< T > & operator*=(const CQuat< T > &)
Definition: quat.hpp:248
CQuat< T > & operator-=(const CQuat< T > &)
Definition: quat.hpp:267
#define END_NCBI_SCOPE
End previously defined NCBI scope.
Definition: ncbistl.hpp:103
#define BEGIN_NCBI_SCOPE
Define ncbi namespace.
Definition: ncbistl.hpp:100
#define Z
printf format modifier for size_t
Definition: mdb.c:334
int i
#define W
Definition: crc32.c:85
Modified on Sat May 25 14:19:09 2024 by modify_doxy.py rev. 669887