Add quaternions & quaternion-based rotations
This commit is contained in:
parent
2d7d38bc6d
commit
e871517909
2 changed files with 250 additions and 0 deletions
140
libs/geom/include/psemek/geom/quaternion.hpp
Normal file
140
libs/geom/include/psemek/geom/quaternion.hpp
Normal file
|
|
@ -0,0 +1,140 @@
|
|||
#pragma once
|
||||
|
||||
#include <psemek/geom/vector.hpp>
|
||||
|
||||
namespace psemek::geom
|
||||
{
|
||||
|
||||
template <typename T>
|
||||
struct quaternion
|
||||
{
|
||||
geom::vector<T, 4> coords;
|
||||
|
||||
quaternion() = default;
|
||||
|
||||
static quaternion<T> zero();
|
||||
static quaternion<T> identity();
|
||||
static quaternion<T> scalar(T value);
|
||||
static quaternion<T> vector(geom::vector<T, 3> const & v);
|
||||
static quaternion<T> rotation(T angle, geom::vector<T, 3> const & axis);
|
||||
|
||||
template <typename ... Args>
|
||||
quaternion(Args && ... args)
|
||||
: coords{ static_cast<T>(std::forward<Args>(args))... }
|
||||
{
|
||||
static_assert(sizeof...(Args) == 4);
|
||||
}
|
||||
|
||||
T & operator[] (std::size_t i) { return coords[i]; }
|
||||
T const & operator[] (std::size_t i) const { return coords[i]; }
|
||||
};
|
||||
|
||||
template <typename ... Args>
|
||||
quaternion(Args && ...) -> quaternion<std::common_type_t<Args...>>;
|
||||
|
||||
template <typename T>
|
||||
quaternion<T> quaternion<T>::zero()
|
||||
{
|
||||
return quaternion<T>{T(0), T(0), T(0), T(0)};
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
quaternion<T> quaternion<T>::identity()
|
||||
{
|
||||
return quaternion<T>{T(1), T(0), T(0), T(0)};
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
quaternion<T> quaternion<T>::scalar(T value)
|
||||
{
|
||||
return {value, T(0), T(0), T(0)};
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
quaternion<T> quaternion<T>::vector(geom::vector<T, 3> const & v)
|
||||
{
|
||||
return {T(0), v[0], v[1], v[2]};
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
quaternion<T> quaternion<T>::rotation(T angle, geom::vector<T, 3> const & axis)
|
||||
{
|
||||
auto const c = std::cos(angle / 2);
|
||||
auto const s = std::sin(angle / 2);
|
||||
|
||||
return quaternion<T>{c, s * axis[0], s * axis[1], s * axis[2]};
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
quaternion<T> operator - (quaternion<T> const & q)
|
||||
{
|
||||
return {-q.coords};
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
quaternion<T> operator + (quaternion<T> const & q1, quaternion<T> const & q2)
|
||||
{
|
||||
return {q1.coords + q2.coords};
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
quaternion<T> operator - (quaternion<T> const & q1, quaternion<T> const & q2)
|
||||
{
|
||||
return {q1.coords - q2.coords};
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
quaternion<T> operator * (quaternion<T> const & q, T s)
|
||||
{
|
||||
return {q.coords * s};
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
quaternion<T> operator * (T s, quaternion<T> const & q)
|
||||
{
|
||||
return {s * q.coords};
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
quaternion<T> operator * (quaternion<T> const & q1, quaternion<T> const & q2)
|
||||
{
|
||||
quaternion<T> r;
|
||||
r[0] = q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3];
|
||||
r[1] = q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2];
|
||||
r[2] = q1[0] * q2[2] - q1[1] * q2[3] + q1[2] * q2[0] + q1[3] * q2[1];
|
||||
r[3] = q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1] + q1[3] * q2[0];
|
||||
return r;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
T norm_sqr(quaternion<T> const & q)
|
||||
{
|
||||
return length_sqr(q.coords);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
T norm(quaternion<T> const & q)
|
||||
{
|
||||
return length(q.coords);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
quaternion<T> conjugate(quaternion<T> const & q)
|
||||
{
|
||||
return {q[0], -q[1], -q[2], -q[3]};
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
quaternion<T> inverse(quaternion<T> const & q)
|
||||
{
|
||||
return conjugate(q) / norm(q);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
vector<T, 3> rotate(quaternion<T> const & q, vector<T, 3> const & v)
|
||||
{
|
||||
auto res = q * quaternion<T>::vector(v) * inverse(q);
|
||||
return {res[1], res[2], res[3]};
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -1,6 +1,7 @@
|
|||
#pragma once
|
||||
|
||||
#include <psemek/geom/affine_transform.hpp>
|
||||
#include <psemek/geom/quaternion.hpp>
|
||||
|
||||
#include <psemek/util/assert.hpp>
|
||||
|
||||
|
|
@ -59,6 +60,30 @@ namespace psemek::geom
|
|||
void fill_matrix(Matrix & m) const;
|
||||
};
|
||||
|
||||
template <typename T>
|
||||
struct quaternion_rotation
|
||||
{
|
||||
quaternion<T> quat;
|
||||
|
||||
quaternion_rotation();
|
||||
quaternion_rotation(quaternion<T> const & quat);
|
||||
quaternion_rotation(T angle, vector<T, 3> const & axis);
|
||||
|
||||
matrix<T, 3, 4> affine_matrix() const;
|
||||
matrix<T, 3, 3> linear_matrix() const;
|
||||
vector<T, 3> translation_vector() const;
|
||||
matrix<T, 4, 4> homogeneous_matrix() const;
|
||||
|
||||
affine_transform<T, 3, 3> transform() const;
|
||||
|
||||
vector<T, 3> operator()(vector<T, 3> const & v) const;
|
||||
point<T, 3> operator()(point<T, 3> const & p) const;
|
||||
|
||||
private:
|
||||
template <typename Matrix>
|
||||
void fill_matrix(Matrix & m) const;
|
||||
};
|
||||
|
||||
template <typename T>
|
||||
axis_rotation(vector<T, 3>, T) -> axis_rotation<T>;
|
||||
|
||||
|
|
@ -239,4 +264,89 @@ namespace psemek::geom
|
|||
return {r.axis, -r.angle};
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
quaternion_rotation<T>::quaternion_rotation()
|
||||
: quat{quaternion<T>::identity()}
|
||||
{}
|
||||
|
||||
template <typename T>
|
||||
quaternion_rotation<T>::quaternion_rotation(quaternion<T> const & quat)
|
||||
: quat{quat}
|
||||
{}
|
||||
|
||||
template <typename T>
|
||||
quaternion_rotation<T>::quaternion_rotation(T angle, vector<T, 3> const & axis)
|
||||
: quat{quaternion<T>::rotation(angle, axis)}
|
||||
{}
|
||||
|
||||
template <typename T>
|
||||
matrix<T, 3, 4> quaternion_rotation<T>::affine_matrix() const
|
||||
{
|
||||
auto result = matrix<T, 3, 4>::identity();
|
||||
fill_matrix(result);
|
||||
return result;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
matrix<T, 3, 3> quaternion_rotation<T>::linear_matrix() const
|
||||
{
|
||||
auto result = matrix<T, 3, 3>::identity();
|
||||
fill_matrix(result);
|
||||
return result;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
vector<T, 3> quaternion_rotation<T>::translation_vector() const
|
||||
{
|
||||
return vector<T, 3>::zero();
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
matrix<T, 4, 4> quaternion_rotation<T>::homogeneous_matrix() const
|
||||
{
|
||||
auto result = matrix<T, 4, 4>::identity();
|
||||
fill_matrix(result);
|
||||
return result;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
affine_transform<T, 3, 3> quaternion_rotation<T>::transform() const
|
||||
{
|
||||
return {affine_matrix()};
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
vector<T, 3> quaternion_rotation<T>::operator()(vector<T, 3> const & v) const
|
||||
{
|
||||
return linear_matrix() * v;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
point<T, 3> quaternion_rotation<T>::operator()(point<T, 3> const & p) const
|
||||
{
|
||||
static constexpr auto o = point<T, 3>::zero();
|
||||
return o + linear_matrix() * (p - o);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
template <typename Matrix>
|
||||
void quaternion_rotation<T>::fill_matrix(Matrix & m) const
|
||||
{
|
||||
m[0][0] = 1 - 2 * (quat[2] * quat[2] + quat[3] * quat[3]);
|
||||
m[0][1] = 2 * (quat[1] * quat[2] - quat[0] * quat[3]);
|
||||
m[0][2] = 2 * (quat[1] * quat[3] + quat[0] * quat[2]);
|
||||
m[1][0] = 2 * (quat[1] * quat[2] + quat[0] * quat[3]);
|
||||
m[1][1] = 1 - 2 * (quat[1] * quat[1] + quat[3] * quat[3]);
|
||||
m[1][2] = 2 * (quat[2] * quat[3] - quat[0] * quat[1]);
|
||||
m[2][0] = 2 * (quat[1] * quat[3] - quat[0] * quat[2]);
|
||||
m[2][1] = 2 * (quat[2] * quat[3] + quat[0] * quat[1]);
|
||||
m[2][2] = 1 - 2 * (quat[1] * quat[1] + quat[2] * quat[2]);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
quaternion_rotation<T> inverse(quaternion_rotation<T> const & r)
|
||||
{
|
||||
return quaternion_rotation<T>{quaternion<T>{r.quat[0], -r.quat[1], -r.quat[2], -r.quat[3]}};
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue