public static Double BearingBetween(Double bearing1, Double bearing2)
{
Double diff = bearing1.AngularDifference(bearing2) / 2;
Double bearing = 0;
if (CounterClockwiseDifference(bearing1, bearing2) > ClockwiseDifference(bearing1, bearing2))
{
bearing = bearing1.AddDegrees(diff);
}
else
{
bearing = bearing1.SubtractDegrees(diff);
}
return(bearing);
}
public PointCloud2DSlice(PointD origin, Double bearing, PointCloud2D cloud, Double sliceSize)
: this(origin, bearing)
{
Double startAngle = bearing.SubtractDegrees(sliceSize / 2);
int offset = cloud.VectorOffset(startAngle);
int endOffset = cloud.VectorOffset(bearing.AddDegrees(sliceSize / 2));
Double vectorSize = cloud.VectorSize;
for (Double angle = startAngle; offset != endOffset; angle = angle.AddDegrees(vectorSize))
{
offset = cloud.VectorOffset(angle);
if (cloud[offset].Range != 0)
{
Add(cloud[offset].Clone());
}
}
}
/// <summary>
/// return a slice of the given point cloud
/// </summary>
/// <param name="origin"></param>
/// <param name="bearing"></param>
/// <param name="from"></param>
/// <param name="to"></param>
/// <returns></returns>
public PointCloud2DSlice GetPointCloud2DSlice(PointD origin, Double bearing, Double from, Double to)
{
PointCloud2DSlice slice = new PointCloud2DSlice(origin, bearing);
for (Double offset = from / VectorSize; offset != to / VectorSize; offset = offset.AddDegrees(VectorSize))
{
if (this[(int)offset].Range != 0)
{
slice.Add(this[(int)offset]);
}
}
return(slice);
}
/// <summary>
/// REturn reciprocal bearing for given value
/// </summary>
/// <param name="value"></param>
/// <returns></returns>
public static Double Reciprocal(this Double value)
{
return(value.AddDegrees(180));
}