Initial commit

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Sensor/RangefinderPoint.cs

@@ -0,0 +1,136 @@
+/*
+ * Copyright 2018 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+using CartographerSharp.Transform;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Sensor;
+
+/// <summary>
+/// Stores 3D position of a point observed by a rangefinder sensor.
+/// </summary>
+public struct RangefinderPoint(Vector3 position)
+{
+    public Vector3 Position { get; set; } = position;
+
+    public static RangefinderPoint operator *(Rigid3f transform, RangefinderPoint point)
+    {
+        return new RangefinderPoint(transform.TransformPoint(point.Position));
+    }
+
+    public static bool operator ==(RangefinderPoint lhs, RangefinderPoint rhs)
+    {
+        return lhs.Position == rhs.Position;
+    }
+
+    public static bool operator !=(RangefinderPoint lhs, RangefinderPoint rhs)
+    {
+        return !(lhs == rhs);
+    }
+
+    public override readonly bool Equals(object? obj)
+    {
+        return obj is RangefinderPoint other && this == other;
+    }
+
+    public override readonly int GetHashCode()
+    {
+        return Position.GetHashCode();
+    }
+
+    /// <summary>
+    /// Converts from proto representation.
+    /// </summary>
+    public static RangefinderPoint FromProto(RangefinderPoint proto)
+    {
+        return new RangefinderPoint(proto.Position);
+    }
+
+    /// <summary>
+    /// Converts to proto representation.
+    /// </summary>
+    public readonly RangefinderPoint ToProto()
+    {
+        return new RangefinderPoint(Position);
+    }
+}
+
+/// <summary>
+/// Stores 3D position of a point with its relative measurement time.
+/// See PointCloud for more details.
+/// </summary>
+public struct TimedRangefinderPoint(Vector3 position, double time)
+{
+    public Vector3 Position { get; set; } = position;
+    public double Time { get; set; } = time;
+
+    public static TimedRangefinderPoint operator *(Rigid3f transform, TimedRangefinderPoint point)
+    {
+        return new TimedRangefinderPoint(transform.TransformPoint(point.Position), point.Time);
+    }
+
+    public static bool operator ==(TimedRangefinderPoint lhs, TimedRangefinderPoint rhs)
+    {
+        return lhs.Position == rhs.Position && lhs.Time == rhs.Time;
+    }
+
+    public static bool operator !=(TimedRangefinderPoint lhs, TimedRangefinderPoint rhs)
+    {
+        return !(lhs == rhs);
+    }
+
+    public override readonly bool Equals(object? obj)
+    {
+        return obj is TimedRangefinderPoint other && this == other;
+    }
+
+    public override readonly int GetHashCode()
+    {
+        return HashCode.Combine(Position, Time);
+    }
+
+    /// <summary>
+    /// Converts from proto representation.
+    /// </summary>
+    public static TimedRangefinderPoint FromProto(TimedRangefinderPoint proto)
+    {
+        return new TimedRangefinderPoint(proto.Position, proto.Time);
+    }
+
+    /// <summary>
+    /// Converts to proto representation.
+    /// </summary>
+    public readonly TimedRangefinderPoint ToProto()
+    {
+        return new TimedRangefinderPoint(Position, Time);
+    }
+
+    /// <summary>
+    /// Converts to RangefinderPoint (drops time information).
+    /// </summary>
+    public readonly RangefinderPoint ToRangefinderPoint()
+    {
+        return new RangefinderPoint(Position);
+    }
+
+    /// <summary>
+    /// Creates TimedRangefinderPoint from RangefinderPoint with time.
+    /// </summary>
+    public static TimedRangefinderPoint FromRangefinderPoint(RangefinderPoint point, double time)
+    {
+        return new TimedRangefinderPoint(point.Position, time);
+    }
+}