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

/*
 * 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})";
    }
}