35#include "quaternion.h"
45static_assert(std::is_standard_layout<Quaternion>::value,
"Quaternion should be standard layout.");
46static_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)
112Quaternion& 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);
125Quaternion& 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();
Quaternion inverse() const
returns the inverse of the Quaternion
void normalize()
normalize to remove accumulated round-off error
bool fromRotMatrix(const RotMatrix< 3 > &m)
set a Quaternion's value from a RotMatrix
Quaternion & rotate(const RotMatrix< 3 > &)
Rotate quaternion using the matrix.
static const Quaternion & IDENTITY()
Quaternion & rotation(int axis, CoordType angle)
sets the Quaternion to a rotation by angle around axis
Quaternion()
Construct a Quaternion.
bool parity() const
Get the parity of the matrix.
CoordType trace() const
Get the trace of the matrix.
CoordType elem(const int i, const int j) const
get the (i, j) element of the matrix
unsigned age() const
current round-off age
void setValid(bool valid=true)
make isValid() return true if you've initialized the vector by hand
CoordType mag() const
The magnitude of a vector.
CoordType sqrMag() const
The squared magnitude of a vector.
Generic library namespace.
double CoordType
Basic floating point type.
CoordType Cross(const Vector< 2 > &v1, const Vector< 2 > &v2)
2D only: get the z component of the cross product of two vectors
RotMatrix< dim > Prod(const RotMatrix< dim > &m1, const RotMatrix< dim > &m2)
returns m1 * m2
An error thrown by certain functions when passed parallel vectors.