rigid_transform.h

#pragma once
 
#include <cgv/math/fvec.h>
#include <cgv/math/quaternion.h>
#include <cgv/math/fmat.h>
 
namespace cgv {
    namespace math {
 
template <typename T>
class rigid_transform
{
public:
    typedef cgv::math::fvec<T,3> vec_type;
    typedef cgv::math::fvec<T,4> hvec_type;
    typedef cgv::math::quaternion<T> quat_type;
    typedef cgv::math::fmat<T,3,3> mat_type;
    typedef cgv::math::fmat<T,4,4> hmat_type;
protected:
    quat_type q;
    vec_type  t;
public:
    rigid_transform() : q(1,0,0,0), t(0,0,0) {}
    rigid_transform(const quat_type& _q, const vec_type& _t) : q(_q), t(_t) {}
    void transform_vector(vec_type& v) const { q.rotate(v); }
    void transform_point(vec_type& p) const { q.rotate(p); p += t; }
    rigid_transform<T> operator * (const rigid_transform<T>& M) const { return rigid_transform<T>(q*M.q, get_transformed_vector(M.t)+t); }
    rigid_transform<T>& operator += (const rigid_transform<T>& T) { q += T.q; t += T.t; return *this; }
    rigid_transform<T>& operator *= (T s) { q.vec() *= s; t *= s; return *this; }
    rigid_transform<T> operator * (T s) const { rigid_transform<T> r(*this); r *= s; return r; }
    void normalize() { q.normalize(); }
    rigid_transform<T> inverse() const { return rigid_transform<T>(q.inverse(), q.inverse().apply(-t)); }
    vec_type get_transformed_vector(const vec_type& v) const { return q.apply(v); }
    vec_type get_transformed_point(const vec_type& p) const { return q.apply(p)+t; }
    const vec_type& get_translation() const { return t; }
    vec_type& ref_translation() { return t; }
    const quat_type& get_quaternion() const { return q; }
    quat_type& ref_quaternion() { return q; }
    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;
    }
    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;
    }
};
 
    }
}