/// <summary>
///
/// </summary>
/// <param name="radii"></param>
/// <returns></returns>
public static List <LidarPoint> ToLidarPoints(this int?[] radii, int maxDistanceFromOrigin = 8000)
{
var lidarPoints = new List <LidarPoint>();
for (int i = 0; i < radii.Length; i++)
{
if (radii[i] != null && radii[i] < maxDistanceFromOrigin)
{
var point = new LidarPoint(radii[i].Value / 1000.0, (i / 4.0) - 45.0, true);
point.id = i;
lidarPoints.Add(point);
}
}
return(lidarPoints);
}
public double DistanceBetween(LidarPoint p2)
{
return(Math.Sqrt((this.X - p2.X) * (this.X - p2.X) + (this.Y - p2.Y) * (this.Y - p2.Y)));
}