sim_cinematique_inverse/labo01/Math3D.h

140 lines
4.1 KiB
C
Raw Normal View History

2024-04-01 17:31:21 -04:00
#pragma once
/**
* @file Math3D.h
*
* @brief Fonctions pour l'intinialisation et la manipulation de matrices de
* rotation, des matrices de transformations en coordonnées homogènes et les
* vecteurs 3D.
*
* Nom: William Nolin
* Code permanent : NOLW76060101
* Email : william.nolin.1@ens.etsmtl.ca
*
*/
#include "Matrix.h"
#include "Vector.h"
#include "Operators.h"
#ifndef _USE_MATH_DEFINES
#define _USE_MATH_DEFINES
#endif
#include <math.h>
namespace gti320 {
// Deux types de vecteurs 3D considérés ici
typedef Vector<double, 3> Vector3d;
typedef Vector<float, 3> Vector3f;
// Dans le cadre de ce projet, nous considérons seulement deux
// cas :
//
// - les rotations
// - les translations
//
// Deux types de matrices en coordonnées homogèes :
typedef Matrix<double, 4, 4, ColumnStorage> Matrix4d;
typedef Matrix<float, 4, 4, ColumnStorage> Matrix4f;
//
// Deux types de matrices pour les rotations
typedef Matrix<double, 3, 3, ColumnStorage> Matrix3d;
typedef Matrix<float, 3, 3, ColumnStorage> Matrix3f;
/**
* Initialise et retourne la matrice identité
*/
template<>
inline void Matrix4d::setIdentity() {
this->setZero();
for (int i = 0; i < 4; i++) {
(*this)(i, i) = 1;
}
}
/**
* Calcul de la matrice inverse, SPÉCIALISÉ pour le cas d'une matrice de
* transformation en coordonnées homogènes.
*/
template<>
inline Matrix4d Matrix4d::inverse() const {
auto transposed_rotation = this->block(0, 0, 3, 3).transpose<double, 3, 3, ColumnStorage>();
auto translation = Vector<double, 3>();
for (int i = 0; i < 3; i++) {
translation(i) = (*this)(i, 3);
}
auto result = Matrix4d();
result(3, 3) = 1;
// Applique la matrice de rotation
result.block(0, 0, 3, 3) = transposed_rotation;
// Applique le vecteur de translation
auto inverted_translation = (-1.0 * transposed_rotation) * translation;
auto trans_block = result.block(0, 3, 3, 1);
trans_block(0, 0) = inverted_translation(0);
trans_block(1, 0) = inverted_translation(1);
trans_block(2, 0) = inverted_translation(2);
return result;
}
/**
* Calcul de la matrice inverse, SPÉCIALISÉ pour le cas d'une matrice de rotation.
*
* (vous pouvez supposer qu'il s'agit d'une matrice de rotation)
*/
template<>
inline Matrix3d Matrix3d::inverse() const {
return this->transpose<double, 3, 3, ColumnStorage>();
}
/**
* Multiplication d'une matrice 4x4 avec un vecteur 3D la matrice
* représente une transformation en coordonnées homogène.
*/
template<typename _Scalar>
Vector<_Scalar, 3> operator*(const Matrix<_Scalar, 4, 4, ColumnStorage> &A, const Vector<_Scalar, 3> &v) {
auto homogeneous = Vector<_Scalar, 4>(v);
homogeneous(3) = 1;
Vector<_Scalar, 4> transformed = A * homogeneous;
return Vector<_Scalar, 3>(transformed);
}
/**
* Initialise et retourne la matrice de rotation définie par les angles
* d'Euler XYZ exprimés en radians.
*
* La matrice doit correspondre au produit : Rz*Ry*Rx.
*/
template<typename _Scalar>
static Matrix<_Scalar, 3, 3> makeRotation(_Scalar x, _Scalar y, _Scalar z) {
double sin_theta = std::sin(x);
double cos_theta = std::cos(x);
double sin_phi = std::sin(y);
double cos_phi = std::cos(y);
double sin_psi = std::sin(z);
double cos_psi = std::cos(z);
auto m = Matrix<_Scalar, 3, 3>();
m(0, 0) = cos_phi * cos_psi;
m(1, 0) = cos_phi * sin_psi;
m(2, 0) = -sin_phi;
m(0, 1) = sin_theta * sin_phi * cos_psi - cos_theta * sin_psi;
m(1, 1) = sin_theta * sin_phi * sin_psi + cos_theta * cos_psi;
m(2, 1) = sin_theta * cos_phi;
m(0, 2) = cos_theta * sin_phi * cos_psi + sin_theta * sin_psi;
m(1, 2) = cos_theta * sin_phi * sin_psi - sin_theta * cos_psi;
m(2, 2) = cos_theta * cos_phi;
return m;
}
}