lisyarus/psemek
1
0
Fork 1
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Implement constructing quaternion from matrix

Browse source
This commit is contained in:
Nikita Lisitsa 2021-03-13 16:41:19 +03:00
parent 05cffcdc73
commit b4731b77df
1 changed files with 31 additions and 0 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

31
libs/geom/include/psemek/geom/quaternion.hpp
View file

@ -1,6 +1,7 @@
#pragma once
#include <psemek/geom/vector.hpp>
#include <psemek/geom/math.hpp>
namespace psemek::geom
{
@ -17,6 +18,7 @@ namespace psemek::geom
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);
static quaternion<T> rotation(matrix<T, 3, 3> const & m);
quaternion(geom::vector<T, 4> const & v)
: coords{v}
@ -62,6 +64,35 @@ namespace psemek::geom
return quaternion<T>{{c, s * axis[0], s * axis[1], s * axis[2]}};
}
template <typename T>
quaternion<T> quaternion<T>::rotation(matrix<T, 3, 3> const & m)
{
// https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
auto tr = trace(m);
if (tr > 0)
{
auto S = std::sqrt(tr + 1) * 2;
return {{S / 4, (m[2][1] - m[1][2]) / S, (m[0][2] - m[2][0]) / S, (m[1][0] - m[0][1]) / S}};
}
else if (m[0][0] > m[1][1] && m[0][0] > m[2][2])
{
auto S = std::sqrt(1 + m[0][0] - m[1][1] - m[2][2]) * 2;
return {{(m[2][1] - m[1][2]) / S, S / 4, (m[0][1] + m[1][0]) / S, (m[0][2] + m[2][0]) / S}};
}
else if (m[1][1] > m[2][2])
{
auto S = std::sqrt(1 - m[0][0] + m[1][1] - m[2][2]) * 2;
return {{(m[0][2] - m[2][0]) / S, (m[0][1] + m[1][0]) / S, S / 4, (m[1][2] + m[2][1]) / S}};
}
else
{
auto S = std::sqrt(1 - m[0][0] - m[1][1] + m[2][2]) * 2;
return {{(m[1][0] - m[0][1]) / S, (m[0][2] + m[2][0]) / S, (m[1][2] + m[2][1]) / S, S / 4}};
}
}
template <typename T>
quaternion<T> operator - (quaternion<T> const & q)
{