Initial commit

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

@@ -0,0 +1,136 @@
+/*
+ * 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;
+using SysNum = System.Numerics;
+
+namespace CartographerSharp.Common.Math;
+
+/// <summary>
+/// Common mathematical utilities for Cartographer.
+/// </summary>
+public static class MathUtils
+{
+    /// <summary>
+    /// Clamps 'value' to be in the range ['min', 'max'].
+    /// </summary>
+    public static T Clamp<T>(T value, T min, T max) where T : IComparable<T>
+    {
+        if (value.CompareTo(max) > 0)
+        {
+            return max;
+        }
+        if (value.CompareTo(min) < 0)
+        {
+            return min;
+        }
+        return value;
+    }
+
+    /// <summary>
+    /// Calculates 'base'^'exponent'.
+    /// </summary>
+    public static T Power<T>(T baseValue, int exponent) where T : SysNum.IMultiplyOperators<T, T, T>, SysNum.IMultiplicativeIdentity<T, T>
+    {
+        if (exponent == 0)
+        {
+            return T.MultiplicativeIdentity;
+        }
+        if (exponent < 0)
+        {
+            throw new ArgumentException("Exponent must be non-negative", nameof(exponent));
+        }
+        
+        T result = baseValue;
+        for (int i = 1; i < exponent; i++)
+        {
+            result *= baseValue;
+        }
+        return result;
+    }
+
+    /// <summary>
+    /// Calculates a^2.
+    /// </summary>
+    public static T Pow2<T>(T a) where T : SysNum.IMultiplyOperators<T, T, T>
+    {
+        return a * a;
+    }
+
+    /// <summary>
+    /// Converts from degrees to radians.
+    /// </summary>
+    public static double DegToRad(double deg)
+    {
+        return System.Math.PI * deg / 180.0;
+    }
+
+    /// <summary>
+    /// Converts from radians to degrees.
+    /// </summary>
+    public static double RadToDeg(double rad)
+    {
+        return 180.0 * rad / System.Math.PI;
+    }
+
+    /// <summary>
+    /// Bring the 'difference' between two angles into [-pi; pi].
+    /// </summary>
+    public static T NormalizeAngleDifference<T>(T difference) where T : SysNum.IFloatingPoint<T>
+    {
+        var kPi = T.CreateChecked(System.Math.PI);
+        var twoPi = T.CreateChecked(2.0 * System.Math.PI);
+        
+        while (difference > kPi)
+        {
+            difference -= twoPi;
+        }
+        while (difference < -kPi)
+        {
+            difference += twoPi;
+        }
+        return difference;
+    }
+
+    /// <summary>
+    /// Calculates atan2 for a 2D vector.
+    /// </summary>
+    public static double Atan2(RobotNet10.Shared.Numbers.Vector2 vector)
+    {
+        return System.Math.Atan2(vector.Y, vector.X);
+    }
+
+    /// <summary>
+    /// Calculates quaternion product: z * w.
+    /// </summary>
+    /// <param name="z">First quaternion as array [w, x, y, z]</param>
+    /// <param name="w">Second quaternion as array [w, x, y, z]</param>
+    /// <param name="zw">Output quaternion as array [w, x, y, z]</param>
+    public static void QuaternionProduct(ReadOnlySpan<double> z, ReadOnlySpan<double> w, Span<double> zw)
+    {
+        if (z.Length < 4 || w.Length < 4 || zw.Length < 4)
+        {
+            throw new ArgumentException("Quaternion arrays must have at least 4 elements");
+        }
+
+        // z = [w, x, y, z] in array, but Quaternion uses [x, y, z, w] order
+        // So z[0] = w, z[1] = x, z[2] = y, z[3] = z
+        zw[0] = z[0] * w[0] - z[1] * w[1] - z[2] * w[2] - z[3] * w[3]; // w component
+        zw[1] = z[0] * w[1] + z[1] * w[0] + z[2] * w[3] - z[3] * w[2]; // x component
+        zw[2] = z[0] * w[2] - z[1] * w[3] + z[2] * w[0] + z[3] * w[1]; // y component
+        zw[3] = z[0] * w[3] + z[1] * w[2] - z[2] * w[1] + z[3] * w[0]; // z component
+    }
+}