ALPAKA/sta-peak
1
0
Fork 0
mirror of https://git.intern.spaceteamaachen.de/ALPAKA/sta-peak.git synced 2025-06-11 18:35:59 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

Add Quaternion to rotation matrix

Browse Source
This commit is contained in:
Milo Priegnitz 2024-06-22 15:30:31 +02:00
parent fc6339564a
commit a2cf92dd69
2 changed files with 20 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
include/sta/math/quaternion.hpp
View File

@ -8,6 +8,7 @@
#ifndef STA_PEAK_QUATERNION_HPP
#define STA_PEAK_QUATERNION_HPP
#include <sta/math/linalg/matrix.hpp>
namespace sta
{
namespace math {
@ -24,6 +25,8 @@ namespace sta
float norm();
matrix toRotationMatrix();
Quaternion normalized();
Quaternion operator*(const Quaternion& quat);

17
src/quaternion.cpp
View File

@ -54,6 +54,23 @@ namespace sta
return Quaternion(qw, qx, qy, qz).normalized();
}
matrix Quaternion::toRotationMatrix(){
matrix R = matrix(3, 3);
R.set(0, 0, 2 *(w*w + x*x) - 1);
R.set(0, 1, 2 *(x*y - w*z));
R.set(0, 2, 2 *(x*z + w*y));
R.set(1, 0, 2 *(x*y + w*z));
R.set(1, 1, 2 *(w*w + y*y) - 1);
R.set(1, 2, 2 *(y*z - w*x));
R.set(2, 0, 2 *(x*z - w*y));
R.set(2, 1, 2 *(y*z + w*x));
R.set(2, 2, 2 *(w*w + z*z) - 1);
return R;
}
Quaternion Quaternion::unit()
{
return Quaternion();