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>
/// Retourne les coordonnées d'une position initiale modifiée par une prise de direction
/// </summary>
/// <param name="startPosition">Position de départ</param>
/// <param name="direction">Direction suivie</param>
/// <returns>Coordonnées du point</returns>
public static Position GetDestination(Position startPosition, Direction direction)
{
AnglePosition endAngle = startPosition.Angle + direction.angle;
double x = startPosition.Coordinates.X + endAngle.Cos * direction.distance;
double y = startPosition.Coordinates.Y - endAngle.Sin * direction.distance;
return(new Position(endAngle, new RealPoint(x, y)));
}
/// <summary>
/// Retourne si l'angle est compris entre les angles données, c'est à dire qu'il se situe dans l'arc de cercle partant de startAngle vers endAngle
/// Exemples :
/// 150° est entre 130° et 160° mais pas entre 160° et 130°
/// 10° est entre 350° et 50° mais pas entre 50° et 350°
/// </summary>
/// <param name="startAngle">Angle de départ</param>
/// <param name="endAngle">Angle d'arrivée</param>
/// <returns>Vrai si l'angle est compris entre les deux angles</returns>
public bool IsOnArc(AnglePosition startAngle, AnglePosition endAngle)
{
bool ok;
if (startAngle.InPositiveDegrees < endAngle.InPositiveDegrees)
{
ok = this.InPositiveDegrees >= startAngle.InPositiveDegrees && this.InPositiveDegrees <= endAngle.InPositiveDegrees;
}
else
{
ok = this.InPositiveDegrees == startAngle.InPositiveDegrees || this.InPositiveDegrees == endAngle.InPositiveDegrees || !this.IsOnArc(endAngle, startAngle);
}
return(ok);
}
public static AnglePosition CenterLongArc(AnglePosition a1, AnglePosition a2)
{
AnglePosition a;
if (Math.Abs(a1.InPositiveDegrees - a2.InPositiveDegrees) > 180)
{
a = new AnglePosition((a1.InPositiveDegrees + a2.InPositiveDegrees) / 2);
}
else
{
a = new AnglePosition((a1.InPositiveDegrees + a2.InPositiveDegrees) / 2 + 180);
}
return(a);
}
public static AnglePosition Center(AnglePosition startAngle, AnglePosition endAngle)
{
AnglePosition a;
if (startAngle.InPositiveDegrees < endAngle.InPositiveDegrees)
{
a = new AnglePosition((startAngle.InPositiveDegrees + endAngle.InPositiveDegrees) / 2);
}
else
{
a = new AnglePosition((startAngle.InPositiveDegrees + endAngle.InPositiveDegrees) / 2 + 180);
}
return(a);
}
/// <summary>
/// Copie une autre position
/// </summary>
/// <param name="position">Position à copier</param>
public void Copy(Position position)
{
Angle = position.Angle;
Coordinates.Set(position.Coordinates.X, position.Coordinates.Y);
}
/// <summary>
/// Fait tourner l'angle de l'angle choisi
/// </summary>
/// <param name="angle">Angle à tourner</param>
public void Turn(AngleDelta angle)
{
Angle += angle;
}
/// <summary>
/// Construit une position selon les paramètres
/// </summary>
/// <param name="angle">Angle de départ</param>
/// <param name="coordinates">Coordonnées de départ</param>
public Position(AnglePosition angle, RealPoint coordinates)
{
Angle = angle;
Coordinates = new RealPoint(coordinates);
}
/// <summary>
/// Constructeur par copie
/// </summary>
/// <param name="other">Position à copier</param>
public Position(Position other)
{
Angle = other.Angle;
Coordinates = new RealPoint(other.Coordinates);
}
/// <summary>
/// Constructeur par défaut
/// Angle de 0° et Coordonnées (0, 0)
/// </summary>
public Position()
{
Angle = new AnglePosition();
Coordinates = new RealPoint();
}
