1 #ifndef __GNUC__
2 #pragma once
3 #endif
4 #ifndef __XR_QUATERNION_H__
5 #define __XR_QUATERNION_H__
6
7 namespace xray_re {
8
9 template<typename T> struct _vector3;
10 template<typename T> struct _matrix;
11
12 template<typename T> struct _quaternion {
13 _quaternion<T>& identity();
14 _quaternion<T>& set(const _matrix<T>& m);
15 _quaternion<T>& set(const _quaternion<T>& a);
16 _quaternion<T>& set(T _x, T _y, T _z, T _w);
17 _quaternion<T>& mul(T s);
18 _quaternion<T>& div(T s);
19 _quaternion<T>& normalize();
20 _quaternion<T>& rotation(const _vector3<T>& axis, T theta);
21 _quaternion<T>& rotation_ypr(T _y, T _p, T _r);
22 _quaternion<T>& rotation(const _vector3<T>& v1, const _vector3<T>& v2);
23 _quaternion<T>& slerp(const _quaternion<T>& q1, const _quaternion<T>& q2, T t);
24 T magnitude() const;
25 T dot_product(const _quaternion<T>& q) const;
26 bool similar(const _quaternion<T>& a, T e = T(1e-6)) const;
27
28 union {
29 struct {
30 T x, y, z, w;
31 };
32 _vector3<T> xyz;
33 };
34 };
35
36 typedef _quaternion<float> fquaternion;
37 typedef _quaternion<double> dquaternion;
38
identity()39 template<typename T> inline _quaternion<T>& _quaternion<T>::identity()
40 {
41 x = 0;
42 y = 0;
43 z = 0;
44 w = T(1);
45 return *this;
46 }
47
set(T _x,T _y,T _z,T _w)48 template<typename T> inline _quaternion<T>& _quaternion<T>::set(T _x, T _y, T _z, T _w)
49 {
50 x = _x;
51 y = _y;
52 z = _z;
53 w = _w;
54 return *this;
55 }
56
set(const _matrix<T> & m)57 template<typename T> _quaternion<T>& _quaternion<T>::set(const _matrix<T>& m)
58 {
59 T trace = m._11 + m._22 + m._33;
60 if (trace > 0) {
61 T s = std::sqrt(trace + T(1));
62 w = s*T(0.5);
63 xr_assert(s > T(1e-6));
64 s = T(0.5)/s;
65 x = s*(m._32 - m._23);
66 y = s*(m._13 - m._31);
67 z = s*(m._21 - m._12);
68 } else {
69 int i = 0;
70 if (m._22 > m._11)
71 i = 1;
72 if (m._33 > m.m[i][i])
73 i = 2;
74 int j = (1 << i)&3, k = (1 << j)&3;
75 // int j = (i + 1)%3, k = (j + 1)%3;
76 T s = std::sqrt(m.m[i][i] - m.m[j][j] - m.m[k][k] + T(1));
77 xyz[i] = s*T(0.5);
78 xr_assert(s > T(1e-6));
79 s = T(0.5)/s;
80 w = s*(m.m[k][j] - m.m[j][k]);
81 xyz[j] = s*(m.m[j][i] + m.m[i][j]);
82 xyz[k] = s*(m.m[k][i] + m.m[i][k]);
83 }
84 return *this;
85 }
86
set(const _quaternion<T> & a)87 template<typename T> inline _quaternion<T>& _quaternion<T>::set(const _quaternion<T>& a)
88 {
89 x = a.x;
90 y = a.y;
91 z = a.z;
92 w = a.w;
93 return *this;
94 }
95
mul(T s)96 template<typename T> inline _quaternion<T>& _quaternion<T>::mul(T s)
97 {
98 x *= s;
99 y *= s;
100 z *= s;
101 w *= s;
102 return *this;
103 }
104
div(T s)105 template<typename T> inline _quaternion<T>& _quaternion<T>::div(T s)
106 {
107 x /= s;
108 y /= s;
109 z /= s;
110 w /= s;
111 return *this;
112 }
113
114 template<typename T> inline
normalize()115 _quaternion<T>& _quaternion<T>::normalize() { return div(magnitude()); }
116
117 template<typename T> inline
rotation(const _vector3<T> & axis,T theta)118 _quaternion<T>& _quaternion<T>::rotation(const _vector3<T>& axis, T theta)
119 {
120 w = std::cos(theta *= T(0.5));
121 xyz.mul(axis, std::sin(theta));
122 return *this;
123 }
124
125 template<typename T> inline
rotation(const _vector3<T> & v1,const _vector3<T> & v2)126 _quaternion<T>& _quaternion<T>::rotation(const _vector3<T>& v1, const _vector3<T>& v2)
127 {
128 _vector3<T> m;
129 if ((w = v1.dot_product(m.add(v1, v2).normalize_safe())) == 0) {
130 if (std::abs(v1.x) > std::abs(v1.y)) {
131 T inv_l = 1/std::sqrt(v1.x*v1.x + v1.z*v1.z);
132 x = -v1.z*inv_l;
133 y = 0;
134 z = v1.x*inv_l;
135 } else {
136 T inv_l = 1/std::sqrt(v1.y*v1.y + v1.z*v1.z);
137 x = 0;
138 y = v1.z*inv_l;
139 z = -v1.y*inv_l;
140 }
141 } else {
142 xyz.cross_product(v1, m);
143 }
144 return *this;
145 }
146
147 template<typename T>
slerp(const _quaternion<T> & q1,const _quaternion<T> & q2,T t)148 _quaternion<T>& _quaternion<T>::slerp(const _quaternion<T>& q1, const _quaternion<T>& q2, T t)
149 {
150 T omega = std::acos(q1.dot_product(q2));
151 if (!equivalent<T>(omega, 0)) {
152 T sin_omega = T(1)/std::sin(omega);
153 T k1 = std::sin((T(1) - t)*omega)/sin_omega;
154 T k2 = std::sin(t*omega)/sin_omega;
155 x = k1*q1.x + k2*q2.x;
156 y = k1*q1.y + k2*q2.y;
157 z = k1*q1.z + k2*q2.z;
158 w = k1*q1.w + k2*q2.w;
159 return *this;
160 } else {
161 return set(q1);
162 }
163 }
164
165 template<typename T> inline
magnitude()166 T _quaternion<T>::magnitude() const { return std::sqrt(x*x + y*y + z*z + w*w); }
167
168 template<typename T> inline
dot_product(const _quaternion<T> & q)169 T _quaternion<T>::dot_product(const _quaternion<T>& q) const { return x*q.x + y*q.y + z*q.z + w*q.w; }
170
similar(const _quaternion<T> & a,T e)171 template<typename T> bool _quaternion<T>::similar(const _quaternion<T>& a, T e) const
172 {
173 if (e <= std::abs(a.x - x))
174 return false;
175 if (e <= std::abs(a.y - y))
176 return false;
177 if (e <= std::abs(a.z - z))
178 return false;
179 if (e <= std::abs(a.w - w))
180 return false;
181 return true;
182 }
183
184 } // end of namespace xray_re
185
186 #endif
187