QUYVN/I150
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial commit

Browse Source
This commit is contained in:
QUYVN
2026-07-03 16:37:12 +07:00
commit 63b8c1ea8b
1931 changed files with 640587 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

171
srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Common/Math/Array2i.cs Normal file
View File

@@ -0,0 +1,171 @@
/*
* 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>
/// 2D integer array (equivalent to Eigen::Array2i).
/// Optimized for performance with value type semantics.
/// </summary>
public struct Array2i : IEquatable<Array2i>
{
public int X { get; set; }
public int Y { get; set; }
public Array2i(int x, int y)
{
X = x;
Y = y;
}
public Array2i(Vector2 vector)
{
X = (int)vector.X;
Y = (int)vector.Y;
}
/// <summary>
/// Zero array (0, 0).
/// </summary>
public static Array2i Zero => new(0, 0);
/// <summary>
/// Creates from Vector2 by rounding.
/// </summary>
public static Array2i FromVector2(Vector2 vector)
{
return new Array2i((int)System.Math.Round(vector.X), (int)System.Math.Round(vector.Y));
}
/// <summary>
/// Converts to Vector2.
/// </summary>
public readonly Vector2 ToVector2()
{
return new Vector2(X, Y);
}
/// <summary>
/// Element-wise addition.
/// </summary>
public static Array2i operator +(Array2i left, Array2i right)
{
return new Array2i(left.X + right.X, left.Y + right.Y);
}
/// <summary>
/// Element-wise subtraction.
/// </summary>
public static Array2i operator -(Array2i left, Array2i right)
{
return new Array2i(left.X - right.X, left.Y - right.Y);
}
/// <summary>
/// Element-wise multiplication.
/// </summary>
public static Array2i operator *(Array2i left, int scalar)
{
return new Array2i(left.X * scalar, left.Y * scalar);
}
/// <summary>
/// Element-wise division.
/// </summary>
public static Array2i operator /(Array2i left, int scalar)
{
return new Array2i(left.X / scalar, left.Y / scalar);
}
/// <summary>
/// Element-wise comparison (all elements must be less than).
/// </summary>
public static bool operator <(Array2i left, Array2i right)
{
return left.X < right.X && left.Y < right.Y;
}
/// <summary>
/// Element-wise comparison (all elements must be less than or equal).
/// </summary>
public static bool operator <=(Array2i left, Array2i right)
{
return left.X <= right.X && left.Y <= right.Y;
}
/// <summary>
/// Element-wise comparison (all elements must be greater than).
/// </summary>
public static bool operator >(Array2i left, Array2i right)
{
return left.X > right.X && left.Y > right.Y;
}
/// <summary>
/// Element-wise comparison (all elements must be greater than or equal).
/// </summary>
public static bool operator >=(Array2i left, Array2i right)
{
return left.X >= right.X && left.Y >= right.Y;
}
/// <summary>
/// Equality comparison.
/// </summary>
public static bool operator ==(Array2i left, Array2i right)
{
return left.X == right.X && left.Y == right.Y;
}
/// <summary>
/// Inequality comparison.
/// </summary>
public static bool operator !=(Array2i left, Array2i right)
{
return !(left == right);
}
public readonly bool Equals(Array2i other)
{
return X == other.X && Y == other.Y;
}
public override readonly bool Equals(object? obj)
{
return obj is Array2i other && Equals(other);
}
public override readonly int GetHashCode()
{
return HashCode.Combine(X, Y);
}
public override readonly string ToString()
{
return $"({X}, {Y})";
}
/// <summary>
/// Deconstructs into x and y.
/// </summary>
public readonly void Deconstruct(out int x, out int y)
{
x = X;
y = Y;
}
}

154
srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Common/Math/Array3i.cs Normal file
View File

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

136
srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Common/Math/MathUtils.cs Normal file
View File

@@ -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
}
}