ALPAKA/sta-peak
1
0
Fork 0
mirror of https://git.intern.spaceteamaachen.de/ALPAKA/sta-peak.git synced 2025-06-13 19:25:58 +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 committed by dario
parent 86cd951d5e
commit 1393ade57b
2 changed files with 20 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -8,6 +8,7 @@
#ifndef STA_PEAK_QUATERNION_HPP #ifndef STA_PEAK_QUATERNION_HPP
#define STA_PEAK_QUATERNION_HPP #define STA_PEAK_QUATERNION_HPP
#include <sta/math/linalg/matrix.hpp>
namespace sta namespace sta
{ {
namespace math { namespace math {
@ -55,11 +56,8 @@ namespace sta
*/ */
float norm(); float norm();
/** matrix toRotationMatrix();
* @brief
*
* @return Quaternion
*/
Quaternion normalized(); Quaternion normalized();
Quaternion conjugate(); Quaternion conjugate();

17
src/quaternion.cpp
View File

@ -54,6 +54,23 @@ namespace sta
return Quaternion(qw, qx, qy, qz).normalized(); 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() Quaternion Quaternion::unit()
{ {
return Quaternion(); return Quaternion();