52 #ifndef ACG_QUATERNION_HH 53 #define ACG_QUATERNION_HH 59 #include "Matrix4x4T.hh" 74 template <
class Scalar>
79 #define W Base::data()[0] 80 #define X Base::data()[1] 81 #define Y Base::data()[2] 82 #define Z Base::data()[3] 93 QuaternionT(Scalar _w=1.0, Scalar _x=0.0, Scalar _y=0.0, Scalar _z=0.0)
94 : Vec4(_w, _x, _y, _z) {}
98 : Vec4(0, _p[0], _p[1], _p[2]) {}
102 : Vec4(_p[0], _p[1], _p[2], _p[3]) {}
108 Scalar theta = 0.5 * _angle;
109 Scalar sin_theta = sin(theta);
111 X = sin_theta * _axis[0];
112 Y = sin_theta * _axis[1];
113 Z = sin_theta * _axis[2];
117 template <
class MatrixT>
128 {
return Quaternion(W, -X, -Y, -Z); }
138 {
return Quaternion(W*_q.W - X*_q.X - Y*_q.Y - Z*_q.Z,
139 W*_q.X + X*_q.W + Y*_q.Z - Z*_q.Y,
140 W*_q.Y - X*_q.Z + Y*_q.W + Z*_q.X,
141 W*_q.Z + X*_q.Y - Y*_q.X + Z*_q.W); }
146 {
return *
this = *
this * _q; }
150 template <
class Vec3T>
153 Quaternion q = *
this * Quaternion(0,_v[0],_v[1],_v[2]) *
conjugate();
154 return Vec3T(q[1], q[2], q[3]);
161 if (fabs(W) > 0.999999)
168 _angle = 2.0 * acos(W);
179 ww(W*W), xx(X*X), yy(Y*Y), zz(Z*Z), wx(W*X),
180 wy(W*Y), wz(W*Z), xy(X*Y), xz(X*Z), yz(Y*Z);
184 m(0,0) = ww + xx - yy - zz;
185 m(1,0) = 2.0*(xy + wz);
186 m(2,0) = 2.0*(xz - wy);
188 m(0,1) = 2.0*(xy - wz);
189 m(1,1) = ww - xx + yy - zz;
190 m(2,1) = 2.0*(yz + wx);
192 m(0,2) = 2.0*(xz + wy);
193 m(1,2) = 2.0*(yz - wx);
194 m(2,2) = ww - xx - yy + zz;
196 m(0,3) = m(1,3) = m(2,3) = m(3,0) = m(3,1) = m(3,2) = 0.0;
232 m(0,0) = W; m(0,1) = -X; m(0,2) = -Y; m(0,3) = -Z;
233 m(1,0) = X; m(1,1) = W; m(1,2) = Z; m(1,3) = -Y;
234 m(2,0) = Y; m(2,1) = -Z; m(2,2) = W; m(2,3) = X;
235 m(3,0) = Z; m(3,1) = Y; m(3,2) = -X; m(3,3) = W;
244 m(0,0) = W; m(0,1) = -X; m(0,2) = -Y; m(0,3) = -Z;
245 m(1,0) = X; m(1,1) = W; m(1,2) = -Z; m(1,3) = Y;
246 m(2,0) = Y; m(2,1) = Z; m(2,2) = W; m(2,3) = -X;
247 m(3,0) = Z; m(3,1) = -Y; m(3,2) = X; m(3,3) = W;
253 template<
class MatrixT>
256 Scalar trace = _rot(0,0) + _rot(1,1) + _rot(2,2);
259 Scalar s = 0.5 / sqrt(trace + 1.0);
261 X = ( _rot(2,1) - _rot(1,2) ) * s;
262 Y = ( _rot(0,2) - _rot(2,0) ) * s;
263 Z = ( _rot(1,0) - _rot(0,1) ) * s;
267 if ( _rot(0,0) > _rot(1,1) && _rot(0,0) > _rot(2,2) )
269 Scalar s = 2.0 * sqrt( 1.0 + _rot(0,0) - _rot(1,1) - _rot(2,2));
270 W = (_rot(2,1) - _rot(1,2) ) / s;
272 Y = (_rot(0,1) + _rot(1,0) ) / s;
273 Z = (_rot(0,2) + _rot(2,0) ) / s;
276 if (_rot(1,1) > _rot(2,2))
278 Scalar s = 2.0 * sqrt( 1.0 + _rot(1,1) - _rot(0,0) - _rot(2,2));
279 W = (_rot(0,2) - _rot(2,0) ) / s;
280 X = (_rot(0,1) + _rot(1,0) ) / s;
282 Z = (_rot(1,2) + _rot(2,1) ) / s;
286 Scalar s = 2.0 * sqrt( 1.0 + _rot(2,2) - _rot(0,0) - _rot(1,1) );
287 W = (_rot(1,0) - _rot(0,1) ) / s;
288 X = (_rot(0,2) + _rot(2,0) ) / s;
289 Y = (_rot(1,2) + _rot(2,1) ) / s;
300 Scalar theta( n.
norm());
301 Scalar sin_theta = sin(theta);
302 Scalar cos_theta = cos(theta);
305 n *= sin_theta/theta;
309 return Quaternion( cos_theta, n[0], n[1], n[2]);
318 if( w > 1.0) w = 1.0;
319 else if( w < -1.0) w = -1.0;
321 Scalar theta_half = acos(w);
324 Scalar n_norm( n.
norm());
327 n *= theta_half/n_norm;
331 return Quaternion( 0, n[0], n[1], n[2]);
343 std::cerr <<
"quaternion : " << (*this) << std::endl;
344 std::cerr <<
"length : " << this->
norm() << std::endl;
345 std::cerr <<
"axis, angle: " << axis <<
", " << angle*180.0/M_PI <<
"\n";
346 std::cerr <<
"rot matrix :\n";
347 std::cerr << m << std::endl;
365 #endif // ACG_QUATERNION_HH defined Quaternion exponential() const
quaternion exponential (for unit quaternions)
Namespace providing different geometric functions concerning angles.
Quaternion operator*(const Quaternion &_q) const
quaternion * quaternion
Matrix rotation_matrix() const
cast to rotation matrix
Quaternion conjugate() const
conjugate quaternion
auto norm() const -> decltype(std::sqrt(std::declval< VectorT< S, DIM >>().sqrnorm()))
compute euclidean norm
decltype(std::declval< S >() *std::declval< S >()) sqrnorm() const
compute squared euclidean norm
auto normalize() -> decltype(*this/=std::declval< VectorT< S, DIM >>().norm())
QuaternionT(const Vec3 &_p)
construct from 3D point (pure imaginary quaternion)
QuaternionT(Scalar _w=1.0, Scalar _x=0.0, Scalar _y=0.0, Scalar _z=0.0)
construct from 4 Scalars (default)
Matrix left_mult_matrix() const
get matrix for mult from left (q*p = Qp)
Matrix right_mult_matrix() const
get matrix for mult from right (p*q = Qp)
void init_from_matrix(const MatrixT &_rot)
get quaternion from rotation matrix
QuaternionT(Vec3 _axis, Scalar _angle)
construct from rotation axis and angle (in radians)
QuaternionT(const MatrixT &_rot)
construct from rotation matrix (only valid for rotation matrices!)
Quaternion logarithm() const
quaternion logarithm (for unit quaternions)
QuaternionT(const Vec4 &_p)
construct from 4D vector
void identity()
identity rotation
void axis_angle(Vec3 &_axis, Scalar &_angle) const
get rotation axis and angle (only valid for unit quaternions!)
Quaternion invert() const
invert quaternion
Vec3T rotate(const Vec3T &_v) const
rotate vector
Quaternion & operator*=(const Quaternion &_q)
quaternion *= quaternion