140 lines
4.1 KiB
C++
140 lines
4.1 KiB
C++
#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 où 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;
|
|
}
|
|
|
|
}
|