RPiPico-Holonome2023/Geometrie.c

#include "Geometrie.h"


/// @brief Retourne l'angle entre -PI et +PI
/// @param angle 
/// @return 
double Geometrie_get_angle_normalisee(double angle){
    while(angle > M_PI){
        angle -= 2* M_PI;
    }
    while(angle < 2* -M_PI){
        angle += 2* M_PI;
    }
    return angle;
}

/// @brief Indique si un angle est compris entre deux angles
/// @param angle : angle à comparer
/// @param angle_min : début de la fourchette
/// @param angle_max : fin de la fourchette
/// @return 1 si l'angle est compris entre min et max, 0 sinon
unsigned int Geometrie_compare_angle(double angle, double angle_min, double angle_max){
    angle = Geometrie_get_angle_normalisee(angle);
    angle_min = Geometrie_get_angle_normalisee(angle_min);
    angle_max = Geometrie_get_angle_normalisee(angle_max);

    if(angle_min > angle_max){
        // cas où la fourchette comprend -PI.
        if( (angle > angle_min) || (angle < angle_max)){
            return 1;
        }
        return 0;
    }else{
        // Cas normal
        if( (angle > angle_min) && (angle < angle_max)){
            return 1;
        }
        return 0;
    }
}

/// @brief Indique si les deux plages d'angle se recoupent
/// @param angle1_min Début de la première plage
/// @param angle1_max Fin de la première plage
/// @param angle2_min Début de la seconde plage
/// @param angle2_max Fin de la seconde plage
/// @return 1 si les deux plages s'intersectent, 0 sinon
unsigned int Geometrie_intersecte_plage_angle(double angle1_min, double angle1_max, double angle2_min, double angle2_max){
    // Pour que les plages s'intersectent, soit :
    // * angle1_min est compris entre angle2_min et angle2_max
    // * angle1_max est compris entre angle2_min et angle2_max
    // * angle2_min et angle2_max sont compris entre angle1_min et angle1_max (tester angle2_min ou angle2_max est suffisant)
    if(Geometrie_compare_angle(angle1_min, angle2_min, angle2_max) ||
        Geometrie_compare_angle(angle1_max, angle2_min, angle2_max) ||
        Geometrie_compare_angle(angle2_min, angle1_min, angle1_max)){
            return 1;
    }
    return 0;
}