137 lines
4.2 KiB
C#
137 lines
4.2 KiB
C#
/*
|
|
* 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
|
|
}
|
|
}
|