Here is some code that i wrote already.
It moves the given 3 Euler angles into a matrix.
#include "pch.h"
#include <iostream>
#include <string>
#include "linalg.h"
using namespace linalg::aliases;
using namespace std;
float x,y,z;
//Pre
void MatrixXZY(float3 angles, float3x3& matrix);
void MatrixXYZ(float3 angles, float3x3& matrix);
void MatrixYXZ(float3 angles, float3x3& matrix);
void MatrixYZX(float3 angles, float3x3& matrix);
void MatrixZYX(float3 angles, float3x3& matrix);
void MatrixZXY(float3 angles, float3x3& matrix);
void PrintMatrix(string name, float3 angles, float3x3& matrix);
void MatrixDecomposeYXZ(float3x3& matrix, float3& angles);
int main()
{
float3 AnglesIn = { 0, 0, 0 };
float3 AnglesOut;
float3x3 Matrix; // Matrix [Spalte][Zeile]
cout << "-----------------------------" << endl;
cout << "Input" << endl;
cout << AnglesIn[0] << " " << AnglesIn[1] << " " << AnglesIn[2] << " " << endl;
cout << "-----------------------------" << endl << endl;
MatrixXZY(AnglesIn, Matrix);
PrintMatrix("XZY", AnglesIn, Matrix);
MatrixXYZ(AnglesIn, Matrix);
PrintMatrix("XYZ", AnglesIn, Matrix);
MatrixYXZ(AnglesIn, Matrix);
PrintMatrix("YXZ", AnglesIn, Matrix);
MatrixDecomposeYXZ(Matrix, AnglesOut);
cout << "-----------------------------" << endl;
cout << AnglesOut.x << " " << AnglesOut.y << " " << AnglesOut.z << " " << endl;
cout << "-----------------------------" << endl << endl;
MatrixYZX(AnglesIn, Matrix);
PrintMatrix("YZX", AnglesIn, Matrix);
MatrixZYX(AnglesIn, Matrix);
PrintMatrix("ZYX", AnglesIn, Matrix);
MatrixZXY(AnglesIn, Matrix);
PrintMatrix("ZXY", AnglesIn, Matrix);
}
void MatrixXZY(float3 angles, float3x3& matrix)
{
float cosX = cosf(angles.x); // X
float sinX = sinf(angles.x);
float cosY = cosf(angles.y); // Y
float sinY = sinf(angles.y);
float cosZ = cosf(angles.z); // Z
float sinZ = sinf(angles.z);
matrix[0][0] = cosZ * cosY; // Spalte 1
matrix[0][1] = sinX * sinY + cosX * cosY * sinZ;
matrix[0][2] = cosY * sinX * sinZ - cosX * sinY;
matrix[1][0] = -sinZ; // Spalte 2
matrix[1][1] = cosX * cosZ;
matrix[1][2] = cosZ * sinX;
matrix[2][0] = cosZ * sinY; // Spalte 3
matrix[2][1] = cosX * sinZ * sinY - cosY * sinX;
matrix[2][2] = cosX * cosY + sinX * sinZ * sinY;
}
void MatrixXYZ(float3 angles, float3x3& matrix)
{
float cosX = cosf(angles.x); // X
float sinX = sinf(angles.x);
float cosY = cosf(angles.y); // Y
float sinY = sinf(angles.y);
float cosZ = cosf(angles.z); // Z
float sinZ = sinf(angles.z);
matrix[0][0] = cosY * cosZ; // Spalte 1
matrix[0][1] = cosX * sinZ + cosZ * sinX * sinY;
matrix[0][2] = sinX * sinZ - cosX * cosZ * sinY;
matrix[1][0] = -cosY * sinZ; // Spalte 2
matrix[1][1] = cosX * cosZ - sinX * sinY * sinZ;
matrix[1][2] = cosZ * sinX + cosX * sinY * sinZ;
matrix[2][0] = sinY; // Spalte 3
matrix[2][1] = -cosY * sinX;
matrix[2][2] = cosX * cosY;
}
void MatrixYXZ(float3 angles, float3x3& matrix)
{
float cosX = cosf(angles.x); // X
float sinX = sinf(angles.x);
float cosY = cosf(angles.y); // Y
float sinY = sinf(angles.y);
float cosZ = cosf(angles.z); // Z
float sinZ = sinf(angles.z);
matrix[0][0] = cosY * cosZ + sinY * sinX * sinZ; // Spalte 1
matrix[0][1] = cosX * sinZ;
matrix[0][2] = cosY * sinX * sinZ - cosZ * sinY;
matrix[1][0] = cosZ * sinY * sinX - cosY * sinZ; // Spalte 2
matrix[1][1] = cosX * cosZ;
matrix[1][2] = cosY * cosZ * sinX + sinY * sinZ;
matrix[2][0] = cosX * sinY; // Spalte 3
matrix[2][1] = -sinX;
matrix[2][2] = cosY * cosX;
}
void MatrixYZX(float3 angles, float3x3& matrix)
{
float cosX = cosf(angles.x); // X
float sinX = sinf(angles.x);
float cosY = cosf(angles.y); // Y
float sinY = sinf(angles.y);
float cosZ = cosf(angles.z); // Z
float sinZ = sinf(angles.z);
matrix[0][0] = cosY * cosZ; // Spalte 1
matrix[0][1] = sinZ;
matrix[0][2] = -cosZ * sinY;
matrix[1][0] = sinY * sinX - cosY * cosX * sinZ; // Spalte 2
matrix[1][1] = cosZ * cosX;
matrix[1][2] = cosY * sinX + cosX * sinY * sinZ;
matrix[2][0] = cosX * sinY + cosY * sinZ * sinX; // Spalte 3
matrix[2][1] = -cosZ * sinX;
matrix[2][2] = cosY * cosX - sinY * sinZ * sinX;
}
void MatrixZYX(float3 angles, float3x3& matrix)
{
float cosX = cosf(angles.x); // X
float sinX = sinf(angles.x);
float cosY = cosf(angles.y); // Y
float sinY = sinf(angles.y);
float cosZ = cosf(angles.z); // Z
float sinZ = sinf(angles.z);
matrix[0][0] = cosZ * cosY; // Spalte 1
matrix[0][1] = cosY * sinZ;
matrix[0][2] = -sinY;
matrix[1][0] = cosZ * sinY * sinX - cosX * sinZ; // Spalte 2
matrix[1][1] = cosZ * cosX + sinZ * sinY * sinX;
matrix[1][2] = cosY * sinX;
matrix[2][0] = sinZ * sinX + cosZ * cosX * sinY; // Spalte 3
matrix[2][1] = cosX * sinZ * sinY - cosZ * sinX;
matrix[2][2] = cosY * cosX;
}
void MatrixZXY(float3 angles, float3x3& matrix)
{
float cosX = cosf(angles.x); // X
float sinX = sinf(angles.x);
float cosY = cosf(angles.y); // Y
float sinY = sinf(angles.y);
float cosZ = cosf(angles.z); // Z
float sinZ = sinf(angles.z);
matrix[0][0] = cosZ * cosY - sinZ * sinX * sinY; // Spalte 1
matrix[0][1] = cosY * sinZ + cosZ * sinX * sinY;
matrix[0][2] = -cosX * sinY;
matrix[1][0] = -cosX * sinZ; // Spalte 2
matrix[1][1] = cosZ * cosX;
matrix[1][2] = sinX;
matrix[2][0] = cosZ * sinY + cosY * sinZ * sinX; // Spalte 3
matrix[2][1] = sinZ * sinY - cosZ * cosY * sinX;
matrix[2][2] = cosX * cosY;
}
void PrintMatrix(string name, float3 angles, float3x3& matrix)
{
cout << "-----------------------------" << endl;
cout << name << "-Matrix" << endl;
cout << "-----------------------------" << endl;
cout << matrix[0][0] << " " << matrix[1][0] << " " << matrix[2][0] << " " << endl;
cout << matrix[0][1] << " " << matrix[1][1] << " " << matrix[2][1] << " " << endl;
cout << matrix[0][2] << " " << matrix[1][2] << " " << matrix[2][2] << " " << endl;
cout << "-----------------------------" << endl << endl << endl;
}
void MatrixDecomposeYXZ(float3x3& matrix, float3& angles)
{
angles.x = asinf(-matrix[2][1]); // X
if (cosf(angles.x) > 0.0001) // Not at poles X
{
angles.y = atan2f(matrix[2][0], matrix[2][2]); // Y
angles.z = atan2f(matrix[0][1], matrix[1][1]); // Z
}
else
{
angles.y = 0.0f; // Y
angles.z = atan2f(-matrix[1][0], matrix[0][0]); // Z
}
}
Okay. Thank you. Unfortunatly i will have to go with Euler angles.
@JohnnyCode mentioned affine transformations and alignment transformations.
I will look into that. I guess i have to really learn this stuff to succeed here.