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

Change quaternion coordinate order: wxyz -> xyzw

Browse source
This commit is contained in:
Nikita Lisitsa 2021-07-13 11:45:11 +03:00
parent 96652584a9
commit b51b0d3a94
2 changed files with 24 additions and 24 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -40,19 +40,19 @@ namespace psemek::geom
template <typename T> template <typename T>
quaternion<T> quaternion<T>::identity() quaternion<T> quaternion<T>::identity()
{ {
return quaternion<T>{{T(1), T(0), T(0), T(0)}}; return quaternion<T>{{T(0), T(0), T(0), T(1)}};
} }
template <typename T> template <typename T>
quaternion<T> quaternion<T>::scalar(T value) quaternion<T> quaternion<T>::scalar(T value)
{ {
return {{value, T(0), T(0), T(0)}}; return {{T(0), T(0), T(0), value}};
} }
template <typename T> template <typename T>
quaternion<T> quaternion<T>::vector(geom::vector<T, 3> const & v) quaternion<T> quaternion<T>::vector(geom::vector<T, 3> const & v)
{ {
return {{T(0), v[0], v[1], v[2]}}; return {{v[0], v[1], v[2], T(0)}};
} }
template <typename T> template <typename T>
@ -61,7 +61,7 @@ namespace psemek::geom
auto const c = std::cos(angle / 2); auto const c = std::cos(angle / 2);
auto const s = std::sin(angle / 2); auto const s = std::sin(angle / 2);
return quaternion<T>{{c, s * axis[0], s * axis[1], s * axis[2]}}; return quaternion<T>{{s * axis[0], s * axis[1], s * axis[2], c}};
} }
template <typename T> template <typename T>
@ -74,22 +74,22 @@ namespace psemek::geom
if (tr > 0) if (tr > 0)
{ {
auto S = std::sqrt(tr + 1) * 2; 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}}; return {{(m[2][1] - m[1][2]) / S, (m[0][2] - m[2][0]) / S, (m[1][0] - m[0][1]) / S, S / 4}};
} }
else if (m[0][0] > m[1][1] && m[0][0] > m[2][2]) 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; 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}}; return {{S / 4, (m[0][1] + m[1][0]) / S, (m[0][2] + m[2][0]) / S, (m[2][1] - m[1][2]) / S}};
} }
else if (m[1][1] > m[2][2]) else if (m[1][1] > m[2][2])
{ {
auto S = std::sqrt(1 - m[0][0] + m[1][1] - m[2][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}}; return {{(m[0][1] + m[1][0]) / S, S / 4, (m[1][2] + m[2][1]) / S, (m[0][2] - m[2][0]) / S}};
} }
else else
{ {
auto S = std::sqrt(1 - m[0][0] - m[1][1] + m[2][2]) * 2; 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}}; return {{(m[0][2] + m[2][0]) / S, (m[1][2] + m[2][1]) / S, S / 4, (m[1][0] - m[0][1]) / S}};
} }
} }
@ -127,10 +127,10 @@ namespace psemek::geom
quaternion<T> operator * (quaternion<T> const & q1, quaternion<T> const & q2) quaternion<T> operator * (quaternion<T> const & q1, quaternion<T> const & q2)
{ {
quaternion<T> r; quaternion<T> r;
r[0] = q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3]; r[0] = q1[3] * q2[0] + q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1];
r[1] = q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2]; r[1] = q1[3] * q2[1] - q1[0] * q2[2] + q1[1] * q2[3] + q1[2] * q2[0];
r[2] = q1[0] * q2[2] - q1[1] * q2[3] + q1[2] * q2[0] + q1[3] * q2[1]; r[2] = q1[3] * q2[2] + q1[0] * q2[1] - q1[1] * q2[0] + q1[2] * q2[3];
r[3] = q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1] + q1[3] * q2[0]; r[3] = q1[3] * q2[3] - q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2];
return r; return r;
} }
@ -149,7 +149,7 @@ namespace psemek::geom
template <typename T> template <typename T>
quaternion<T> conjugate(quaternion<T> const & q) quaternion<T> conjugate(quaternion<T> const & q)
{ {
return {{q[0], -q[1], -q[2], -q[3]}}; return {{-q[0], -q[1], -q[2], q[3]}};
} }
template <typename T> template <typename T>
@ -162,7 +162,7 @@ namespace psemek::geom
vector<T, 3> rotate(quaternion<T> const & q, vector<T, 3> const & v) vector<T, 3> rotate(quaternion<T> const & q, vector<T, 3> const & v)
{ {
auto res = q * quaternion<T>::vector(v) * inverse(q); auto res = q * quaternion<T>::vector(v) * inverse(q);
return {res[1], res[2], res[3]}; return {res[0], res[1], res[2]};
} }
template <typename T> template <typename T>

20
libs/geom/include/psemek/geom/rotation.hpp
View file

@ -332,21 +332,21 @@ namespace psemek::geom
template <typename Matrix> template <typename Matrix>
void quaternion_rotation<T>::fill_matrix(Matrix & m) const void quaternion_rotation<T>::fill_matrix(Matrix & m) const
{ {
m[0][0] = 1 - 2 * (quat[2] * quat[2] + quat[3] * quat[3]); m[0][0] = 1 - 2 * (quat[1] * quat[1] + quat[2] * quat[2]);
m[0][1] = 2 * (quat[1] * quat[2] - quat[0] * quat[3]); m[0][1] = 2 * (quat[0] * quat[1] - quat[3] * quat[2]);
m[0][2] = 2 * (quat[1] * quat[3] + quat[0] * quat[2]); m[0][2] = 2 * (quat[0] * quat[2] + quat[3] * quat[1]);
m[1][0] = 2 * (quat[1] * quat[2] + quat[0] * quat[3]); m[1][0] = 2 * (quat[0] * quat[1] + quat[3] * quat[2]);
m[1][1] = 1 - 2 * (quat[1] * quat[1] + quat[3] * quat[3]); m[1][1] = 1 - 2 * (quat[0] * quat[0] + quat[2] * quat[2]);
m[1][2] = 2 * (quat[2] * quat[3] - quat[0] * quat[1]); m[1][2] = 2 * (quat[1] * quat[2] - quat[3] * quat[0]);
m[2][0] = 2 * (quat[1] * quat[3] - quat[0] * quat[2]); m[2][0] = 2 * (quat[0] * quat[2] - quat[3] * quat[1]);
m[2][1] = 2 * (quat[2] * quat[3] + quat[0] * quat[1]); m[2][1] = 2 * (quat[1] * quat[2] + quat[3] * quat[0]);
m[2][2] = 1 - 2 * (quat[1] * quat[1] + quat[2] * quat[2]); m[2][2] = 1 - 2 * (quat[0] * quat[0] + quat[1] * quat[1]);
} }
template <typename T> template <typename T>
quaternion_rotation<T> inverse(quaternion_rotation<T> const & r) 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]}}; return quaternion_rotation<T>{quaternion<T>{-r.quat[0], -r.quat[1], -r.quat[2], r.quat[3]}};
} }
} }