83 lines
3.0 KiB
C
83 lines
3.0 KiB
C
|
#include "Geometrie.h"
|
||
|
|
#include "math.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 A partir de l'orientation actuelle du robot et de l'orientation souhaitée,
|
||
|
|
/// donne l'angle consigne pour limiter les rotations inutiles.
|
||
|
|
/// Tous les angles sont en radian
|
||
|
|
/// @param angle_depart
|
||
|
|
/// @param angle_souhaite
|
||
|
|
/// @return angle_optimal en radian
|
||
|
|
float Geometrie_get_angle_optimal(float angle_depart, float angle_souhaite){
|
||
|
|
while((angle_depart - angle_souhaite) > M_PI){
|
||
|
|
angle_souhaite += 2* M_PI;
|
||
|
|
}
|
||
|
|
while((angle_depart - angle_souhaite) < -M_PI){
|
||
|
|
angle_souhaite -= 2* M_PI;
|
||
|
|
}
|
||
|
|
return angle_souhaite;
|
||
|
|
}
|
||
|
|
|
||
|
|
/// @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;
|
||
|
|
}
|
||
|
|
|
||
|
|
/// @brief Déplace un point de la distance indiquée en se servant de l'angle de la position donnée.
|
||
|
|
struct position_t Geometrie_deplace(struct position_t position_depart, float distance_mm){
|
||
|
|
struct position_t position_arrivée;
|
||
|
|
position_arrivée.angle_radian = position_depart.angle_radian;
|
||
|
|
position_arrivée.x_mm = position_depart.x_mm + cosf(position_depart.angle_radian) * distance_mm;
|
||
|
|
position_arrivée.y_mm = position_depart.y_mm + sinf(position_depart.angle_radian) * distance_mm;
|
||
|
|
|
||
|
|
return position_arrivée;
|
||
|
|
}
|