Initial commit

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Common/Math/Array3i.cs

@@ -0,0 +1,154 @@
+/*
+ * 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  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Common.Math;
+
+/// <summary>
+/// 3D integer array (equivalent to Eigen::Array3i).
+/// Optimized for performance with value type semantics.
+/// </summary>
+public struct Array3i : IEquatable<Array3i>
+{
+    public int X { get; set; }
+    public int Y { get; set; }
+    public int Z { get; set; }
+
+    public Array3i(int x, int y, int z)
+    {
+        X = x;
+        Y = y;
+        Z = z;
+    }
+
+    public Array3i(Vector3 vector)
+    {
+        X = (int)vector.X;
+        Y = (int)vector.Y;
+        Z = (int)vector.Z;
+    }
+
+    /// <summary>
+    /// Zero array (0, 0, 0).
+    /// </summary>
+    public static Array3i Zero => new(0, 0, 0);
+
+    /// <summary>
+    /// Creates from Vector3 by rounding.
+    /// </summary>
+    public static Array3i FromVector3(Vector3 vector)
+    {
+        return new Array3i(
+            (int)System.Math.Round(vector.X),
+            (int)System.Math.Round(vector.Y),
+            (int)System.Math.Round(vector.Z));
+    }
+
+    /// <summary>
+    /// Converts to Vector3.
+    /// </summary>
+    public readonly Vector3 ToVector3()
+    {
+        return new Vector3(X, Y, Z);
+    }
+
+    /// <summary>
+    /// Deconstructs into components.
+    /// </summary>
+    public readonly void Deconstruct(out int x, out int y, out int z)
+    {
+        x = X;
+        y = Y;
+        z = Z;
+    }
+
+    // Operators
+    public static Array3i operator +(Array3i left, Array3i right)
+    {
+        return new Array3i(left.X + right.X, left.Y + right.Y, left.Z + right.Z);
+    }
+
+    public static Array3i operator -(Array3i left, Array3i right)
+    {
+        return new Array3i(left.X - right.X, left.Y - right.Y, left.Z - right.Z);
+    }
+
+    public static Array3i operator *(Array3i left, int right)
+    {
+        return new Array3i(left.X * right, left.Y * right, left.Z * right);
+    }
+
+    public static Array3i operator *(int left, Array3i right)
+    {
+        return right * left;
+    }
+
+    public static Array3i operator /(Array3i left, int right)
+    {
+        return new Array3i(left.X / right, left.Y / right, left.Z / right);
+    }
+
+    public static bool operator <(Array3i left, Array3i right)
+    {
+        return left.X < right.X && left.Y < right.Y && left.Z < right.Z;
+    }
+
+    public static bool operator <=(Array3i left, Array3i right)
+    {
+        return left.X <= right.X && left.Y <= right.Y && left.Z <= right.Z;
+    }
+
+    public static bool operator >(Array3i left, Array3i right)
+    {
+        return left.X > right.X && left.Y > right.Y && left.Z > right.Z;
+    }
+
+    public static bool operator >=(Array3i left, Array3i right)
+    {
+        return left.X >= right.X && left.Y >= right.Y && left.Z >= right.Z;
+    }
+
+    public static bool operator ==(Array3i left, Array3i right)
+    {
+        return left.X == right.X && left.Y == right.Y && left.Z == right.Z;
+    }
+
+    public static bool operator !=(Array3i left, Array3i right)
+    {
+        return !(left == right);
+    }
+
+    public readonly bool Equals(Array3i other)
+    {
+        return this == other;
+    }
+
+    public override readonly bool Equals(object? obj)
+    {
+        return obj is Array3i other && Equals(other);
+    }
+
+    public override readonly int GetHashCode()
+    {
+        return HashCode.Combine(X, Y, Z);
+    }
+
+    public override readonly string ToString()
+    {
+        return $"({X}, {Y}, {Z})";
+    }
+}