pose.h

Go to the documentation of this file.

#pragma once
 
#include "fvec.h"
#include "fmat.h"
#include "quaternion.h"
#include <cassert>
 
namespace cgv { 
        namespace math { 
 
template <typename T> fmat<T, 3, 3>& pose_orientation(fmat<T, 3, 4>& pose) { return reinterpret_cast<fmat<T, 3, 3>&>(pose); }
template <typename T> const fmat<T, 3, 3>& pose_orientation(const fmat<T, 3, 4>& pose) { return reinterpret_cast<const fmat<T, 3, 3>&>(pose); }
template <typename T> fvec<T, 3>& pose_position(fmat<T, 3, 4>& pose) { return reinterpret_cast<fvec<T, 3>&>(pose(0, 3)); }
template <typename T> const fvec<T, 3>& pose_position(const fmat<T, 3, 4>& pose) { return reinterpret_cast<const fvec<T, 3>&>(pose(0, 3)); }
 
template <typename T>
fvec<T, 3> pose_transform_point(const fmat<T, 3, 4>& pose, const fvec<T, 3>& p) { return pose_orientation(pose)* p + pose_position(pose); }
template <typename T>
fvec<T, 3> pose_transform_point(const quaternion<T>& q, const fvec<T,3>& pos, const fvec<T, 3>& p) { return q.get_rotated(p) + pos; }
template <typename T>
fvec<T, 3> pose_transform_vector(const fmat<T, 3, 4>& pose, const fvec<T, 3>& v) { return pose_orientation(pose) * v; }
template <typename T>
fvec<T, 3> inverse_pose_transform_point(const fmat<T, 3, 4>& pose, const fvec<T, 3>& p) { return (p - pose_position(pose))*pose_orientation(pose); }
template <typename T>
fvec<T, 3> inverse_pose_transform_vector(const fmat<T, 3, 4>& pose, const fvec<T, 3>& v) { return v * pose_orientation(pose); }
template <typename T> void invert_pose(fmat<T, 3, 4>& pose) { pose_orientation(pose).transpose(); pose_position(pose) = -pose_orientation(pose)*pose_position(pose); }
template <typename T> fmat<T, 3, 4> pose_inverse(const fmat<T, 3, 4>& pose) { fmat<T, 3, 4> inv_pose = pose; invert_pose(inv_pose); return inv_pose; }
template <typename T> fmat<T, 3, 4> pose_construct(const fmat<T, 3, 3>& orientation, const fvec<T, 3>& position) { fmat<T, 3, 4> pose; pose_orientation(pose) = orientation; pose_position(pose) = position; return pose; }
template <typename T> fmat<T, 3, 4> pose_construct(const quaternion<T>& orientation, const fvec<T, 3>& position) { fmat<T, 3, 4> pose; orientation.put_matrix(pose_orientation(pose)); pose_position(pose) = position; return pose; }
template <typename T> void pose_append(fmat<T, 3, 4>& pose_1, const fmat<T, 3, 4>& pose_2) {
        pose_position(pose_1) += pose_orientation(pose_1)*pose_position(pose_2);
        pose_orientation(pose_1) *= pose_orientation(pose_2);
}
template <typename T> void pose_transform(const fmat<T, 3, 4>& pose_transform, fmat<T, 3, 4>& pose) {
        pose_position(pose) = pose_position(pose_transform) + pose_orientation(pose_transform)*pose_position(pose);
        pose_orientation(pose) = pose_orientation(pose_transform)*pose_orientation(pose);
}
template <typename T> fmat<T, 3, 4> pose_concat(const fmat<T, 3, 4>& pose_1, const fmat<T, 3, 4>& pose_2) {
        fmat<T, 3, 4> pose = pose_1;
        pose_append(pose, pose_2);
        return pose;
}
 
        }
}