Initial commit

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

@@ -0,0 +1,110 @@
+/*
+ * 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 System.Collections;
+
+namespace CartographerSharp.Mapping.D2D;
+
+/// <summary>
+/// Iterates in row-major order through a range of xy-indices.
+/// </summary>
+/// <remarks>
+/// Constructs a new iterator for the specified range.
+/// </remarks>
+public class XYIndexRangeIterator(Array2i minXYIndex, Array2i maxXYIndex) : IEnumerator<Array2i>
+{
+    private readonly Array2i _minXYIndex = minXYIndex;
+    private readonly Array2i _maxXYIndex = maxXYIndex;
+    // Match C++: IEnumerator starts before first element
+    // Initialize to position before first element (minXYIndex with X decremented by 1)
+    private Array2i _xyIndex = new Array2i(minXYIndex.X - 1, minXYIndex.Y);
+
+    /// <summary>
+    /// Constructs a new iterator for everything contained in 'cell_limits'.
+    /// </summary>
+    public XYIndexRangeIterator(CellLimits cellLimits)
+        : this(Array2i.Zero, new Array2i(cellLimits.NumXCells - 1, cellLimits.NumYCells - 1))
+    {
+    }
+
+    public Array2i Current => _xyIndex;
+
+    object IEnumerator.Current => Current;
+
+    // Match C++ operator++() logic
+    public bool MoveNext()
+    {
+        // Match C++: if (xy_index_.x() < max_xy_index_.x()) { ++xy_index_.x(); }
+        // else { xy_index_.x() = min_xy_index_.x(); ++xy_index_.y(); }
+        if (_xyIndex.X < _maxXYIndex.X)
+        {
+            _xyIndex = new Array2i(_xyIndex.X + 1, _xyIndex.Y);
+        }
+        else
+        {
+            _xyIndex = new Array2i(_minXYIndex.X, _xyIndex.Y + 1);
+        }
+        
+        // Check if we've reached the end (match C++ end() condition: min_x, max_y + 1)
+        // End condition: Y > maxY || (Y == maxY && X > maxX)
+        if (_xyIndex.Y > _maxXYIndex.Y || 
+            (_xyIndex.Y == _maxXYIndex.Y && _xyIndex.X > _maxXYIndex.X))
+        {
+            return false;
+        }
+        return true;
+    }
+
+    public void Reset()
+    {
+        // Match C++: Reset to position before first element
+        _xyIndex = new Array2i(_minXYIndex.X - 1, _minXYIndex.Y);
+    }
+
+    public void Dispose()
+    {
+        // Nothing to dispose
+        GC.SuppressFinalize(this);
+    }
+
+    public bool IsEnd()
+    {
+        return _xyIndex.Y > _maxXYIndex.Y || 
+               (_xyIndex.Y == _maxXYIndex.Y && _xyIndex.X > _maxXYIndex.X);
+    }
+}
+
+/// <summary>
+/// Range of XY indices for iteration.
+/// </summary>
+public class XYIndexRange(Array2i minXYIndex, Array2i maxXYIndex) : IEnumerable<Array2i>
+{
+    public XYIndexRange(CellLimits cellLimits)
+        : this(Array2i.Zero, new Array2i(cellLimits.NumXCells - 1, cellLimits.NumYCells - 1))
+    {
+    }
+
+    public IEnumerator<Array2i> GetEnumerator()
+    {
+        return new XYIndexRangeIterator(minXYIndex, maxXYIndex);
+    }
+
+    IEnumerator IEnumerable.GetEnumerator()
+    {
+        return GetEnumerator();
+    }
+}