105 lines
3.4 KiB
C#
105 lines
3.4 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using MultiWheelC.TrajectoryPlanning.Utils;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
|
|
public sealed class SparseTripletBuilder
|
|
{
|
|
private readonly int rowCount;
|
|
private readonly int columnCount;
|
|
private readonly bool upperTriangleOnly;
|
|
private readonly List<Triplet> triplets = new List<Triplet>();
|
|
|
|
public SparseTripletBuilder(int rowCount, int columnCount)
|
|
: this(rowCount, columnCount, false)
|
|
{
|
|
}
|
|
|
|
public SparseTripletBuilder(int rowCount, int columnCount, bool upperTriangleOnly)
|
|
{
|
|
if (rowCount < 0)
|
|
throw new ArgumentOutOfRangeException(nameof(rowCount));
|
|
if (columnCount < 0)
|
|
throw new ArgumentOutOfRangeException(nameof(columnCount));
|
|
|
|
this.rowCount = rowCount;
|
|
this.columnCount = columnCount;
|
|
this.upperTriangleOnly = upperTriangleOnly;
|
|
}
|
|
|
|
public void Add(int row, int column, double value)
|
|
{
|
|
if (row < 0 || row >= rowCount)
|
|
throw new ArgumentOutOfRangeException(nameof(row));
|
|
if (column < 0 || column >= columnCount)
|
|
throw new ArgumentOutOfRangeException(nameof(column));
|
|
if (!NumericGuard.IsFinite(value))
|
|
throw new ArgumentOutOfRangeException(nameof(value), "Sparse triplet values must be finite.");
|
|
if (upperTriangleOnly && row > column)
|
|
throw new ArgumentOutOfRangeException(nameof(row), "Upper-triangular storage does not accept lower-triangular entries.");
|
|
|
|
triplets.Add(new Triplet(row, column, value));
|
|
}
|
|
|
|
public SparseCscMatrix Build()
|
|
{
|
|
var ordered = new List<Triplet>(triplets);
|
|
ordered.Sort(CompareTriplets);
|
|
|
|
var values = new List<double>();
|
|
var rows = new List<int>();
|
|
var pointers = new List<int>(columnCount + 1) { 0 };
|
|
int nextTriplet = 0;
|
|
|
|
for (int column = 0; column < columnCount; column++)
|
|
{
|
|
while (nextTriplet < ordered.Count && ordered[nextTriplet].Column == column)
|
|
{
|
|
int row = ordered[nextTriplet].Row;
|
|
double sum = 0d;
|
|
do
|
|
{
|
|
sum += ordered[nextTriplet].Value;
|
|
nextTriplet++;
|
|
}
|
|
while (nextTriplet < ordered.Count && ordered[nextTriplet].Column == column && ordered[nextTriplet].Row == row);
|
|
|
|
if (!NumericGuard.IsFinite(sum))
|
|
throw new ArgumentOutOfRangeException(nameof(triplets), "Duplicate sparse triplets must sum to a finite value.");
|
|
if (sum != 0d)
|
|
{
|
|
rows.Add(row);
|
|
values.Add(sum);
|
|
}
|
|
}
|
|
|
|
pointers.Add(values.Count);
|
|
}
|
|
|
|
return new SparseCscMatrix(rowCount, columnCount, values, rows, pointers);
|
|
}
|
|
|
|
private static int CompareTriplets(Triplet left, Triplet right)
|
|
{
|
|
int columnComparison = left.Column.CompareTo(right.Column);
|
|
return columnComparison != 0 ? columnComparison : left.Row.CompareTo(right.Row);
|
|
}
|
|
|
|
private sealed class Triplet
|
|
{
|
|
public Triplet(int row, int column, double value)
|
|
{
|
|
Row = row;
|
|
Column = column;
|
|
Value = value;
|
|
}
|
|
|
|
public int Row { get; }
|
|
|
|
public int Column { get; }
|
|
|
|
public double Value { get; }
|
|
}
|
|
}
|