#ifndef Foundation_Math_transform_INCLUDED
#define Foundation_Math_transform_INCLUDED


// transformers

#include <foundation/Math/Quat.hpp>
#include <foundation/Math/Matrix.hpp>


// transformees

#include <foundation/Math/Vec.hpp>
#include <foundation/Math/Point.hpp>


namespace Tn {

namespace math {


template <typename T>
Point<3,T> transform(Quat<T> const & q, Point<3,T> const & p) {
    Quat<T> qp(T(0), p[0], p[1], p[2]);
    Quat<T> rp = q * qp * conjugate(q);
    return Point3<T>(rp.i, rp.j, rp.k);
}


template <typename T>
Vec<3,T> transform(Quat<T> const & q, Vec<3,T> const & v) {
    Quat<T> qp(T(0), v[0], v[1], v[2]);
    Quat<T> rp = q * qp * conjugate(q);
    return Vec3<T>(rp.i, rp.j, rp.k);
}


template <typename T>
Point<3,T> transform(Matrix<T> const & m, Point<3,T> const & p) {

    Point<3,T> result;

    T w = m.m[0][3]*p[0] + m.m[1][3]*p[1] + m.m[2][3]*p[2] + m.m[3][3];
    T inv_w = invert(w);

    result[0] = (m.m[0][0]*p[0] + m.m[1][0]*p[1] + m.m[2][0]*p[2] + m.m[3][0])*inv_w;
    result[1] = (m.m[0][1]*p[0] + m.m[1][1]*p[1] + m.m[2][1]*p[2] + m.m[3][1])*inv_w;
    result[2] = (m.m[0][2]*p[0] + m.m[1][2]*p[1] + m.m[2][2]*p[2] + m.m[3][2])*inv_w;
    
    return result;
}


template <typename T>
Vec<3,T> transform(Matrix<T> const & m, Vec<3,T> const & v) {

    Vec<3,T> result;
    result[0] = m.m[0][0]*v[0] + m.m[1][0]*v[1] + m.m[2][0]*v[2];
    result[1] = m.m[0][1]*v[0] + m.m[1][1]*v[1] + m.m[2][1]*v[2];
    result[2] = m.m[0][2]*v[0] + m.m[1][2]*v[1] + m.m[2][2]*v[2];
    return result;
}

} // end namespace math

} // end namespace Tn


#endif

