API Documentation
Loading...
Searching...
No Matches
Quaternion.hpp
Go to the documentation of this file.
1#pragma once
2#include <NDEVR/Vector.h>
3#include <NDEVR/Angle.h>
4#include <NDEVR/Matrix.h>
5namespace NDEVR
6{
7 /**--------------------------------------------------------------------------------------------------
8 Class: AxisAngle
9
10 \brief An angle about a particular, defined 3D axis
11 *-----------------------------------------------------------------------------------------------**/
12 template<class t_angle_type, class t_axis_type>
18 template<class t_type>
19 class Quaternion : public Vector<4, t_type>
20 {
21 public:
22 constexpr Quaternion()
23 : Vector<4, t_type>(t_type(1), t_type(0), t_type(0), t_type(0))
24 {}
25 constexpr Quaternion(const Vector<4, t_type>& vec)
26 : Vector<4, t_type>(vec)
27 {}
28 constexpr Quaternion(t_type x, t_type y, t_type z, t_type w)
29 : Vector<4, t_type>(x, y, z, w)
30 {}
31 constexpr explicit Quaternion(t_type x)
32 : Vector<4, t_type>(x, x, x, x)
33 {}
34 template<class t_angle_type, class t_axis_type>
35 constexpr Quaternion(Angle<t_angle_type> angle, Vector<3, t_axis_type> axis)
36 {
37 // Formula from http://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToQuaternion/
38 Vector<4, t_type>::m_values[W] = cos(angle / 2.0);
39 t_axis_type c = cast<t_axis_type>(sin(angle / 2.0));
40 for (uint01 i = 0; i < 3; i++)
41 Vector<4, t_type>::m_values[i] = cast<t_type>(c * axis[i]);
42 }
43
44 template<class t_angle_type>
45 AxisAngle<t_angle_type, t_type> axisAngle() const
46 {
47 AxisAngle<t_angle_type, t_type> axis_angle;
48 axis_angle.angle = 2.0 * Angle<t_angle_type>::acos(Vector<4, t_type>::m_values[W]);
49 t_type divider = sqrt(cast<t_type>(1) - Vector<4, t_type>::m_values[W] * Vector<4, t_type>::m_values[W]);
50
51 if (divider != 0.0)
52 {
53 // Calculate the axis
54 axis_angle.axis = Vector<4, t_type>::template as<3, t_type>() / divider;
55 }
56 else
57 {
58 // Arbitrary normalized axis
59 axis_angle.axis[X] = cast<t_type>(1);
60 axis_angle.axis[Y] = cast<t_type>(0);
61 axis_angle.axis[Z] = cast<t_type>(0);
62 }
63 return axis_angle;
64 }
65
66 template<class t_angle_type>
67 void setFromEuler(Vector<3, Angle<t_angle_type>> euler)
68 {
69 double cy = cos(euler[ROLL] * 0.5);
70 double sy = sin(euler[ROLL] * 0.5);
71 double cr = cos(euler[YAW] * 0.5);
72 double sr = sin(euler[YAW] * 0.5);
73 double cp = cos(euler[PITCH] * 0.5);
74 double sp = sin(euler[PITCH] * 0.5);
75
76 Vector<4, t_type>::m_values[W] = cy * cr * cp + sy * sr * sp;
77 Vector<4, t_type>::m_values[X] = cy * sr * cp - sy * cr * sp;
78 Vector<4, t_type>::m_values[Y] = cy * cr * sp + sy * sr * cp;
79 Vector<4, t_type>::m_values[Z] = sy * cr * cp - cy * sr * sp;
80 }
81
82 template<class t_angle_type>
83 Vector<3, Angle<t_angle_type>> euler() const
84 {
85 Vector<3, Angle<t_angle_type>> output;
86
87 // Roll (x-axis rotation)
88 t_type sinr_cosp = cast<t_type>(2) * (Vector<4, t_type>::m_values[W] * Vector<4, t_type>::m_values[X] + Vector<4, t_type>::m_values[Y] * Vector<4, t_type>::m_values[Z]);
89 t_type cosr_cosp = cast<t_type>(1) - cast<t_type>(2) * (Vector<4, t_type>::m_values[X] * Vector<4, t_type>::m_values[X] + Vector<4, t_type>::m_values[Y] * Vector<4, t_type>::m_values[Y]);
90 output[0] = Angle<t_angle_type>::atan2(sinr_cosp, cosr_cosp);
91
92 // Pitch (y-axis rotation)
93 t_type sinp = cast<t_type>(2) * (Vector<4, t_type>::m_values[W] * Vector<4, t_type>::m_values[1] - Vector<4, t_type>::m_values[2] * Vector<4, t_type>::m_values[0]);
94 if (abs(sinp) >= 1)
95 output[1] = sinp > 0 ? Angle<t_angle_type>(DEGREES, 90.0) : Angle<t_angle_type>(DEGREES, -90.0); // use 90 degrees if out of range
96 else
97 output[1] = Angle<t_angle_type>::asin(sinp);
98
99 // Yaw (z-axis rotation)
100 t_type siny_cosp = cast<t_type>(2) * (Vector<4, t_type>::m_values[W] * Vector<4, t_type>::m_values[2] + Vector<4, t_type>::m_values[0] * Vector<4, t_type>::m_values[1]);
101 t_type cosy_cosp = cast<t_type>(1) - cast<t_type>(2) * (Vector<4, t_type>::m_values[1] * Vector<4, t_type>::m_values[1] + Vector<4, t_type>::m_values[2] * Vector<4, t_type>::m_values[2]);
102 output[2] = Angle<t_angle_type>::atan2(siny_cosp, cosy_cosp);
103 }
104
105 Quaternion conjugate() const
106 {
107 Quaternion conj;
108 conj[W] = Vector<4, t_type>::m_values[W];
109 for (uint01 i = 0; i < 3; i++)
110 conj[i] = -Vector<4, t_type>::m_values[i];
111 return conj;
112 }
113 Quaternion invert() const
114 {
115 Quaternion q = conjugate();
116 return q / dot((*this), (*this));
117 }
118 const Quaternion operator/(t_type s) const
119 {
120 return Quaternion((*this)[X] / s, (*this)[Y] / s, (*this)[Z] / s, (*this)[W] / s);
121 }
122 Quaternion operator*(const Quaternion& q2) const
123 {
124 Quaternion result;
125
126 /*
127 Formula from http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/arithmetic/index.htm
128 a*e - b*f - c*g - d*h
129 + i (b*e + a*f + c*h- d*g)
130 + j (a*g - b*h + c*e + d*f)
131 + k (a*h + b*g - c*f + d*e)
132 */
133 result[W] = Vector<4, t_type>::m_values[W] * q2[W] - Vector<4, t_type>::m_values[0] * q2[0] - Vector<4, t_type>::m_values[1] * q2[1] - Vector<4, t_type>::m_values[2] * q2[2];
134 result[X] = Vector<4, t_type>::m_values[0] * q2[W] + Vector<4, t_type>::m_values[W] * q2[0] + Vector<4, t_type>::m_values[1] * q2[2] - Vector<4, t_type>::m_values[2] * q2[1];
135 result[Y] = Vector<4, t_type>::m_values[W] * q2[1] - Vector<4, t_type>::m_values[0] * q2[2] + Vector<4, t_type>::m_values[1] * q2[W] + Vector<4, t_type>::m_values[2] * q2[0];
136 result[Z] = Vector<4, t_type>::m_values[W] * q2[2] + Vector<4, t_type>::m_values[0] * q2[1] - Vector<4, t_type>::m_values[1] * q2[0] + Vector<4, t_type>::m_values[2] * q2[W];
137
138 return result;
139 }
140
141 template<class t_vec_type>
142 Vector<3, t_vec_type> operator*(const Vector<3, t_vec_type>& v) const
143 {
144 Vector<3, t_vec_type> result;
145
146 t_type ww = Vector<4, t_type>::m_values[W] * Vector<4, t_type>::m_values[W];
147 t_type xx = Vector<4, t_type>::m_values[0] * Vector<4, t_type>::m_values[0];
148 t_type yy = Vector<4, t_type>::m_values[1] * Vector<4, t_type>::m_values[1];
149 t_type zz = Vector<4, t_type>::m_values[2] * Vector<4, t_type>::m_values[2];
150 t_type wx = Vector<4, t_type>::m_values[W] * Vector<4, t_type>::m_values[0];
151 t_type wy = Vector<4, t_type>::m_values[W] * Vector<4, t_type>::m_values[1];
152 t_type wz = Vector<4, t_type>::m_values[W] * Vector<4, t_type>::m_values[2];
153 t_type xy = Vector<4, t_type>::m_values[0] * Vector<4, t_type>::m_values[1];
154 t_type xz = Vector<4, t_type>::m_values[0] * Vector<4, t_type>::m_values[2];
155 t_type yz = Vector<4, t_type>::m_values[1] * Vector<4, t_type>::m_values[2];
156
157 // Formula from http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/index.htm
158 // p2.x = w*w*p1.x + 2*y*w*p1.z - 2*z*w*p1.y + x*x*p1.x + 2*y*x*p1.y + 2*z*x*p1.z - z*z*p1.x - y*y*p1.x;
159 // p2.y = 2*x*y*p1.x + y*y*p1.y + 2*z*y*p1.z + 2*w*z*p1.x - z*z*p1.y + w*w*p1.y - 2*x*w*p1.z - x*x*p1.y;
160 // p2.z = 2*x*z*p1.x + 2*y*z*p1.y + z*z*p1.z - 2*w*y*p1.x - y*y*p1.z + 2*w*x*p1.y - x*x*p1.z + w*w*p1.z;
161
162 result[0] = ww * v[0] + 2 * wy * v[2] - 2 * wz * v[1] +
163 xx * v[0] + 2 * xy * v[1] + 2 * xz * v[2] -
164 zz * v[0] - yy * v[0];
165 result[1] = 2 * xy * v[0] + yy * v[1] + 2 * yz * v[2] +
166 2 * wz * v[0] - zz * v[1] + ww * v[1] -
167 2 * wx * v[2] - xx * v[1];
168 result[2] = 2 * xz * v[0] + 2 * yz * v[1] + zz * v[2] -
169 2 * wy * v[0] - yy * v[2] + 2 * wx * v[1] -
170 xx * v[2] + ww * v[2];
171 return result;
172 }
173 /*
174 void Quaternion_slerp(Quaternion* q1, Quaternion* q2, double t, Quaternion* output)
175 {
176 Quaternion result;
177
178 // Based on http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/slerp/index.htm
179 double cosHalfTheta = q1->w * q2->w + q1->v[0] * q2->v[0] + q1->v[1] * q2->v[1] + q1->v[2] * q2->v[2];
180
181 // if q1=q2 or qa=-q2 then theta = 0 and we can return qa
182 if (fabs(cosHalfTheta) >= 1.0) {
183 Quaternion_copy(q1, output);
184 return;
185 }
186
187 double halfTheta = acos(cosHalfTheta);
188 double sinHalfTheta = sqrt(1.0 - cosHalfTheta * cosHalfTheta);
189 // If theta = 180 degrees then result is not fully defined
190 // We could rotate around any axis normal to q1 or q2
191 if (fabs(sinHalfTheta) < QUATERNION_EPS) {
192 result.w = (q1->w * 0.5 + q2->w * 0.5);
193 result.v[0] = (q1->v[0] * 0.5 + q2->v[0] * 0.5);
194 result.v[1] = (q1->v[1] * 0.5 + q2->v[1] * 0.5);
195 result.v[2] = (q1->v[2] * 0.5 + q2->v[2] * 0.5);
196 }
197 else {
198 // Default quaternion calculation
199 double ratioA = sin((1 - t) * halfTheta) / sinHalfTheta;
200 double ratioB = sin(t * halfTheta) / sinHalfTheta;
201 result.w = (q1->w * ratioA + q2->w * ratioB);
202 result.v[0] = (q1->v[0] * ratioA + q2->v[0] * ratioB);
203 result.v[1] = (q1->v[1] * ratioA + q2->v[1] * ratioB);
204 result.v[2] = (q1->v[2] * ratioA + q2->v[2] * ratioB);
205 }
206 *output = result;
207 }*/
208
209
210 inline Matrix<t_type> toMatrix() const
211 {
212 // NOTE: assume the quaternion is unit length
213 // compute common values
214 t_type x2 = (*this)[X] + (*this)[X];
215 t_type y2 = (*this)[Y] + (*this)[Y];
216 t_type z2 = (*this)[Z] + (*this)[Z];
217 t_type xx2 = (*this)[X] * x2;
218 t_type xy2 = (*this)[X] * y2;
219 t_type xz2 = (*this)[X] * z2;
220 t_type yy2 = (*this)[Y] * y2;
221 t_type yz2 = (*this)[Y] * z2;
222 t_type zz2 = (*this)[Z] * z2;
223 t_type sx2 = (*this)[W] * x2;
224 t_type sy2 = (*this)[W] * y2;
225 t_type sz2 = (*this)[W] * z2;
226
227 // build 4x4 matrix (column-major) and return
228 return Matrix<t_type>(
229 1 - (yy2 + zz2), xy2 + sz2, xz2 - sy2, 0,
230 xy2 - sz2, 1 - (xx2 + zz2), yz2 + sx2, 0,
231 xz2 + sy2, yz2 - sx2, 1 - (xx2 + yy2), 0,
232 0, 0, 0, 1);//
233 }
234 };
235 static Quaternion<fltp08> difference(const Quaternion<fltp08>& q1, const Quaternion<fltp08>& q2)
236 {
237 return q1.invert() * q2;
238 }
239 static Quaternion<fltp04> difference(const Quaternion<fltp04>& q1, const Quaternion<fltp04>& q2)
240 {
241 return q1.invert() * q2;
242 }
243 /**--------------------------------------------------------------------------------------------------
244 Struct: Constant<Vertex<t_dims,t_type,t_vector_type>>
245
246 A constant.
247
248 Author: Tyler Parke
249
250 Date: 2017-11-19
251 *-----------------------------------------------------------------------------------------------**/
252 template<class t_type>
253 struct Constant<Quaternion<t_type>>
254 {
255 constexpr const static Quaternion<t_type> NaN{ Constant<t_type>::NaN };
256 constexpr const static Quaternion<t_type> Min{ 0 };
257 constexpr const static Quaternion<t_type> Max{ Constant<t_type>::Max };
258 };
259
260 /**--------------------------------------------------------------------------------------------------
261 Fn: template<uint01 t_dims, class t_type, class t_vector_type> static constexpr bool isNaN(const Vertex<t_dims, t_type, t_vector_type>& value)
262
263 Query if 'value' is NaN.
264
265 Author: Tyler Parke
266
267 Date: 2017-11-19
268
269 Typeparams:
270 t_dims - Type of the dims.
271 t_type - Type of the type.
272 t_vector_type - Type of the vector type.
273 Parameters:
274 value - The value.
275
276 Returns: True if nan, false if not.
277 *-----------------------------------------------------------------------------------------------**/
278
279 template<class t_type>
280 static constexpr bool isNaN(const Quaternion<t_type>& value)
281 {
282 for (uint01 dim = 0; dim < 4; ++dim)
283 {
284 if (isNaN(value[dim]))
285 return true;
286 }
287 return false;
288 }
289}
NDEVR::Angle
Stores an angle in an optimized format.
Definition StringStream.h:352
NDEVR::Angle::atan2
static Angle atan2(fltp04 value_a, fltp04 value_b)
Definition Angle.h:481
NDEVR::Angle::asin
static Angle asin(fltp04 value)
Definition Angle.h:461
NDEVR::Angle::acos
static Angle acos(fltp04 value)
Definition Angle.h:437
NDEVR::Matrix
Definition Matrix.hpp:173
NDEVR::Quaternion
Definition Quaternion.hpp:20
NDEVR::Quaternion::Quaternion
constexpr Quaternion()
Definition Quaternion.hpp:22
NDEVR::Quaternion::euler
Vector< 3, Angle< t_angle_type > > euler() const
Definition Quaternion.hpp:83
NDEVR::Quaternion::axisAngle
AxisAngle< t_angle_type, t_type > axisAngle() const
Definition Quaternion.hpp:45
NDEVR::Quaternion::operator*
Quaternion operator*(const Quaternion &q2) const
Definition Quaternion.hpp:122
NDEVR::Quaternion::Quaternion
constexpr Quaternion(const Vector< 4, t_type > &vec)
Definition Quaternion.hpp:25
NDEVR::Quaternion::conjugate
Quaternion conjugate() const
Definition Quaternion.hpp:105
NDEVR::Quaternion::setFromEuler
void setFromEuler(Vector< 3, Angle< t_angle_type > > euler)
Definition Quaternion.hpp:67
NDEVR::Quaternion::Quaternion
constexpr Quaternion(Angle< t_angle_type > angle, Vector< 3, t_axis_type > axis)
Definition Quaternion.hpp:35
NDEVR::Quaternion::operator/
const Quaternion operator/(t_type s) const
Definition Quaternion.hpp:118
NDEVR::Quaternion::toMatrix
Matrix< t_type > toMatrix() const
Definition Quaternion.hpp:210
NDEVR::Quaternion::Quaternion
constexpr Quaternion(t_type x, t_type y, t_type z, t_type w)
Definition Quaternion.hpp:28
NDEVR::Quaternion::operator*
Vector< 3, t_vec_type > operator*(const Vector< 3, t_vec_type > &v) const
Definition Quaternion.hpp:142
NDEVR::Quaternion::Quaternion
constexpr Quaternion(t_type x)
Definition Quaternion.hpp:31
NDEVR::Quaternion::invert
Quaternion invert() const
Definition Quaternion.hpp:113
NDEVR::Vector
An element of a vector space. An element of the real coordinate space Rn Basis vector,...
Definition Vector.hpp:62
NDEVR::Vector< 4, t_type >::as
constexpr Vector< t_dims, t_new_type > as() const
Definition Vector.hpp:324
NDEVR
Definition ACIColor.h:37
NDEVR::dot
t_type dot(const Vector< t_dims, t_type > &v1, const Vector< t_dims, t_type > &v2)
Definition VectorFunctions.hpp:1096
NDEVR::ROLL
@ ROLL
Definition Angle.h:51
NDEVR::YAW
@ YAW
Definition Angle.h:53
NDEVR::PITCH
@ PITCH
Definition Angle.h:52
NDEVR::uint01
uint8_t uint01
-Defines an alias representing a 1 byte, unsigned integer -Can represent exact integer values 0 throu...
Definition BaseValues.hpp:98
NDEVR::DEGREES
@ DEGREES
Definition Angle.h:66
NDEVR::cos
std::enable_if<!ObjectInfo< t_type >::Float, fltp08 >::type cos(const Angle< t_type > &angle)
Definition AngleFunctions.h:154
NDEVR::sqrt
t_type sqrt(const t_type &value)
Definition VectorFunctions.hpp:1309
NDEVR::cast
constexpr t_to cast(const Angle< t_from > &value)
Definition Angle.h:514
NDEVR::abs
constexpr Angle< t_angle_type > abs(const Angle< t_angle_type > &value)
Definition AngleFunctions.h:750
NDEVR::sin
std::enable_if<!ObjectInfo< t_type >::Float, fltp08 >::type sin(const Angle< t_type > &angle)
Definition AngleFunctions.h:113
NDEVR::Y
@ Y
Definition BaseValues.hpp:202
NDEVR::X
@ X
Definition BaseValues.hpp:200
NDEVR::Z
@ Z
Definition BaseValues.hpp:204
NDEVR::W
@ W
Definition BaseValues.hpp:206
NDEVR::AxisAngle
An angle about a particular, defined 3D axis.
Definition Quaternion.hpp:14
NDEVR::AxisAngle::axis
Vector< 3, t_axis_type > axis
Definition Quaternion.hpp:16
NDEVR::AxisAngle::angle
Angle< t_angle_type > angle
Definition Quaternion.hpp:15
NDEVR::Constant
Definition BaseValues.hpp:272
NDEVR::Constant::Min
static const t_type Min
Definition BaseValues.hpp:276
NDEVR::Constant::Max
static const t_type Max
Definition BaseValues.hpp:278
NDEVR::Constant::NaN
static const t_type NaN
Definition BaseValues.hpp:274