suanPan
Quaternion.hpp
Go to the documentation of this file.
1/*******************************************************************************
2 * Copyright (C) 2017-2024 Theodore Chang
3 *
4 * This program is free software: you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation, either version 3 of the License, or
7 * (at your option) any later version.
8 *
9 * This program is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
13 *
14 * You should have received a copy of the GNU General Public License
15 * along with this program. If not, see <http://www.gnu.org/licenses/>.
16 ******************************************************************************/
29#ifndef QUATERNION_H
30#define QUATERNION_H
31
32#include <Toolbox/tensor.h>
33
34template<typename T> class Quaternion {
35 T re;
36 Col<T> im;
37
38public:
39 Quaternion();
40 Quaternion(T, T, T, T);
41 Quaternion(T, const Col<T>&);
42 Quaternion(T, Col<T>&&);
43
44 const T& real() const;
45 const Col<T>& imag() const;
46
47 T norm() const;
49
50 [[nodiscard]] Quaternion inv() const;
51 [[nodiscard]] Quaternion conj() const;
52
53 Quaternion operator+(const Quaternion&) const;
55 Quaternion operator-(const Quaternion&) const;
57 Quaternion operator*(const Quaternion&) const;
59 Quaternion operator/(const Quaternion&) const;
61
62 void print() const;
63
64 friend bool operator==(const Quaternion& A, const Quaternion& B) {
65 constexpr int ulp = 10000;
66 if(suanpan::approx_equal(A.re, B.re, ulp) && suanpan::approx_equal(A.im(0), B.im(0), ulp) && suanpan::approx_equal(A.im(1), B.im(1), ulp) && suanpan::approx_equal(A.im(2), B.im(2), ulp)) return true;
67
68 if(suanpan::approx_equal(A.re, -B.re, ulp) && suanpan::approx_equal(A.im(0), -B.im(0), ulp) && suanpan::approx_equal(A.im(1), -B.im(1), ulp) && suanpan::approx_equal(A.im(2), -B.im(2), ulp)) return true;
69
70 return false;
71 }
72
73 friend Quaternion operator-(const Quaternion& A) { return Quaternion(-A.re, -A.im); }
74
76 A.re = -A.re;
77 A.im *= -1.;
78 return std::forward<Quaternion>(A);
79 }
80
81 Mat<T> operator*(const Mat<T>&) const;
82
83 Mat<T> to_mat() const;
84 Col<T> to_pseudo() const;
85};
86
87template<typename T> Quaternion<T>::Quaternion()
88 : re(T(0))
89 , im(arma::zeros<Col<T>>(3)) {}
90
91template<typename T> Quaternion<T>::Quaternion(const T R, const T I, const T J, const T K)
92 : re(R)
93 , im({I, J, K}) {}
94
95template<typename T> Quaternion<T>::Quaternion(const T R, const Col<T>& I)
96 : re(R)
97 , im(I) {}
98
99template<typename T> Quaternion<T>::Quaternion(const T R, Col<T>&& I)
100 : re(R)
101 , im(std::move(I)) {}
102
103template<typename T> const T& Quaternion<T>::real() const { return re; }
104
105template<typename T> const Col<T>& Quaternion<T>::imag() const { return im; }
106
107template<typename T> T Quaternion<T>::norm() const { return re * re + arma::dot(im, im); }
108
110 const auto magnitude = std::sqrt(norm());
111 re /= magnitude;
112 im /= magnitude;
113 return *this;
114}
115
116template<typename T> Quaternion<T> Quaternion<T>::inv() const {
117 const auto L = norm();
118
119 return Quaternion<T>(re / L, -im / L);
120}
121
122template<typename T> Quaternion<T> Quaternion<T>::conj() const { return Quaternion<T>(re, -im); }
123
124template<typename T> Quaternion<T> Quaternion<T>::operator+(const Quaternion& B) const {
125 Quaternion<T> A = *this;
126
127 return A += B;
128}
129
130template<typename T> Quaternion<T>& Quaternion<T>::operator+=(const Quaternion& B) {
131 re += B.re;
132 im += B.im;
133
134 return *this;
135}
136
137template<typename T> Quaternion<T> Quaternion<T>::operator-(const Quaternion& B) const {
138 Quaternion<T> A = *this;
139
140 return A -= B;
141}
142
143template<typename T> Quaternion<T>& Quaternion<T>::operator-=(const Quaternion& B) {
144 re -= B.re;
145 im -= B.im;
146
147 return *this;
148}
149
150template<typename T> Quaternion<T> Quaternion<T>::operator*(const Quaternion& B) const {
152
153 A.re = re * B.re - arma::dot(im, B.im);
154 A.im = re * B.im + B.re * im + arma::cross(im, B.im);
155
156 return A;
157}
158
159template<typename T> Quaternion<T>& Quaternion<T>::operator*=(const Quaternion& B) { return *this = *this * B; }
160
161template<typename T> Quaternion<T> Quaternion<T>::operator/(const Quaternion& B) const { return *this * B.inv(); }
162
163template<typename T> Quaternion<T>& Quaternion<T>::operator/=(const Quaternion& B) { return *this = *this * B.inv(); }
164
165template<typename T> void Quaternion<T>::print() const {
166 suanpan_info("re: {:+0.6E} im: {:+0.6E} {:+0.6E} {:+0.6E}\n", re, im(0), im(1), im(2));
167}
168
169template<typename T> Mat<T> Quaternion<T>::operator*(const Mat<T>& I) const { return to_mat() * I; }
170
171template<typename T> Mat<T> Quaternion<T>::to_mat() const { return 2. * re * transform::skew_symm(im) + 2. * im * im.t() + (re * re - arma::dot(im, im)) * eye(3, 3); }
172
173template<typename T> Col<T> Quaternion<T>::to_pseudo() const {
174 const auto norm_im = arma::norm(im);
175
176 if(suanpan::approx_equal(norm_im, 0., 1000)) return Col<T>(3, fill::zeros);
177
178 vec rotation = re < 0. ? -im : im;
179
180 rotation *= 2. / norm_im * (norm_im < std::abs(re) ? std::asin(norm_im) : std::acos(std::abs(re)));
181
182 return rotation;
183}
184
185#endif
186
An Quaternion class.
Definition: Quaternion.hpp:34
friend bool operator==(const Quaternion &A, const Quaternion &B)
Definition: Quaternion.hpp:64
friend Quaternion operator-(Quaternion &&A)
Definition: Quaternion.hpp:75
friend Quaternion operator-(const Quaternion &A)
Definition: Quaternion.hpp:73
Mat< T > to_mat(const MetaMat< T > &in_mat)
Definition: MetaMat.hpp:266
Quaternion inv() const
Definition: Quaternion.hpp:116
Quaternion operator-(const Quaternion &) const
Definition: Quaternion.hpp:137
Quaternion operator/(const Quaternion &) const
Definition: Quaternion.hpp:161
Quaternion operator*(const Quaternion &) const
Definition: Quaternion.hpp:150
Quaternion & normalise()
Definition: Quaternion.hpp:109
Quaternion & operator-=(const Quaternion &)
Definition: Quaternion.hpp:143
T norm() const
Definition: Quaternion.hpp:107
Col< T > to_pseudo() const
Definition: Quaternion.hpp:173
Quaternion operator+(const Quaternion &) const
Definition: Quaternion.hpp:124
Quaternion & operator*=(const Quaternion &)
Definition: Quaternion.hpp:159
const Col< T > & imag() const
Definition: Quaternion.hpp:105
Quaternion & operator/=(const Quaternion &)
Definition: Quaternion.hpp:163
Quaternion & operator+=(const Quaternion &)
Definition: Quaternion.hpp:130
const T & real() const
Definition: Quaternion.hpp:103
Quaternion()
Definition: Quaternion.hpp:87
Quaternion conj() const
Definition: Quaternion.hpp:122
Mat< T > to_mat() const
Definition: Quaternion.hpp:171
void print() const
Definition: Quaternion.hpp:165
std::enable_if_t<!std::numeric_limits< T >::is_integer, bool > approx_equal(T x, T y, int ulp=2)
Definition: utility.h:60
double norm(const vec &)
Definition: tensor.cpp:370
Mat< T > skew_symm(const Mat< T > &R)
Definition: tensor.h:132
#define suanpan_info
Definition: suanPan.h:305