147 lines
4.8 KiB
C#
147 lines
4.8 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using MultiWheelC.TrajectoryPlanning.Utils;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
|
|
|
|
/// <summary>
|
|
/// 为 Hybrid A* Open List 提供确定性优先级的二叉最小堆。
|
|
/// 排序严格依次比较 F、H、较大的 G 和插入序号；F、H、G 必须为有限且非负的等效米代价。
|
|
/// </summary>
|
|
/// <typeparam name="T">与一组搜索代价关联的节点或条目类型。</typeparam>
|
|
public sealed class BinaryMinHeap<T>
|
|
{
|
|
private readonly List<HeapEntry> _entries = new List<HeapEntry>();
|
|
private long _nextInsertionSequence;
|
|
|
|
/// <summary>创建空的确定性 Open List 堆。</summary>
|
|
public BinaryMinHeap()
|
|
{
|
|
}
|
|
|
|
/// <summary>当前堆内尚未出队的条目数量。</summary>
|
|
public int Count { get { return _entries.Count; } }
|
|
|
|
/// <summary>
|
|
/// 将一个条目和其搜索排序代价压入堆。
|
|
/// 参数：item 为关联条目；f、h、g 均为有限且非负的等效米代价。
|
|
/// 失败：任一代价无效、item 为 null（仅引用类型）或插入序号耗尽时抛出异常。
|
|
/// </summary>
|
|
public void Push(T item, double f, double h, double g)
|
|
{
|
|
if (ReferenceEquals(item, null)) throw new ArgumentNullException(nameof(item));
|
|
ValidateCost(f, nameof(f));
|
|
ValidateCost(h, nameof(h));
|
|
ValidateCost(g, nameof(g));
|
|
if (_nextInsertionSequence == long.MaxValue)
|
|
throw new InvalidOperationException("The binary heap insertion sequence has been exhausted.");
|
|
|
|
var entry = new HeapEntry(item, f, h, g, _nextInsertionSequence);
|
|
_nextInsertionSequence++;
|
|
_entries.Add(entry);
|
|
SiftUp(_entries.Count - 1);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 弹出当前排序最优的条目。
|
|
/// 返回：按 F、H、较大 G 和插入序号排序后的最小条目；空堆时抛出 <see cref="InvalidOperationException"/>。
|
|
/// </summary>
|
|
public T Pop()
|
|
{
|
|
if (_entries.Count == 0) throw new InvalidOperationException("The binary heap is empty.");
|
|
|
|
HeapEntry result = _entries[0];
|
|
int lastIndex = _entries.Count - 1;
|
|
if (lastIndex == 0)
|
|
{
|
|
_entries.RemoveAt(0);
|
|
return result.Item;
|
|
}
|
|
|
|
_entries[0] = _entries[lastIndex];
|
|
_entries.RemoveAt(lastIndex);
|
|
SiftDown(0);
|
|
return result.Item;
|
|
}
|
|
|
|
/// <summary>清空尚未出队的条目；后续插入序号继续单调递增以保持整个实例内的确定性。</summary>
|
|
public void Clear()
|
|
{
|
|
_entries.Clear();
|
|
}
|
|
|
|
private void SiftUp(int index)
|
|
{
|
|
while (index > 0)
|
|
{
|
|
int parentIndex = (index - 1) / 2;
|
|
if (Compare(_entries[index], _entries[parentIndex]) >= 0) return;
|
|
Swap(index, parentIndex);
|
|
index = parentIndex;
|
|
}
|
|
}
|
|
|
|
private void SiftDown(int index)
|
|
{
|
|
while (true)
|
|
{
|
|
int leftChildIndex = index * 2 + 1;
|
|
if (leftChildIndex >= _entries.Count) return;
|
|
|
|
int bestChildIndex = leftChildIndex;
|
|
int rightChildIndex = leftChildIndex + 1;
|
|
if (rightChildIndex < _entries.Count && Compare(_entries[rightChildIndex], _entries[leftChildIndex]) < 0)
|
|
bestChildIndex = rightChildIndex;
|
|
|
|
if (Compare(_entries[bestChildIndex], _entries[index]) >= 0) return;
|
|
Swap(index, bestChildIndex);
|
|
index = bestChildIndex;
|
|
}
|
|
}
|
|
|
|
private void Swap(int firstIndex, int secondIndex)
|
|
{
|
|
HeapEntry temporary = _entries[firstIndex];
|
|
_entries[firstIndex] = _entries[secondIndex];
|
|
_entries[secondIndex] = temporary;
|
|
}
|
|
|
|
private static int Compare(HeapEntry left, HeapEntry right)
|
|
{
|
|
int comparison = left.F.CompareTo(right.F);
|
|
if (comparison != 0) return comparison;
|
|
|
|
comparison = left.H.CompareTo(right.H);
|
|
if (comparison != 0) return comparison;
|
|
|
|
comparison = right.G.CompareTo(left.G);
|
|
if (comparison != 0) return comparison;
|
|
|
|
return left.InsertionSequence.CompareTo(right.InsertionSequence);
|
|
}
|
|
|
|
private static void ValidateCost(double value, string parameterName)
|
|
{
|
|
if (!NumericGuard.IsFinite(value) || value < 0d)
|
|
throw new ArgumentOutOfRangeException(parameterName, "Search costs must be finite and non-negative.");
|
|
}
|
|
|
|
private sealed class HeapEntry
|
|
{
|
|
public HeapEntry(T item, double f, double h, double g, long insertionSequence)
|
|
{
|
|
Item = item;
|
|
F = f;
|
|
H = h;
|
|
G = g;
|
|
InsertionSequence = insertionSequence;
|
|
}
|
|
|
|
public T Item { get; }
|
|
public double F { get; }
|
|
public double H { get; }
|
|
public double G { get; }
|
|
public long InsertionSequence { get; }
|
|
}
|
|
}
|