91 lines
2.3 KiB
C#
91 lines
2.3 KiB
C#
namespace RobotNet10.RobotApp.Detection;
|
|
|
|
/// <summary>
|
|
/// Represents a 2D point with additional laser scan metadata
|
|
/// </summary>
|
|
public class Point(double x, double y, double range, double alpha)
|
|
{
|
|
/// <summary>
|
|
/// X coordinate in meters
|
|
/// </summary>
|
|
public double X { get; set; } = x;
|
|
|
|
/// <summary>
|
|
/// Y coordinate in meters
|
|
/// </summary>
|
|
public double Y { get; set; } = y;
|
|
|
|
/// <summary>
|
|
/// Range of point from Lidar sensor in meters
|
|
/// </summary>
|
|
public double Range { get; set; } = range;
|
|
|
|
/// <summary>
|
|
/// Angle of laser beam to the point in radians
|
|
/// </summary>
|
|
public double Alpha { get; set; } = alpha;
|
|
|
|
/// <summary>
|
|
/// Cluster ID assigned by clustering algorithm (-1 if unassigned)
|
|
/// </summary>
|
|
public int Cluster { get; set; } = -1;
|
|
|
|
/// <summary>
|
|
/// Reachability distance for OPTICS algorithm
|
|
/// </summary>
|
|
public double ReachabilityDistance { get; set; } = double.MaxValue;
|
|
|
|
/// <summary>
|
|
/// Whether this point has been processed by the algorithm
|
|
/// </summary>
|
|
public bool Processed { get; set; } = false;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Simple 2D point structure for cluster results
|
|
/// </summary>
|
|
public struct Point2D(double x, double y) : IEquatable<Point2D>, IComparable<Point2D>
|
|
{
|
|
/// <summary>
|
|
/// X coordinate in meters
|
|
/// </summary>
|
|
public double X { get; set; } = x;
|
|
|
|
/// <summary>
|
|
/// Y coordinate in meters
|
|
/// </summary>
|
|
public double Y { get; set; } = y;
|
|
|
|
public readonly bool Equals(Point2D other)
|
|
{
|
|
return Math.Abs(X - other.X) < 3e-3 && Math.Abs(Y - other.Y) < 3e-3;
|
|
}
|
|
|
|
public override readonly bool Equals(object? obj)
|
|
{
|
|
return obj is Point2D other && Equals(other);
|
|
}
|
|
|
|
public override readonly int GetHashCode()
|
|
{
|
|
return HashCode.Combine(X, Y);
|
|
}
|
|
|
|
public readonly int CompareTo(Point2D other)
|
|
{
|
|
if (!X.Equals(other.X))
|
|
return X.CompareTo(other.X);
|
|
return Y.CompareTo(other.Y);
|
|
}
|
|
|
|
public static bool operator ==(Point2D left, Point2D right)
|
|
{
|
|
return left.Equals(right);
|
|
}
|
|
|
|
public static bool operator !=(Point2D left, Point2D right)
|
|
{
|
|
return !left.Equals(right);
|
|
}
|
|
}
|