#include <math.h>
#include "configuration-2d.h"

/*
    Retourne un angle équivalent à <angle>, mais appartenant à
    l'intervalle [-PI, PI).
*/

static double normaliser_angle(double angle)
{
  double r;

  r = fmod(angle, 2.0 * M_PI);
  if (r < -M_PI)
    r += 2.0 * M_PI;
  else
    if (r >= M_PI)
      r -= 2.0 * M_PI;

  return r;
}

/*
    Initialise la configuration *<c> avec la position (<x>, y>) et
    l'orientation <theta>.
 */

void init_configuration(configuration *c, double x, double y, double theta)
{
  c -> x = x;
  c -> y = y;
  c -> theta = normaliser_angle(theta);
}

/*
    Met à jour la configuration *<c> après un déplacement de longueur <l>
    dans la direction courante.
 */

void deplacer_configuration(configuration *c, double l)
{
  c -> x -= l * sin(c -> theta);
  c -> y += l * cos(c -> theta);
}

/*
    Met à jour la configuration *<c> après une rotation d'angle <angle>.
 */

void tourner_configuration(configuration *c, double angle)
{
  c -> theta = normaliser_angle(c -> theta + angle);
}