Initial commit
This commit is contained in:
@@ -0,0 +1,74 @@
|
||||
/*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
namespace CartographerSharp.Common;
|
||||
|
||||
/// <summary>
|
||||
/// Signals when a sample should be taken from a stream of data to select a
|
||||
/// uniformly distributed fraction of the data.
|
||||
/// </summary>
|
||||
public class FixedRatioSampler
|
||||
{
|
||||
/// <summary>
|
||||
/// Sampling occurs if the proportion of samples to pulses drops below this number.
|
||||
/// </summary>
|
||||
private readonly double _ratio;
|
||||
|
||||
private long _numPulses = 0;
|
||||
private long _numSamples = 0;
|
||||
|
||||
/// <summary>
|
||||
/// Creates a fixed ratio sampler with the given ratio.
|
||||
/// </summary>
|
||||
/// <param name="ratio">Ratio between 0.0 and 1.0. Sampling occurs if the proportion of samples to pulses drops below this number.</param>
|
||||
public FixedRatioSampler(double ratio)
|
||||
{
|
||||
if (ratio < 0.0 || ratio > 1.0)
|
||||
{
|
||||
throw new ArgumentException($"Ratio must be between 0.0 and 1.0, got {ratio}", nameof(ratio));
|
||||
}
|
||||
|
||||
if (ratio == 0.0)
|
||||
{
|
||||
// Warning: FixedRatioSampler is dropping all data
|
||||
}
|
||||
|
||||
_ratio = ratio;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Returns true if this pulse should result in a sample.
|
||||
/// </summary>
|
||||
public bool Pulse()
|
||||
{
|
||||
_numPulses++;
|
||||
if (_numSamples / _numPulses < _ratio)
|
||||
{
|
||||
_numSamples++;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Returns a debug string describing the current ratio of samples to pulses.
|
||||
/// </summary>
|
||||
public string DebugString()
|
||||
{
|
||||
var percentage = _numPulses > 0 ? 100.0 * _numSamples / _numPulses : 0.0;
|
||||
return $"{_numSamples} ({percentage:F2}%)";
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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})";
|
||||
}
|
||||
}
|
||||
@@ -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
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,220 @@
|
||||
/*
|
||||
* Copyright 2018 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.
|
||||
*/
|
||||
|
||||
namespace CartographerSharp.Common.Threading;
|
||||
|
||||
/// <summary>
|
||||
/// Task state enumeration.
|
||||
/// </summary>
|
||||
public enum TaskState
|
||||
{
|
||||
New,
|
||||
Dispatched,
|
||||
DependenciesCompleted,
|
||||
Running,
|
||||
Completed
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Represents a task that can be executed by a thread pool.
|
||||
/// Tasks can have dependencies on other tasks.
|
||||
/// </summary>
|
||||
public class Task : IDisposable
|
||||
{
|
||||
private readonly Lock _lock = new();
|
||||
private Action? _workItem;
|
||||
private ThreadPoolInterface? _threadPoolToNotify;
|
||||
private TaskState _state = TaskState.New;
|
||||
private int _uncompletedDependencies;
|
||||
private readonly HashSet<Task> _dependentTasks = [];
|
||||
private bool _disposed = false;
|
||||
|
||||
/// <summary>
|
||||
/// Gets the current state of the task.
|
||||
/// </summary>
|
||||
public TaskState GetState()
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
return _state;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Sets the work item to execute. State must be 'New'.
|
||||
/// </summary>
|
||||
public void SetWorkItem(Action workItem)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(workItem);
|
||||
|
||||
lock (_lock)
|
||||
{
|
||||
if (_state != TaskState.New)
|
||||
{
|
||||
throw new InvalidOperationException($"Cannot set work item when state is {_state}");
|
||||
}
|
||||
_workItem = workItem;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Adds a dependency on another task. State must be 'New'.
|
||||
/// 'dependency' may be null, in which case it is assumed completed.
|
||||
/// </summary>
|
||||
public void AddDependency(WeakReference<Task>? dependency)
|
||||
{
|
||||
Task? depTask = null;
|
||||
lock (_lock)
|
||||
{
|
||||
if (_state != TaskState.New)
|
||||
{
|
||||
throw new InvalidOperationException($"Cannot add dependency when state is {_state}");
|
||||
}
|
||||
|
||||
if (dependency != null && dependency.TryGetTarget(out depTask))
|
||||
{
|
||||
_uncompletedDependencies++;
|
||||
}
|
||||
}
|
||||
|
||||
// Call AddDependentTask outside the lock to match C++ implementation
|
||||
depTask?.AddDependentTask(this);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Adds a dependent task that depends on this task.
|
||||
/// </summary>
|
||||
internal void AddDependentTask(Task dependentTask)
|
||||
{
|
||||
bool shouldNotifyImmediately = false;
|
||||
lock (_lock)
|
||||
{
|
||||
// If this task is already completed, notify the dependent task immediately
|
||||
if (_state == TaskState.Completed)
|
||||
{
|
||||
shouldNotifyImmediately = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (!_dependentTasks.Add(dependentTask))
|
||||
{
|
||||
throw new InvalidOperationException("Given dependency is already a dependency.");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Call OnDependencyCompleted outside the lock to avoid potential deadlock
|
||||
if (shouldNotifyImmediately)
|
||||
{
|
||||
dependentTask.OnDependencyCompleted();
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Sets the thread pool that will execute this task. State must be 'New'.
|
||||
/// </summary>
|
||||
internal void SetThreadPool(ThreadPoolInterface threadPool)
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
if (_state != TaskState.New)
|
||||
{
|
||||
throw new InvalidOperationException($"Cannot set thread pool when state is {_state}");
|
||||
}
|
||||
|
||||
_threadPoolToNotify = threadPool;
|
||||
_state = TaskState.Dispatched;
|
||||
|
||||
if (_uncompletedDependencies == 0)
|
||||
{
|
||||
_state = TaskState.DependenciesCompleted;
|
||||
_threadPoolToNotify.NotifyDependenciesCompleted(this);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Called when a dependency completes. State must be 'New' or 'Dispatched'.
|
||||
/// If 'Dispatched', may become 'DependenciesCompleted'.
|
||||
/// </summary>
|
||||
internal void OnDependencyCompleted()
|
||||
{
|
||||
ThreadPoolInterface? threadPoolToNotify = null;
|
||||
lock (_lock)
|
||||
{
|
||||
if (_state != TaskState.New && _state != TaskState.Dispatched)
|
||||
{
|
||||
throw new InvalidOperationException($"Cannot complete dependency when state is {_state}");
|
||||
}
|
||||
|
||||
_uncompletedDependencies--;
|
||||
if (_uncompletedDependencies == 0 && _state == TaskState.Dispatched)
|
||||
{
|
||||
_state = TaskState.DependenciesCompleted;
|
||||
threadPoolToNotify = _threadPoolToNotify;
|
||||
}
|
||||
}
|
||||
|
||||
threadPoolToNotify?.NotifyDependenciesCompleted(this);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Executes the task. State must be 'DependenciesCompleted' and becomes 'Completed'.
|
||||
/// </summary>
|
||||
internal void Execute()
|
||||
{
|
||||
Action? workItem;
|
||||
lock (_lock)
|
||||
{
|
||||
if (_state != TaskState.DependenciesCompleted)
|
||||
{
|
||||
throw new InvalidOperationException($"Cannot execute task when state is {_state}");
|
||||
}
|
||||
_state = TaskState.Running;
|
||||
workItem = _workItem;
|
||||
}
|
||||
|
||||
// Execute the work item outside the lock
|
||||
try
|
||||
{
|
||||
workItem?.Invoke();
|
||||
}
|
||||
finally
|
||||
{
|
||||
// Set state to Completed and notify all dependent tasks in a single lock
|
||||
lock (_lock)
|
||||
{
|
||||
_state = TaskState.Completed;
|
||||
|
||||
// Notify all dependent tasks
|
||||
foreach (var dependentTask in _dependentTasks)
|
||||
{
|
||||
dependentTask.OnDependencyCompleted();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
if (!_disposed)
|
||||
{
|
||||
_disposed = true;
|
||||
}
|
||||
|
||||
GC.SuppressFinalize(this);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,222 @@
|
||||
/*
|
||||
* 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 Microsoft.Extensions.Logging;
|
||||
using System.Runtime.InteropServices;
|
||||
|
||||
namespace CartographerSharp.Common.Threading;
|
||||
|
||||
/// <summary>
|
||||
/// Interface for thread pool implementations.
|
||||
/// </summary>
|
||||
public abstract class ThreadPoolInterface
|
||||
{
|
||||
/// <summary>
|
||||
/// Schedules a task for execution.
|
||||
/// </summary>
|
||||
/// <param name="task">The task to schedule</param>
|
||||
/// <returns>A weak reference to the task</returns>
|
||||
public abstract WeakReference<Task> Schedule(Task task);
|
||||
|
||||
/// <summary>
|
||||
/// Executes a task.
|
||||
/// </summary>
|
||||
protected static void Execute(Task task)
|
||||
{
|
||||
task.Execute();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Sets the thread pool for a task.
|
||||
/// </summary>
|
||||
protected void SetThreadPool(Task task)
|
||||
{
|
||||
task.SetThreadPool(this);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Notifies that dependencies of a task are completed.
|
||||
/// </summary>
|
||||
internal abstract void NotifyDependenciesCompleted(Task task);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// A fixed number of threads working on tasks. Adding a task does not block.
|
||||
/// Tasks may be added whether or not their dependencies are completed.
|
||||
/// When all dependencies of a task are completed, it is queued up for execution
|
||||
/// in a background thread. The queue must be empty before calling the destructor.
|
||||
/// The thread pool will then wait for the currently executing work items to finish
|
||||
/// and then destroy the threads.
|
||||
/// </summary>
|
||||
public partial class ThreadPool : ThreadPoolInterface, IDisposable
|
||||
{
|
||||
private readonly object _lock = new();
|
||||
private readonly List<Thread> _pool = [];
|
||||
private readonly Queue<Task> _taskQueue = new();
|
||||
private readonly Dictionary<Task, Task> _tasksNotReady = [];
|
||||
private bool _running = true;
|
||||
private bool _disposed = false;
|
||||
|
||||
/// <summary>
|
||||
/// Creates a new thread pool with the specified number of threads.
|
||||
/// </summary>
|
||||
public ThreadPool(int numThreads)
|
||||
{
|
||||
if (numThreads <= 0)
|
||||
{
|
||||
throw new ArgumentException("ThreadPool requires a positive num_threads!", nameof(numThreads));
|
||||
}
|
||||
|
||||
lock (_lock)
|
||||
{
|
||||
for (int i = 0; i < numThreads; i++)
|
||||
{
|
||||
var thread = new Thread(DoWork)
|
||||
{
|
||||
IsBackground = false,
|
||||
Name = $"CartographerThreadPool-{i}"
|
||||
};
|
||||
_pool.Add(thread);
|
||||
thread.Start();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Schedules a task for execution.
|
||||
/// </summary>
|
||||
public override WeakReference<Task> Schedule(Task task)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(task);
|
||||
|
||||
lock (_lock)
|
||||
{
|
||||
if (!_running)
|
||||
{
|
||||
throw new InvalidOperationException("ThreadPool is not running");
|
||||
}
|
||||
|
||||
if (_tasksNotReady.ContainsKey(task))
|
||||
{
|
||||
throw new InvalidOperationException("Schedule called twice for the same task");
|
||||
}
|
||||
|
||||
_tasksNotReady[task] = task;
|
||||
}
|
||||
|
||||
SetThreadPool(task);
|
||||
return new WeakReference<Task>(task);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Notifies that dependencies of a task are completed.
|
||||
/// </summary>
|
||||
internal override void NotifyDependenciesCompleted(Task task)
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
if (!_tasksNotReady.TryGetValue(task, out var storedTask))
|
||||
{
|
||||
throw new InvalidOperationException("Task not found in tasks_not_ready");
|
||||
}
|
||||
|
||||
_taskQueue.Enqueue(storedTask);
|
||||
_tasksNotReady.Remove(task);
|
||||
Monitor.PulseAll(_lock);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Worker thread method.
|
||||
/// </summary>
|
||||
private void DoWork()
|
||||
{
|
||||
Thread.BeginThreadAffinity();
|
||||
try
|
||||
{
|
||||
while (true)
|
||||
{
|
||||
Task? task = null;
|
||||
|
||||
lock (_lock)
|
||||
{
|
||||
while (_taskQueue.Count == 0 && _running)
|
||||
{
|
||||
Monitor.Wait(_lock);
|
||||
}
|
||||
|
||||
if (!_running && _taskQueue.Count == 0)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
if (_taskQueue.Count > 0)
|
||||
{
|
||||
task = _taskQueue.Dequeue();
|
||||
}
|
||||
}
|
||||
|
||||
if (task != null)
|
||||
{
|
||||
if (task.GetState() != TaskState.DependenciesCompleted)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
Execute(task);
|
||||
}
|
||||
}
|
||||
}
|
||||
finally
|
||||
{
|
||||
Thread.EndThreadAffinity();
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Disposes the thread pool, waiting for all tasks to complete.
|
||||
/// </summary>
|
||||
public void Dispose()
|
||||
{
|
||||
if (_disposed)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
lock (_lock)
|
||||
{
|
||||
if (!_running)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
_running = false;
|
||||
Monitor.PulseAll(_lock);
|
||||
}
|
||||
|
||||
foreach (var thread in _pool)
|
||||
{
|
||||
thread.Join();
|
||||
}
|
||||
|
||||
_disposed = true;
|
||||
|
||||
GC.SuppressFinalize(this);
|
||||
}
|
||||
|
||||
[LibraryImport("libc", SetLastError = true)]
|
||||
private static partial int nice(int inc);
|
||||
}
|
||||
@@ -0,0 +1,126 @@
|
||||
/*
|
||||
* 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 System.Runtime.InteropServices;
|
||||
|
||||
namespace CartographerSharp.Common.Time;
|
||||
|
||||
/// <summary>
|
||||
/// Universal Time Scale clock for Cartographer.
|
||||
/// Represents durations and timestamps as 64-bit integers representing
|
||||
/// 100 nanosecond ticks since the Epoch (January 1, 1 at the start of day in UTC).
|
||||
/// </summary>
|
||||
public static partial class TimeUtils
|
||||
{
|
||||
/// <summary>
|
||||
/// UTS Epoch offset from Unix Epoch in seconds.
|
||||
/// </summary>
|
||||
public const long UtsEpochOffsetFromUnixEpochInSeconds = 719162L * 24L * 60L * 60L;
|
||||
|
||||
/// <summary>
|
||||
/// Ticks per second in Universal Time Scale (100 nanoseconds = 10,000,000 ticks per second).
|
||||
/// </summary>
|
||||
public const long TicksPerSecond = 10_000_000L;
|
||||
|
||||
/// <summary>
|
||||
/// Creates a Duration from seconds.
|
||||
/// </summary>
|
||||
public static TimeSpan FromSeconds(double seconds)
|
||||
{
|
||||
return TimeSpan.FromTicks((long)(seconds * TicksPerSecond));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Creates a Duration from milliseconds.
|
||||
/// </summary>
|
||||
public static TimeSpan FromMilliseconds(long milliseconds)
|
||||
{
|
||||
// Convert milliseconds to 100-nanosecond ticks (1 ms = 10,000 ticks)
|
||||
// This matches the C++ implementation: std::chrono::duration_cast<Duration>(std::chrono::milliseconds(milliseconds))
|
||||
return TimeSpan.FromTicks(milliseconds * 10_000L);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Returns the given duration in seconds.
|
||||
/// </summary>
|
||||
public static double ToSeconds(TimeSpan duration)
|
||||
{
|
||||
return duration.TotalSeconds;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Creates a Time from Universal Time Scale ticks.
|
||||
/// </summary>
|
||||
public static DateTime FromUniversal(long ticks)
|
||||
{
|
||||
// Universal Time Scale epoch is January 1, 1 at the start of day in UTC
|
||||
// We need to convert from UTS epoch to .NET DateTime epoch (January 1, 0001)
|
||||
// UTS epoch offset: 719162 days = January 1, 1
|
||||
// .NET DateTime epoch: January 1, 0001
|
||||
// So we can use DateTime directly since both use the same epoch
|
||||
return new DateTime(ticks, DateTimeKind.Utc);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Outputs the Universal Time Scale timestamp for a given Time.
|
||||
/// </summary>
|
||||
public static long ToUniversal(DateTime time)
|
||||
{
|
||||
return time.ToUniversalTime().Ticks;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// CPU time consumed by the thread so far, in seconds.
|
||||
/// </summary>
|
||||
public static double GetThreadCpuTimeSeconds()
|
||||
{
|
||||
if (RuntimeInformation.IsOSPlatform(OSPlatform.Windows))
|
||||
{
|
||||
// Windows implementation would use GetThreadTimes
|
||||
// For now, return 0 on Windows
|
||||
return 0.0;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Linux implementation using clock_gettime with CLOCK_THREAD_CPUTIME_ID
|
||||
var timespec = new Timespec();
|
||||
if (clock_gettime(ClockId.CLOCK_THREAD_CPUTIME_ID, ref timespec) == 0)
|
||||
{
|
||||
return timespec.tv_sec + 1e-9 * timespec.tv_nsec;
|
||||
}
|
||||
return 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
private struct Timespec
|
||||
{
|
||||
public long tv_sec; // seconds
|
||||
public long tv_nsec; // nanoseconds
|
||||
}
|
||||
|
||||
private enum ClockId
|
||||
{
|
||||
CLOCK_REALTIME = 0,
|
||||
CLOCK_MONOTONIC = 1,
|
||||
CLOCK_PROCESS_CPUTIME_ID = 2,
|
||||
CLOCK_THREAD_CPUTIME_ID = 3,
|
||||
CLOCK_MONOTONIC_RAW = 4,
|
||||
}
|
||||
|
||||
[LibraryImport("libc", SetLastError = true)]
|
||||
private static partial int clock_gettime(ClockId clockid, ref Timespec tp);
|
||||
}
|
||||
Reference in New Issue
Block a user