CGII/framework/include/cgv/math/rigid_transform.h

89 lines
3.3 KiB
C
Raw Normal View History

2018-05-17 13:50:03 +00:00
#pragma once
#include <cgv/math/fvec.h>
#include <cgv/math/quaternion.h>
#include <cgv/math/fmat.h>
namespace cgv {
namespace math {
/** implementation of a rigid body transformation with a quaternion and a
translation vector. */
template <typename T>
class rigid_transform
{
public:
/// type of 3d vector
typedef cgv::math::fvec<T,3> vec_type;
/// type of homogenous vector
typedef cgv::math::fvec<T,4> hvec_type;
/// type of quaternions
typedef cgv::math::quaternion<T> quat_type;
/// type of 3x3 matrix
typedef cgv::math::fmat<T,3,3> mat_type;
/// type of 4x4 matrix
typedef cgv::math::fmat<T,4,4> hmat_type;
protected:
/// store transformation as quaternion
quat_type q;
/// and translation vector
vec_type t;
public:
/// construct identity
rigid_transform() : q(1,0,0,0), t(0,0,0) {}
/// construct from quaternion and translation vector to the transformation that first rotates and then translates
rigid_transform(const quat_type& _q, const vec_type& _t) : q(_q), t(_t) {}
/// apply transformation to vector
void transform_vector(vec_type& v) const { q.rotate(v); }
/// apply transformation to point
void transform_point(vec_type& p) const { q.rotate(p); p += t; }
/// concatenate two rigid transformations
rigid_transform<T> operator * (const rigid_transform<T>& M) const { return rigid_transform<T>(q*M.q, get_transformed_vector(M.t)+t); }
/// multiply quaternion and translation with scalar
rigid_transform<T>& operator += (const rigid_transform<T>& T) { q += T.q; t += T.t; return *this; }
/// multiply quaternion and translation with scalar
rigid_transform<T>& operator *= (T s) { q.vec() *= s; t *= s; return *this; }
/// multiply quaternion and translation with scalar
rigid_transform<T> operator * (T s) const { rigid_transform<T> r(*this); r *= s; return r; }
///
void normalize() { q.normalize(); }
/// return the inverse transformation
rigid_transform<T> inverse() const { return rigid_transform<T>(q.inverse(), q.inverse().apply(-t)); }
/// apply transformation to vector
vec_type get_transformed_vector(const vec_type& v) const { return q.apply(v); }
/// apply transformation to point
vec_type get_transformed_point(const vec_type& p) const { return q.apply(p)+t; }
/// return the translational part
const vec_type& get_translation() const { return t; }
/// return the translational part
vec_type& ref_translation() { return t; }
/// return the rotation as quaternion
const quat_type& get_quaternion() const { return q; }
/// return the rotation as quaternion
quat_type& ref_quaternion() { return q; }
/// convert transformation to homogeneous transformation
hmat_type get_hmat() const {
T M[9];
q.put_matrix(M);
hmat_type H;
H.set_col(0, hvec_type(M[0], M[3], M[6], 0));
H.set_col(1, hvec_type(M[1], M[4], M[7], 0));
H.set_col(2, hvec_type(M[2], M[5], M[8], 0));
H.set_col(3, hvec_type(t(0), t(1), t(2), 1));
return H;
}
/// convert transformation to transpose of homogeneous transformation
hmat_type get_transposed_hmat() const {
T M[9];
q.put_matrix(M);
hmat_type H;
H.set_row(0, hvec_type(M[0], M[3], M[6], 0));
H.set_row(1, hvec_type(M[1], M[4], M[7], 0));
H.set_row(2, hvec_type(M[2], M[5], M[8], 0));
H.set_row(3, hvec_type(t(0), t(1), t(2), 1));
return H;
}
};
}
}