88 lines
3.6 KiB
C#
88 lines
3.6 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Collections.ObjectModel;
|
|
using MultiWheelC.TrajectoryPlanning.Utils;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
|
|
public sealed class SparseCscMatrix
|
|
{
|
|
public SparseCscMatrix(
|
|
int rowCount,
|
|
int columnCount,
|
|
IReadOnlyList<double> values,
|
|
IReadOnlyList<int> rowIndices,
|
|
IReadOnlyList<int> columnPointers)
|
|
{
|
|
if (rowCount < 0)
|
|
throw new ArgumentOutOfRangeException(nameof(rowCount));
|
|
if (columnCount < 0)
|
|
throw new ArgumentOutOfRangeException(nameof(columnCount));
|
|
if (values == null)
|
|
throw new ArgumentNullException(nameof(values));
|
|
if (rowIndices == null)
|
|
throw new ArgumentNullException(nameof(rowIndices));
|
|
if (columnPointers == null)
|
|
throw new ArgumentNullException(nameof(columnPointers));
|
|
if (values.Count != rowIndices.Count)
|
|
throw new ArgumentException("CSC values and row indices must have the same length.", nameof(rowIndices));
|
|
if (columnPointers.Count != columnCount + 1)
|
|
throw new ArgumentException("CSC column-pointer count must equal column count plus one.", nameof(columnPointers));
|
|
if (columnPointers[0] != 0)
|
|
throw new ArgumentOutOfRangeException(nameof(columnPointers), "The first CSC column pointer must be zero.");
|
|
if (columnPointers[columnCount] != values.Count)
|
|
throw new ArgumentOutOfRangeException(nameof(columnPointers), "The final CSC column pointer must equal the nonzero count.");
|
|
|
|
for (int column = 0; column < columnCount; column++)
|
|
{
|
|
if (columnPointers[column] > columnPointers[column + 1])
|
|
throw new ArgumentOutOfRangeException(nameof(columnPointers), "CSC column pointers must be monotonic.");
|
|
if (columnPointers[column] < 0 || columnPointers[column + 1] > values.Count)
|
|
throw new ArgumentOutOfRangeException(nameof(columnPointers), "CSC column pointers must stay within the nonzero count.");
|
|
}
|
|
|
|
for (int index = 0; index < values.Count; index++)
|
|
{
|
|
if (!NumericGuard.IsFinite(values[index]))
|
|
throw new ArgumentOutOfRangeException(nameof(values), "CSC values must be finite.");
|
|
if (rowIndices[index] < 0 || rowIndices[index] >= rowCount)
|
|
throw new ArgumentOutOfRangeException(nameof(rowIndices), "CSC row indices must be within matrix bounds.");
|
|
}
|
|
|
|
for (int column = 0; column < columnCount; column++)
|
|
{
|
|
int previousRow = -1;
|
|
for (int index = columnPointers[column]; index < columnPointers[column + 1]; index++)
|
|
{
|
|
if (rowIndices[index] <= previousRow)
|
|
throw new ArgumentException("CSC row indices must be strictly ascending in each column.", nameof(rowIndices));
|
|
previousRow = rowIndices[index];
|
|
}
|
|
}
|
|
|
|
RowCount = rowCount;
|
|
ColumnCount = columnCount;
|
|
Values = Copy(values);
|
|
RowIndices = Copy(rowIndices);
|
|
ColumnPointers = Copy(columnPointers);
|
|
}
|
|
|
|
public int RowCount { get; }
|
|
|
|
public int ColumnCount { get; }
|
|
|
|
public IReadOnlyList<double> Values { get; }
|
|
|
|
public IReadOnlyList<int> RowIndices { get; }
|
|
|
|
public IReadOnlyList<int> ColumnPointers { get; }
|
|
|
|
private static IReadOnlyList<T> Copy<T>(IReadOnlyList<T> source)
|
|
{
|
|
var copy = new List<T>(source.Count);
|
|
for (int index = 0; index < source.Count; index++)
|
|
copy.Add(source[index]);
|
|
return new ReadOnlyCollection<T>(copy);
|
|
}
|
|
}
|