89 lines
3.3 KiB
C
89 lines
3.3 KiB
C
|
#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;
|
||
|
|
}
|
||
|
|
};
|
||
|
|
|
||
|
|
}
|
||
|
|
}
|