RPiPico-Holonome2023/Geometrie.c
2023-04-28 23:51:43 +02:00

55 lines
1.9 KiB
C

#include "Geometrie.h"
/// @brief Retourne l'angle entre -PI et +PI
/// @param angle
/// @return
float Geometrie_get_angle_normalisee(float angle){
while(angle > M_PI){
angle -= 2* M_PI;
}
while(angle < -M_PI){
angle += 2* M_PI;
}
return angle;
}
/// @brief Indique si un angle est compris entre deux angles. Les angles doivent être entre -PI et PI.
/// @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(float angle, float angle_min, float 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(float angle1_min, float angle1_max, float angle2_min, float 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;
}