sim_cinematique_inverse/labo01/Math3D.h

#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;
    }

}