NDEVR
API Documentation
Quaternion.hpp
1#pragma once
2#include <NDEVR/Vector.h>
3#include <NDEVR/Angle.h>
4#include <NDEVR/Matrix.h>
5namespace NDEVR
6{
12 template<class t_angle_type, class t_axis_type>
13 struct AxisAngle
14 {
15 Angle<t_angle_type> angle;
16 Vector<3, t_axis_type> axis;
17 };
18
21 template<class t_type>
22 class Quaternion : public Vector<4, t_type>
23 {
24 public:
25 constexpr Quaternion()
26 : Vector<4, t_type>(t_type(1), t_type(0), t_type(0), t_type(0))
27 {}
28 constexpr Quaternion(const Vector<4, t_type>& vec)
29 : Vector<4, t_type>(vec)
30 {}
31 constexpr Quaternion(t_type x, t_type y, t_type z, t_type w)
32 : Vector<4, t_type>(x, y, z, w)
33 {}
34 constexpr explicit Quaternion(t_type x)
35 : Vector<4, t_type>(x, x, x, x)
36 {}
37 template<class t_angle_type, class t_axis_type>
38 constexpr Quaternion(Angle<t_angle_type> angle, Vector<3, t_axis_type> axis)
39 {
40 // Formula from http://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToQuaternion/
41 Vector<4, t_type>::m_values[W] = cos(angle / 2.0);
42 t_axis_type c = cast<t_axis_type>(sin(angle / 2.0));
43 for (uint01 i = 0; i < 3; i++)
44 Vector<4, t_type>::m_values[i] = cast<t_type>(c * axis[i]);
45 }
46
47 template<class t_angle_type>
48 AxisAngle<t_angle_type, t_type> axisAngle() const
49 {
50 AxisAngle<t_angle_type, t_type> axis_angle;
51 axis_angle.angle = 2.0 * Angle<t_angle_type>::acos(Vector<4, t_type>::m_values[W]);
52 t_type divider = sqrt(cast<t_type>(1) - Vector<4, t_type>::m_values[W] * Vector<4, t_type>::m_values[W]);
53
54 if (divider != 0.0)
55 {
56 // Calculate the axis
57 axis_angle.axis = Vector<4, t_type>::template as<3, t_type>() / divider;
58 }
59 else
60 {
61 // Arbitrary normalized axis
62 axis_angle.axis[X] = cast<t_type>(1);
63 axis_angle.axis[Y] = cast<t_type>(0);
64 axis_angle.axis[Z] = cast<t_type>(0);
65 }
66 return axis_angle;
67 }
68
69 template<class t_angle_type>
70 void setFromEuler(Vector<3, Angle<t_angle_type>> euler)
71 {
72 double cy = cos(euler[ROLL] * 0.5);
73 double sy = sin(euler[ROLL] * 0.5);
74 double cr = cos(euler[YAW] * 0.5);
75 double sr = sin(euler[YAW] * 0.5);
76 double cp = cos(euler[PITCH] * 0.5);
77 double sp = sin(euler[PITCH] * 0.5);
78
79 Vector<4, t_type>::m_values[W] = cy * cr * cp + sy * sr * sp;
80 Vector<4, t_type>::m_values[X] = cy * sr * cp - sy * cr * sp;
81 Vector<4, t_type>::m_values[Y] = cy * cr * sp + sy * sr * cp;
82 Vector<4, t_type>::m_values[Z] = sy * cr * cp - cy * sr * sp;
83 }
84
85 template<class t_angle_type>
86 Vector<3, Angle<t_angle_type>> euler() const
87 {
88 Vector<3, Angle<t_angle_type>> output;
89
90 // Roll (x-axis rotation)
91 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]);
92 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]);
93 output[0] = Angle<t_angle_type>::atan2(sinr_cosp, cosr_cosp);
94
95 // Pitch (y-axis rotation)
96 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]);
97 if (abs(sinp) >= 1)
98 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
99 else
100 output[1] = Angle<t_angle_type>::asin(sinp);
101
102 // Yaw (z-axis rotation)
103 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]);
104 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]);
105 output[2] = Angle<t_angle_type>::atan2(siny_cosp, cosy_cosp);
106 }
107
108 Quaternion conjugate() const
109 {
110 Quaternion conj;
111 conj[W] = Vector<4, t_type>::m_values[W];
112 for (uint01 i = 0; i < 3; i++)
113 conj[i] = -Vector<4, t_type>::m_values[i];
114 return conj;
115 }
116 Quaternion invert() const
117 {
118 Quaternion q = conjugate();
119 return q / dot((*this), (*this));
120 }
121 const Quaternion operator/(t_type s) const
122 {
123 return Quaternion((*this)[X] / s, (*this)[Y] / s, (*this)[Z] / s, (*this)[W] / s);
124 }
125 Quaternion operator*(const Quaternion& q2) const
126 {
127 Quaternion result;
128
129 /*
130 Formula from http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/arithmetic/index.htm
131 a*e - b*f - c*g - d*h
132 + i (b*e + a*f + c*h- d*g)
133 + j (a*g - b*h + c*e + d*f)
134 + k (a*h + b*g - c*f + d*e)
135 */
136 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];
137 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];
138 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];
139 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];
140
141 return result;
142 }
143
144 template<class t_vec_type>
145 Vector<3, t_vec_type> operator*(const Vector<3, t_vec_type>& v) const
146 {
147 Vector<3, t_vec_type> result;
148
149 t_type ww = Vector<4, t_type>::m_values[W] * Vector<4, t_type>::m_values[W];
150 t_type xx = Vector<4, t_type>::m_values[0] * Vector<4, t_type>::m_values[0];
151 t_type yy = Vector<4, t_type>::m_values[1] * Vector<4, t_type>::m_values[1];
152 t_type zz = Vector<4, t_type>::m_values[2] * Vector<4, t_type>::m_values[2];
153 t_type wx = Vector<4, t_type>::m_values[W] * Vector<4, t_type>::m_values[0];
154 t_type wy = Vector<4, t_type>::m_values[W] * Vector<4, t_type>::m_values[1];
155 t_type wz = Vector<4, t_type>::m_values[W] * Vector<4, t_type>::m_values[2];
156 t_type xy = Vector<4, t_type>::m_values[0] * Vector<4, t_type>::m_values[1];
157 t_type xz = Vector<4, t_type>::m_values[0] * Vector<4, t_type>::m_values[2];
158 t_type yz = Vector<4, t_type>::m_values[1] * Vector<4, t_type>::m_values[2];
159
160 // Formula from http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/index.htm
161 // 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;
162 // 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;
163 // 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;
164
165 result[0] = ww * v[0] + 2 * wy * v[2] - 2 * wz * v[1] +
166 xx * v[0] + 2 * xy * v[1] + 2 * xz * v[2] -
167 zz * v[0] - yy * v[0];
168 result[1] = 2 * xy * v[0] + yy * v[1] + 2 * yz * v[2] +
169 2 * wz * v[0] - zz * v[1] + ww * v[1] -
170 2 * wx * v[2] - xx * v[1];
171 result[2] = 2 * xz * v[0] + 2 * yz * v[1] + zz * v[2] -
172 2 * wy * v[0] - yy * v[2] + 2 * wx * v[1] -
173 xx * v[2] + ww * v[2];
174 return result;
175 }
176 /*
177 void Quaternion_slerp(Quaternion* q1, Quaternion* q2, double t, Quaternion* output)
178 {
179 Quaternion result;
180
181 // Based on http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/slerp/index.htm
182 double cosHalfTheta = q1->w * q2->w + q1->v[0] * q2->v[0] + q1->v[1] * q2->v[1] + q1->v[2] * q2->v[2];
183
184 // if q1=q2 or qa=-q2 then theta = 0 and we can return qa
185 if (fabs(cosHalfTheta) >= 1.0) {
186 Quaternion_copy(q1, output);
187 return;
188 }
189
190 double halfTheta = acos(cosHalfTheta);
191 double sinHalfTheta = sqrt(1.0 - cosHalfTheta * cosHalfTheta);
192 // If theta = 180 degrees then result is not fully defined
193 // We could rotate around any axis normal to q1 or q2
194 if (fabs(sinHalfTheta) < QUATERNION_EPS) {
195 result.w = (q1->w * 0.5 + q2->w * 0.5);
196 result.v[0] = (q1->v[0] * 0.5 + q2->v[0] * 0.5);
197 result.v[1] = (q1->v[1] * 0.5 + q2->v[1] * 0.5);
198 result.v[2] = (q1->v[2] * 0.5 + q2->v[2] * 0.5);
199 }
200 else {
201 // Default quaternion calculation
202 double ratioA = sin((1 - t) * halfTheta) / sinHalfTheta;
203 double ratioB = sin(t * halfTheta) / sinHalfTheta;
204 result.w = (q1->w * ratioA + q2->w * ratioB);
205 result.v[0] = (q1->v[0] * ratioA + q2->v[0] * ratioB);
206 result.v[1] = (q1->v[1] * ratioA + q2->v[1] * ratioB);
207 result.v[2] = (q1->v[2] * ratioA + q2->v[2] * ratioB);
208 }
209 *output = result;
210 }*/
211
212
213 inline Matrix<t_type> toMatrix() const
214 {
215 // NOTE: assume the quaternion is unit length
216 // compute common values
217 t_type x2 = (*this)[X] + (*this)[X];
218 t_type y2 = (*this)[Y] + (*this)[Y];
219 t_type z2 = (*this)[Z] + (*this)[Z];
220 t_type xx2 = (*this)[X] * x2;
221 t_type xy2 = (*this)[X] * y2;
222 t_type xz2 = (*this)[X] * z2;
223 t_type yy2 = (*this)[Y] * y2;
224 t_type yz2 = (*this)[Y] * z2;
225 t_type zz2 = (*this)[Z] * z2;
226 t_type sx2 = (*this)[W] * x2;
227 t_type sy2 = (*this)[W] * y2;
228 t_type sz2 = (*this)[W] * z2;
229
230 // build 4x4 matrix (column-major) and return
231 return Matrix<t_type>(
232 1 - (yy2 + zz2), xy2 + sz2, xz2 - sy2, 0,
233 xy2 - sz2, 1 - (xx2 + zz2), yz2 + sx2, 0,
234 xz2 + sy2, yz2 - sx2, 1 - (xx2 + yy2), 0,
235 0, 0, 0, 1);//
236 }
237 };
238 static inline Quaternion<fltp08> difference(const Quaternion<fltp08>& q1, const Quaternion<fltp08>& q2)
239 {
240 return q1.invert() * q2;
241 }
242 static inline Quaternion<fltp04> difference(const Quaternion<fltp04>& q1, const Quaternion<fltp04>& q2)
243 {
244 return q1.invert() * q2;
245 }
255 template<class t_type>
256 struct Constant<Quaternion<t_type>>
257 {
258 constexpr const static Quaternion<t_type> Invalid{ Constant<t_type>::Invalid };
259 constexpr const static Quaternion<t_type> Min{ 0 };
260 constexpr const static Quaternion<t_type> Max{ Constant<t_type>::Max };
261 };
262
279
280 template<class t_type>
281 static constexpr bool IsInvalid(const Quaternion<t_type>& value)
282 {
283 for (uint01 dim = 0; dim < 4; ++dim)
284 {
285 if (IsInvalid(value[dim]))
286 return true;
287 }
288 return false;
289 }
290 template<class t_type>
291 static constexpr bool IsValid(const Quaternion<t_type>& value)
292 {
293 for (uint01 dim = 0; dim < 4; ++dim)
294 {
295 if (IsInvalid(value[dim]))
296 return false;
297 }
298 return true;
299 }
300}
Angle
Stores an angle in an optimized internal format with support for efficient trigonometric operations.
Definition Angle.h:83
Angle::asin
static Angle asin(t_value_type value)
Computes the principal value of the arc sine of the given value.
Definition Angle.h:277
Angle::acos
static Angle acos(t_value_type value)
Computes the principal value of the arc cosine of the given value.
Definition Angle.h:266
Angle::atan2
static Angle atan2(t_value_type x, t_value_type y)
measures the counterclockwise angle between the positive x-axis and the point (x, y)
Definition Angle.h:300
Matrix
Templated logic for doing matrix multiplication.
Definition Matrix.hpp:182
Quaternion
https://www.3dgep.com/understanding-quaternions/
Definition Quaternion.hpp:23
Vector
A fixed-size array with N dimensions used as the basis for geometric and mathematical types.
Definition Vector.hpp:62
Vector::m_values
t_type m_values[t_dims]
The values[t dims].
Definition Vector.hpp:630
Vector< 4, t_type >::as
constexpr decltype(auto) as() const
Definition Vector.hpp:301
NDEVR
The primary namespace for the NDEVR SDK.
Definition ArialTileFetcherModule.h:35
YAW
@ YAW
Rotation about the vertical axis (Z).
Definition Angle.h:47
PITCH
@ PITCH
Rotation about the lateral axis (Y).
Definition Angle.h:46
ROLL
@ ROLL
Rotation about the forward axis (X).
Definition Angle.h:45
IsValid
static constexpr bool IsValid(const Angle< t_type > &value)
Checks whether the given Angle holds a valid value.
Definition Angle.h:398
dot
t_type dot(const Vector< t_dims, t_type > &v1, const Vector< t_dims, t_type > &v2)
Definition VectorFunctions.hpp:1031
abs
constexpr Angle< t_angle_type > abs(const Angle< t_angle_type > &value)
Changes an input with a negative sign, to a positive sign.
Definition AngleFunctions.h:753
DEGREES
@ DEGREES
Angle measured in degrees (0 to 360 for a full circle).
Definition Angle.h:58
uint01
uint8_t uint01
-Defines an alias representing a 1 byte, unsigned integer -Can represent exact integer values 0 throu...
Definition BaseValues.hpp:81
IsInvalid
static constexpr bool IsInvalid(const Angle< t_type > &value)
Checks whether the given Angle holds an invalid value.
Definition Angle.h:388
difference
static constexpr Angle< t_type > difference(const Angle< t_type > &angle_a, const Angle< t_type > &angle_b)
Calculates minimal absolute signed angle between two angles.
Definition AngleFunctions.h:212
sqrt
t_type sqrt(const t_type &value)
Definition VectorFunctions.hpp:1181
sin
std::enable_if<!ObjectInfo< t_type >::Float, fltp08 >::type sin(const Angle< t_type > &angle)
Performs optimized sine operation on the given angle using pre-computed lookup table for optimal spee...
Definition AngleFunctions.h:79
cos
std::enable_if<!ObjectInfo< t_type >::Float, fltp08 >::type cos(const Angle< t_type > &angle)
Performs optimized cosine operation on the given angle using pre-computed lookup table for optimal sp...
Definition AngleFunctions.h:124
cast
constexpr t_to cast(const Angle< t_from > &value)
Casts an Angle from one backing type to another.
Definition Angle.h:408
AxisAngle
An angle about a particular, defined 3D axis.
Definition Quaternion.hpp:14
Constant
Defines for a given type (such as sint04, fltp08, UUID, etc) a maximum, minimum, and reserved 'invali...
Definition BaseValues.hpp:261