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;
        }
};
 
        }
}