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