tesseract++ 0.0.1
N-dimensional tensor library for embedded systems
Loading...
Searching...
No Matches
fused_quaternion.h
Go to the documentation of this file.
1// #ifndef FUSEDQUATERNION_H
2// #define FUSEDQUATERNION_H
3
4// #include "fused/fused_vector.h"
5// #include "fused/BaseExpr.h"
6// #include "fused/microkernels/microkernel_base.h"
7// #include "config.h"
8// #include <cmath>
9
10// /**
11// * @brief Quaternion class using expression templates for SIMD optimization
12// *
13// * Storage order: [w, x, y, z] where w is scalar part, (x,y,z) is vector part
14// * Follows Hamilton convention: i² = j² = k² = ijk = -1
15// */
16// template <typename T>
17// class FusedQuaternion : public BaseExpr<FusedQuaternion<T>, T>
18// {
19// public:
20// // Compile-time constants for expression template compatibility
21// static constexpr my_size_t NumDims = 1;
22// static constexpr my_size_t Dim[] = {4};
23// static constexpr my_size_t TotalSize = 4;
24
25// using Self = FusedQuaternion<T>;
26// using value_type = T;
27// using VecType = typename Microkernel<T, BITS, DefaultArch>::VecType;
28// using microkernel = Microkernel<T, BITS, DefaultArch>;
29// static constexpr my_size_t simdWidth = microkernel::simdWidth;
30
31// // ========================
32// // Constructors
33// // ========================
34
35// FusedQuaternion() noexcept
36// : data_()
37// {
38// // Initialize to identity quaternion
39// data_(0) = T{1}; // w
40// data_(1) = T{0}; // x
41// data_(2) = T{0}; // y
42// data_(3) = T{0}; // z
43// }
44
45// FusedQuaternion(T w, T x, T y, T z) noexcept
46// : data_()
47// {
48// data_(0) = w;
49// data_(1) = x;
50// data_(2) = y;
51// data_(3) = z;
52// }
53
54// explicit FusedQuaternion(T initValue) noexcept
55// : data_(initValue) {}
56
57// // Construct from scalar and vector part
58// template <my_size_t N>
59// FusedQuaternion(T scalar, const FusedTensorND<T, N> &vec) noexcept
60// {
61// static_assert(N == 3, "Vector part must be 3D");
62// data_(0) = scalar;
63// data_(1) = vec(0);
64// data_(2) = vec(1);
65// data_(3) = vec(2);
66// }
67
68// // Copy constructor
69// FusedQuaternion(const FusedQuaternion &other) noexcept
70// : data_(other.data_) {}
71
72// // Move constructor
73// FusedQuaternion(FusedQuaternion &&other) noexcept
74// : data_(move(other.data_)) {}
75
76// // ========================
77// // Assignment from expressions (enables SIMD fusion)
78// // ========================
79
80// template <typename Expr>
81// FusedQuaternion &operator=(const BaseExpr<Expr, T> &expr)
82// {
83// const auto &e = expr.derived();
84
85// // Compile-time dimension check
86// static_assert(Expr::TotalSize == 4, "Expression must have 4 elements for quaternion assignment");
87
88// if constexpr (!is_same_v<DefaultArch, GenericArch> && simdWidth >= 4)
89// {
90// // Single SIMD load for all 4 components
91// auto vec = e.evalu(0);
92// microkernel::store(data_.data(), vec);
93// }
94// else
95// {
96// // Scalar fallback
97// my_size_t indices[1];
98// for (my_size_t i = 0; i < 4; ++i)
99// {
100// indices[0] = i;
101// data_(i) = e(indices);
102// }
103// }
104// return *this;
105// }
106
107// FusedQuaternion &operator=(const FusedQuaternion &other) noexcept
108// {
109// if (this != &other)
110// {
111// data_ = other.data_;
112// }
113// return *this;
114// }
115
116// FusedQuaternion &operator=(FusedQuaternion &&other) noexcept
117// {
118// if (this != &other)
119// {
120// data_ = move(other.data_);
121// }
122// return *this;
123// }
124
125// // ========================
126// // Expression template interface
127// // ========================
128
129// // SIMD evaluation - loads all 4 components at once if possible
130// VecType evalu(my_size_t flat) const noexcept
131// {
132// return microkernel::load(data_.data() + flat);
133// }
134
135// // Scalar access for expression evaluation
136// inline T operator()(const my_size_t *indices) const noexcept
137// {
138// return data_(indices[0]);
139// }
140
141// inline T &operator()(const my_size_t *indices) noexcept
142// {
143// return data_(indices[0]);
144// }
145
146// // ========================
147// // Component accessors
148// // ========================
149
150// FORCE_INLINE T &w() noexcept { return data_(0); }
151// FORCE_INLINE T &x() noexcept { return data_(1); }
152// FORCE_INLINE T &y() noexcept { return data_(2); }
153// FORCE_INLINE T &z() noexcept { return data_(3); }
154
155// FORCE_INLINE const T &w() const noexcept { return data_(0); }
156// FORCE_INLINE const T &x() const noexcept { return data_(1); }
157// FORCE_INLINE const T &y() const noexcept { return data_(2); }
158// FORCE_INLINE const T &z() const noexcept { return data_(3); }
159
160// // Index access
161// FORCE_INLINE T &operator[](my_size_t i) noexcept { return data_(i); }
162// FORCE_INLINE const T &operator[](my_size_t i) const noexcept { return data_(i); }
163
164// // Raw data pointer (for SIMD operations)
165// FORCE_INLINE T *data() noexcept { return data_.data(); }
166// FORCE_INLINE const T *data() const noexcept { return data_.data(); }
167
168// // ========================
169// // Quaternion-specific operations
170// // ========================
171
172// // Conjugate: q* = (w, -x, -y, -z)
173// FusedQuaternion conjugate() const noexcept
174// {
175// return FusedQuaternion(w(), -x(), -y(), -z());
176// }
177
178// // Squared norm: |q|² = w² + x² + y² + z²
179// T normSquared() const noexcept
180// {
181// if constexpr (!is_same_v<DefaultArch, GenericArch> && simdWidth >= 4)
182// {
183// // SIMD dot product
184// VecType v = microkernel::load(data_.data());
185// VecType sq = microkernel::mul(v, v);
186// return microkernel::horizontal_sum(sq);
187// }
188// else
189// {
190// return w() * w() + x() * x() + y() * y() + z() * z();
191// }
192// }
193
194// // Norm: |q| = sqrt(w² + x² + y² + z²)
195// T norm() const noexcept
196// {
197// return std::sqrt(normSquared());
198// }
199
200// // Normalize in-place
201// FusedQuaternion &normalize()
202// {
203// T n = norm();
204// if (n > PRECISION_TOLERANCE)
205// {
206// T invNorm = T{1} / n;
207// if constexpr (!is_same_v<DefaultArch, GenericArch> && simdWidth >= 4)
208// {
209// VecType v = microkernel::load(data_.data());
210// VecType scale = microkernel::broadcast(invNorm);
211// microkernel::store(data_.data(), microkernel::mul(v, scale));
212// }
213// else
214// {
215// data_(0) *= invNorm;
216// data_(1) *= invNorm;
217// data_(2) *= invNorm;
218// data_(3) *= invNorm;
219// }
220// }
221// return *this;
222// }
223
224// // Return normalized copy
225// FusedQuaternion normalized() const
226// {
227// FusedQuaternion result(*this);
228// result.normalize();
229// return result;
230// }
231
232// // Inverse: q⁻¹ = q* / |q|²
233// FusedQuaternion inverse() const
234// {
235// T ns = normSquared();
236// if (ns < PRECISION_TOLERANCE)
237// {
238// MyErrorHandler::error("Cannot invert quaternion with zero norm");
239// }
240// T invNormSq = T{1} / ns;
241// return FusedQuaternion(
242// w() * invNormSq,
243// -x() * invNormSq,
244// -y() * invNormSq,
245// -z() * invNormSq);
246// }
247
248// // ========================
249// // Hamilton product: p * q
250// // ========================
251
252// FusedQuaternion operator*(const FusedQuaternion &q) const noexcept
253// {
254// // Hamilton product formula:
255// // (p.w * q.w - p.x * q.x - p.y * q.y - p.z * q.z,
256// // p.w * q.x + p.x * q.w + p.y * q.z - p.z * q.y,
257// // p.w * q.y - p.x * q.z + p.y * q.w + p.z * q.x,
258// // p.w * q.z + p.x * q.y - p.y * q.x + p.z * q.w)
259
260// const T pw = w(), px = x(), py = y(), pz = z();
261// const T qw = q.w(), qx = q.x(), qy = q.y(), qz = q.z();
262
263// return FusedQuaternion(
264// pw * qw - px * qx - py * qy - pz * qz, // w
265// pw * qx + px * qw + py * qz - pz * qy, // x
266// pw * qy - px * qz + py * qw + pz * qx, // y
267// pw * qz + px * qy - py * qx + pz * qw // z
268// );
269// }
270
271// FusedQuaternion &operator*=(const FusedQuaternion &q) noexcept
272// {
273// *this = *this * q;
274// return *this;
275// }
276
277// // ========================
278// // Vector rotation: v' = q * v * q⁻¹
279// // ========================
280
281// template <my_size_t N>
282// FusedTensorND<T, 3> rotate(const FusedTensorND<T, N> &v) const
283// {
284// static_assert(N == 3, "Can only rotate 3D vectors");
285
286// // Optimized rotation formula (avoiding full quaternion multiplication):
287// // v' = v + 2w(u × v) + 2(u × (u × v))
288// // where u = (x, y, z) is the vector part of the quaternion
289
290// const T vx = v(0), vy = v(1), vz = v(2);
291// const T ux = x(), uy = y(), uz = z();
292
293// // t = 2 * (u × v)
294// T tx = T{2} * (uy * vz - uz * vy);
295// T ty = T{2} * (uz * vx - ux * vz);
296// T tz = T{2} * (ux * vy - uy * vx);
297
298// // v' = v + w*t + (u × t)
299// FusedTensorND<T, 3> result;
300// result(0) = vx + w() * tx + (uy * tz - uz * ty);
301// result(1) = vy + w() * ty + (uz * tx - ux * tz);
302// result(2) = vz + w() * tz + (ux * ty - uy * tx);
303
304// return result;
305// }
306
307// // ========================
308// // Static factory methods
309// // ========================
310
311// static FusedQuaternion identity() noexcept
312// {
313// return FusedQuaternion(T{1}, T{0}, T{0}, T{0});
314// }
315
316// // Create from axis-angle representation
317// template <my_size_t N>
318// static FusedQuaternion fromAxisAngle(const FusedTensorND<T, N> &axis, T angle)
319// {
320// static_assert(N == 3, "Axis must be 3D");
321
322// T halfAngle = angle * T{0.5};
323// T s = std::sin(halfAngle);
324// T c = std::cos(halfAngle);
325
326// // Normalize axis
327// T axisNorm = std::sqrt(axis(0) * axis(0) + axis(1) * axis(1) + axis(2) * axis(2));
328// if (axisNorm < PRECISION_TOLERANCE)
329// {
330// return identity();
331// }
332// T invNorm = T{1} / axisNorm;
333
334// return FusedQuaternion(
335// c,
336// axis(0) * invNorm * s,
337// axis(1) * invNorm * s,
338// axis(2) * invNorm * s);
339// }
340
341// // Create from Euler angles (ZYX convention: yaw, pitch, roll)
342// static FusedQuaternion fromEulerAngles(T roll, T pitch, T yaw)
343// {
344// T cr = std::cos(roll * T{0.5});
345// T sr = std::sin(roll * T{0.5});
346// T cp = std::cos(pitch * T{0.5});
347// T sp = std::sin(pitch * T{0.5});
348// T cy = std::cos(yaw * T{0.5});
349// T sy = std::sin(yaw * T{0.5});
350
351// return FusedQuaternion(
352// cr * cp * cy + sr * sp * sy, // w
353// sr * cp * cy - cr * sp * sy, // x
354// cr * sp * cy + sr * cp * sy, // y
355// cr * cp * sy - sr * sp * cy // z
356// );
357// }
358
359// // Convert to Euler angles (returns roll, pitch, yaw)
360// FusedTensorND<T, 3> toEulerAngles() const
361// {
362// FusedTensorND<T, 3> angles;
363
364// // Roll (x-axis rotation)
365// T sinr_cosp = T{2} * (w() * x() + y() * z());
366// T cosr_cosp = T{1} - T{2} * (x() * x() + y() * y());
367// angles(0) = std::atan2(sinr_cosp, cosr_cosp);
368
369// // Pitch (y-axis rotation)
370// T sinp = T{2} * (w() * y() - z() * x());
371// if (std::abs(sinp) >= T{1})
372// angles(1) = std::copysign(T{3.14159265358979323846} / T{2}, sinp);
373// else
374// angles(1) = std::asin(sinp);
375
376// // Yaw (z-axis rotation)
377// T siny_cosp = T{2} * (w() * z() + x() * y());
378// T cosy_cosp = T{1} - T{2} * (y() * y() + z() * z());
379// angles(2) = std::atan2(siny_cosp, cosy_cosp);
380
381// return angles;
382// }
383
384// // Spherical linear interpolation (SLERP)
385// static FusedQuaternion slerp(const FusedQuaternion &q0, const FusedQuaternion &q1, T t)
386// {
387// // Compute dot product
388// T dot = q0.w() * q1.w() + q0.x() * q1.x() + q0.y() * q1.y() + q0.z() * q1.z();
389
390// FusedQuaternion q1_adj = q1;
391
392// // If dot < 0, negate one quaternion to take shorter path
393// if (dot < T{0})
394// {
395// q1_adj = FusedQuaternion(-q1.w(), -q1.x(), -q1.y(), -q1.z());
396// dot = -dot;
397// }
398
399// // If quaternions are very close, use linear interpolation
400// if (dot > T{0.9995})
401// {
402// FusedQuaternion result(
403// q0.w() + t * (q1_adj.w() - q0.w()),
404// q0.x() + t * (q1_adj.x() - q0.x()),
405// q0.y() + t * (q1_adj.y() - q0.y()),
406// q0.z() + t * (q1_adj.z() - q0.z()));
407// return result.normalized();
408// }
409
410// T theta_0 = std::acos(dot);
411// T theta = theta_0 * t;
412// T sin_theta = std::sin(theta);
413// T sin_theta_0 = std::sin(theta_0);
414
415// T s0 = std::cos(theta) - dot * sin_theta / sin_theta_0;
416// T s1 = sin_theta / sin_theta_0;
417
418// return FusedQuaternion(
419// s0 * q0.w() + s1 * q1_adj.w(),
420// s0 * q0.x() + s1 * q1_adj.x(),
421// s0 * q0.y() + s1 * q1_adj.y(),
422// s0 * q0.z() + s1 * q1_adj.z());
423// }
424
425// // ========================
426// // Utility functions
427// // ========================
428
429// bool isUnit(T tolerance = PRECISION_TOLERANCE) const noexcept
430// {
431// return std::abs(normSquared() - T{1}) < tolerance;
432// }
433
434// FORCE_INLINE constexpr my_size_t getTotalSize() const noexcept
435// {
436// return TotalSize;
437// }
438
439// FORCE_INLINE constexpr my_size_t getNumDims() const noexcept
440// {
441// return NumDims;
442// }
443
444// FORCE_INLINE my_size_t getDim(my_size_t i) const noexcept
445// {
446// return (i == 0) ? 4 : 0;
447// }
448
449// void print() const
450// {
451// MyErrorHandler::log("Quaternion(w=");
452// MyErrorHandler::log(w());
453// MyErrorHandler::log(", x=");
454// MyErrorHandler::log(x());
455// MyErrorHandler::log(", y=");
456// MyErrorHandler::log(y());
457// MyErrorHandler::log(", z=");
458// MyErrorHandler::log(z());
459// MyErrorHandler::log(")\n");
460// }
461
462// private:
463// // Storage: 4-element vector [w, x, y, z]
464// FusedTensorND<T, 4> data_;
465// };
466
467// // ========================
468// // Type alias for common types
469// // ========================
470// using Quaternionf = FusedQuaternion<float>;
471// using Quaterniond = FusedQuaternion<double>;
472
473// #endif // FUSEDQUATERNION_H
Generated by doxygen 1.9.8