35 #include "quaternion.h"
37 #include "rotmatrix.h"
45 static_assert(std::is_standard_layout<Quaternion>::value,
"Quaternion should be standard layout.");
46 static_assert(std::is_trivially_copyable<Quaternion>::value,
"Quaternion should be trivially copyable.");
53 : m_w(0), m_vec(), m_valid(true), m_age(1)
55 CoordType norm = std::sqrt(w_in*w_in + x_in*x_in + y_in*y_in + z_in*z_in);
58 m_vec[0] = x_in / norm;
59 m_vec[1] = y_in / norm;
60 m_vec[2] = z_in / norm;
83 if (!q.m_valid || !m_valid) {
87 if(std::fabs(m_w - q.m_w) <= epsilon) {
89 for(i = 0; i < 3; ++i)
90 if(std::fabs(m_vec[i] - q.m_vec[i]) > epsilon)
97 if(std::fabs(m_w + q.m_w) <= epsilon) {
98 for(
int i = 0; i < 3; ++i)
99 if(std::fabs(m_vec[i] + q.m_vec[i]) > epsilon)
112 Quaternion& Quaternion::operator*= (
const Quaternion& rhs)
114 m_valid = m_valid && rhs.m_valid;
115 m_age = m_age + rhs.m_age;
116 checkNormalization();
119 m_w = m_w * rhs.m_w - Dot(m_vec, rhs.m_vec);
120 m_vec = old_w * rhs.m_vec + rhs.m_w * m_vec -
Cross(m_vec, rhs.m_vec);
125 Quaternion& Quaternion::operator/= (
const Quaternion& rhs)
127 m_valid = m_valid && rhs.m_valid;
128 m_age = m_age + rhs.m_age;
129 checkNormalization();
132 m_w = m_w * rhs.m_w + Dot(m_vec, rhs.m_vec);
133 m_vec = rhs.m_w * m_vec - old_w * rhs.m_vec +
Cross(m_vec, rhs.m_vec);
141 bool not_flip = !m.
parity();
143 m_valid = m.isValid();
152 const int nxt[3] = {1, 2, 0};
157 s = std::sqrt(tr + 1.0f);
161 m_vec[0] = (m_ref.
elem(2, 1) - m_ref.
elem(1, 2)) * s;
162 m_vec[1] = (m_ref.
elem(0, 2) - m_ref.
elem(2, 0)) * s;
163 m_vec[2] = (m_ref.
elem(1, 0) - m_ref.
elem(0, 1)) * s;
168 if (m_ref.
elem(1, 1) > m_ref.
elem(0, 0)) i = 1;
169 if (m_ref.
elem(2, 2) > m_ref.
elem(i, i)) i = 2;
171 int j = nxt[i], k = nxt[j];
173 s = std::sqrt (1.0f + m_ref.
elem(i, i) - m_ref.
elem(j, j) - m_ref.
elem(k, k));
174 m_vec[i] = -(s * 0.5f);
176 assert(
"sqrt() returns positive" && s > 0.0);
179 m_w = (m_ref.
elem(k, j) - m_ref.
elem(j, k)) * s;
180 m_vec[j] = (m_ref.
elem(i, j) + m_ref.
elem(j, i)) * s;
181 m_vec[k] = (m_ref.
elem(i, k) + m_ref.
elem(k, i)) * s;
209 if (axis < 0 || axis > 2) {
216 m_w = std::cos(half_angle);
217 for(
int i = 0; i < 3; ++i)
219 m_vec[i] = (i == axis) ? std::sin(half_angle) : 0;
238 m_w = std::cos(half_angle);
239 m_vec = axis * (std::sin(half_angle) / axis_mag);
241 m_valid = axis.isValid();
257 m_w = std::cos(half_angle);
258 m_vec = axis * (std::sin(half_angle) / axis_mag);
260 m_valid = axis.isValid();
269 CoordType ctheta_plus_1 = Dot(from, to) / mag_prod + 1;
281 m_w = std::sqrt(ctheta_plus_1 / 2.f);
286 m_vec =
Cross(from, to) / (2 * mag_prod * m_w);
288 m_valid = from.isValid() && to.isValid();
297 CoordType ctheta_plus_1 = Dot(from, to) / mag_prod + 1;
309 m_w = std::sqrt(ctheta_plus_1 / 2.f);
314 m_vec =
Cross(from, to) / (2 * mag_prod * m_w);
315 m_valid = from.isValid() && to.isValid();