10#ifndef MATH_QUATERNION_HEADER
11#define MATH_QUATERNION_HEADER
18template <
typename T>
class Quaternion;
40 Quaternion (T
const& v1, T
const& v2, T
const& v3, T
const& v4);
49 void set_from_rotation_matrix (T
const* matrix);
52 void get_axis_angle (T* axis, T& angle);
55 void to_rotation_matrix (T* matrix)
const;
123 this->set(axis, angle);
130 this->v[0] = std::cos(angle / T(2));
131 T sa = std::sin(angle / T(2));
132 this->v[1] = axis[0] * sa;
133 this->v[2] = axis[1] * sa;
134 this->v[3] = axis[2] * sa;
141 float trace = rot[0] + rot[4] + rot[8];
144 float s = T(0.5) / std::sqrt(trace + T(1.0));
146 this->v[0] = T(0.25) / s;
147 this->v[1] = (rot[7] - rot[5]) * s;
148 this->v[2] = (rot[2] - rot[6]) * s;
149 this->v[3] = (rot[3] - rot[1]) * s;
153 if (rot[0] > rot[4] && rot[0] > rot[8])
155 float s = T(2.0) * std::sqrt(T(1.0) + rot[0] - rot[4] - rot[8]);
156 this->v[0] = (rot[7] - rot[5]) / s;
157 this->v[1] = T(0.25) * s;
158 this->v[2] = (rot[1] + rot[3]) / s;
159 this->v[3] = (rot[2] + rot[6]) / s;
161 else if (rot[4] > rot[8])
163 float s = T(2.0) * std::sqrt(T(1.0) + rot[4] - rot[0] - rot[8]);
164 this->v[0] = (rot[2] - rot[6]) / s;
165 this->v[1] = (rot[1] + rot[3]) / s;
166 this->v[2] = T(0.25) * s;
167 this->v[3] = (rot[5] + rot[7]) / s;
171 float s = T(2.0) * std::sqrt(T(1.0) + rot[8] - rot[0] - rot[4]);
172 this->v[0] = (rot[3] - rot[1]) / s;
173 this->v[1] = (rot[2] + rot[6]) / s;
174 this->v[2] = (rot[5] + rot[7]) / s;
175 this->v[3] = T(0.25) * s;
184 const T len = std::sqrt(
MATH_POW2(this->v[1])
195 axis[0] = this->v[1] / len;
196 axis[1] = this->v[2] / len;
197 axis[2] = this->v[3] / len;
198 angle = T(2) * std::acos(this->v[0]);
205 T
const xxzz = this->v[1] * this->v[1] - this->v[3] * this->v[3];
206 T
const rryy = this->v[0] * this->v[0] - this->v[2] * this->v[2];
207 T
const yyrrxxzz = this->v[2] * this->v[2] + this->v[0] * this->v[0]
208 - this->v[1] * this->v[1] - this->v[3] * this->v[3];
210 T
const xr2 = this->v[1] * this->v[0] * T(2);
211 T
const xy2 = this->v[1] * this->v[2] * T(2);
212 T
const xz2 = this->v[1] * this->v[3] * T(2);
213 T
const yr2 = this->v[2] * this->v[0] * T(2);
214 T
const yz2 = this->v[2] * this->v[3] * T(2);
215 T
const zr2 = this->v[3] * this->v[0] * T(2);
217 matrix[0] = xxzz + rryy;
218 matrix[1] = xy2 - zr2;
219 matrix[2] = xz2 + yr2;
220 matrix[3] = xy2 + zr2;
221 matrix[4] = yyrrxxzz;
222 matrix[5] = yz2 - xr2;
223 matrix[6] = xz2 - yr2;
224 matrix[7] = yz2 + xr2;
225 matrix[8] = rryy - xxzz;
233 this->to_rotation_matrix(rot);
236 ret[0] = rot[0] * vec[0] + rot[1] * vec[1] + rot[2] * vec[2];
237 ret[1] = rot[3] * vec[0] + rot[4] * vec[1] + rot[5] * vec[2];
238 ret[2] = rot[6] * vec[0] + rot[7] * vec[1] + rot[8] * vec[2];
247 this->v[0] * rhs.
v[0] - this->v[1] * rhs.
v[1]
248 - this->v[2] * rhs.
v[2] - this->v[3] * rhs.
v[3],
249 this->v[0] * rhs.
v[1] + this->v[1] * rhs.
v[0]
250 + this->v[2] * rhs.
v[3] - this->v[3] * rhs.
v[2],
251 this->v[0] * rhs.
v[2] - this->v[1] * rhs.
v[3]
252 + this->v[2] * rhs.
v[0] + this->v[3] * rhs.
v[1],
253 this->v[0] * rhs.
v[3] + this->v[1] * rhs.
v[2]
254 - this->v[2] * rhs.
v[1] + this->v[3] * rhs.
v[0]);
269 this->v[1] = -this->v[1];
270 this->v[2] = -this->v[2];
271 this->v[3] = -this->v[3];
291template <
typename T,
int N>
295 for (
int i = 0; i < 4; ++i)
296 os << v[i] << ( i < 3 ?
" " :
"");
Quaternion class for arbitrary types (WORK IN PROGRESS).
Quaternion< T > & operator*=(Quaternion< T > const &rhs)
Quaternion self-multiplication.
Quaternion< T > & conjugate(void)
Conjugates self and returns reference to self.
Quaternion(void)
Creates a new, uninitialized quaternion.
Quaternion< T > conjugated(void) const
Returns a conjugated copy of self.
void to_rotation_matrix(T *matrix) const
Conversion to a 3x3 rotation matrix.
void set_from_rotation_matrix(T const *matrix)
Sets the quaternion from an orthonormal rotation matrix.
void set(Vector< T, 3 > const &axis, T const &angle)
Sets the quaternion from axis and angle.
Vector< T, 3 > rotate(Vector< T, 3 > const &v) const
Rotates a vector using the quaternion.
void get_axis_angle(T *axis, T &angle)
Provides the axis and angle of the quaternion.
Vector class for arbitrary dimensions and types.
T * operator*(void)
Dereference operator to value array.
#define MATH_NAMESPACE_BEGIN
#define MATH_NAMESPACE_END
std::ostream & operator<<(std::ostream &os, Matrix< T, N, M > const &m)
Serializing a vector to an output stream.