Initial commit

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/2D/MapLimits.cs

@@ -0,0 +1,132 @@
+/*
+ * Copyright 2016 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.Common.Math;
+using CartographerSharp.Models.Transform;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Mapping.D2D;
+
+/// <summary>
+/// Defines the limits of a grid map.
+/// </summary>
+public class MapLimits
+{
+    private readonly double _resolution;
+    private readonly Vector2 _max;
+    private readonly CellLimits _cellLimits;
+
+    public MapLimits(double resolution, Vector2 max, CellLimits cellLimits)
+    {
+        if (resolution <= 0.0)
+        {
+            throw new ArgumentException("Resolution must be positive", nameof(resolution));
+        }
+        if (cellLimits.NumXCells <= 0 || cellLimits.NumYCells <= 0)
+        {
+            throw new ArgumentException("Cell limits must be positive", nameof(cellLimits));
+        }
+
+        _resolution = resolution;
+        _max = max;
+        _cellLimits = cellLimits;
+    }
+
+    /// <summary>
+    /// Creates from proto representation.
+    /// Match C++: explicit MapLimits(const proto::MapLimits& map_limits)
+    /// </summary>
+    public MapLimits(Models.Mapping.MapLimits proto)
+    {
+        _resolution = proto.Resolution;
+        _max = new Vector2(proto.Max.X, proto.Max.Y);
+        // Match C++: cell_limits_(map_limits.cell_limits())
+        _cellLimits = new CellLimits(proto.CellLimits);
+    }
+
+    /// <summary>
+    /// Returns the cell size in meters. All cells are square and the resolution is
+    /// the length of one side.
+    /// </summary>
+    public double Resolution => _resolution;
+
+    /// <summary>
+    /// Returns the corner of the limits, i.e., all pixels have positions with
+    /// smaller coordinates.
+    /// </summary>
+    public Vector2 Max => _max;
+
+    /// <summary>
+    /// Returns the limits of the grid in number of cells.
+    /// </summary>
+    public CellLimits CellLimits => _cellLimits;
+
+    /// <summary>
+    /// Returns the index of the cell containing the 'point' which may be outside
+    /// the map, i.e., negative or too large indices that will return false for
+    /// Contains().
+    /// </summary>
+    public Array2i GetCellIndex(Vector2 point)
+    {
+        // Index values are row major and the top left has (0, 0)
+        // and contains (centered_max_x, centered_max_y). We need to flip and rotate.
+        // Match C++: common::RoundToInt(x) = std::lround(x) - rounds to nearest long, then casts to int
+        // std::lround rounds to nearest integer using current rounding mode (default: round half away from zero)
+        // In C#, Math.Round with MidpointRounding.AwayFromZero matches std::lround behavior
+        // CRITICAL: Use std::lround equivalent - Math.Round with MidpointRounding.AwayFromZero
+        // But std::lround actually uses "round half to even" (banker's rounding) by default in C++11+
+        // However, C++ code uses common::RoundToInt which is std::lround, and std::lround uses current rounding mode
+        // In practice, std::lround with default rounding mode rounds half away from zero
+        // So Math.Round with MidpointRounding.AwayFromZero should match
+        var x = (int)Math.Round((_max.Y - point.Y) / _resolution - 0.5, MidpointRounding.AwayFromZero);
+        var y = (int)Math.Round((_max.X - point.X) / _resolution - 0.5, MidpointRounding.AwayFromZero);
+        return new Array2i(x, y);
+    }
+
+    /// <summary>
+    /// Returns the center of the cell at 'cell_index'.
+    /// </summary>
+    public Vector2 GetCellCenter(Array2i cellIndex)
+    {
+        return new Vector2(
+            (_max.X - Resolution * (cellIndex.Y + 0.5)),
+            (_max.Y - Resolution * (cellIndex.X + 0.5))
+        );
+    }
+
+    /// <summary>
+    /// Returns true if the grid contains 'cell_index'.
+    /// </summary>
+    public bool Contains(Array2i cellIndex)
+    {
+        return cellIndex.X >= 0 && cellIndex.Y >= 0 &&
+               cellIndex.X < _cellLimits.NumXCells &&
+               cellIndex.Y < _cellLimits.NumYCells;
+    }
+
+    /// <summary>
+    /// Converts to proto representation.
+    /// </summary>
+    public Models.Mapping.MapLimits ToProto()
+    {
+        var cellLimitsProto = _cellLimits.ToProto();
+        return new Models.Mapping.MapLimits(
+            _resolution,
+            new Vector2d(_max.X, _max.Y),
+            new Models.Mapping.CellLimits(cellLimitsProto.NumXCells, cellLimitsProto.NumYCells)
+        );
+    }
+}