/// <summary>
/// Retourne la direction (angle et distance) à suivre pour arriver à un point donné en partant d'une position précise (coordonnées et angle)
/// </summary>
/// <param name="startPosition">Position de départ</param>
/// <param name="endPoint">Coordonnées d'arrivée</param>
/// <returns>Direction à suivre</returns>
public static Direction GetDirection(Position startPosition, RealPoint endPoint)
{
Direction result = new Direction();
result.distance = startPosition.Coordinates.Distance(endPoint);
double angleCalc = 0;
// Deux points sur le même axe vertical : 90° ou -90° selon le point le plus haut
if (endPoint.X == startPosition.Coordinates.X)
{
angleCalc = Math.PI / 2;
if (endPoint.Y > startPosition.Coordinates.Y)
{
angleCalc = -angleCalc;
}
}
// Deux points sur le même axe horizontal : 0° ou 180° selon le point le plus à gauche
else if (endPoint.Y == startPosition.Coordinates.Y)
{
angleCalc = Math.PI;
if (endPoint.X > startPosition.Coordinates.X)
{
angleCalc = 0;
}
}
// Cas général : Calcul de l'angle
else
{
angleCalc = Math.Acos((endPoint.X - startPosition.Coordinates.X) / result.distance);
if (endPoint.Y > startPosition.Coordinates.Y)
{
angleCalc = -angleCalc;
}
}
// Prendre en compte l'angle initial
AngleDelta angle = new AngleDelta(angleCalc, AngleType.Radian);
angle = angle + startPosition.Angle;
result.angle = angle.Modulo();
return(result);
}
private void _manager_NewMeasure(List <PepperlManager.PepperlPoint> measure, Geometry.AnglePosition startAngle, Geometry.AngleDelta resolution)
{
List <RealPoint> points = ValuesToPositions(measure.Select(p => p.distance).ToList(), startAngle, resolution, false, 0, 5000, _position);
_lastMeasure = new List <RealPoint>(points);
_lockMeasure?.ReleaseMutex();
OnNewMeasure(points);
}
/// <summary>
/// Fait tourner l'angle de l'angle choisi
/// </summary>
/// <param name="angle">Angle à tourner</param>
public void Turn(AngleDelta angle)
{
Angle += angle;
}
