lisyarus/psemek
1
0
Fork 1
Code Issues Pull requests 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>
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>
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>
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>
@ -61,7 +61,7 @@ namespace psemek::geom
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]}};
return quaternion<T>{{s * axis[0], s * axis[1], s * axis[2], c}};
}
template <typename T>
@ -74,22 +74,22 @@ namespace psemek::geom
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}};
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])
{
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])
{
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
{
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> 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];
r[0] = q1[3] * q2[0] + q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1];
r[1] = q1[3] * q2[1] - q1[0] * q2[2] + q1[1] * q2[3] + q1[2] * q2[0];
r[2] = q1[3] * q2[2] + q1[0] * q2[1] - q1[1] * q2[0] + q1[2] * q2[3];
r[3] = q1[3] * q2[3] - q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2];
return r;
}
@ -149,7 +149,7 @@ namespace psemek::geom
template <typename T>
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>
@ -162,7 +162,7 @@ namespace psemek::geom
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]};
return {res[0], res[1], res[2]};
}
template <typename T>

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

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