114 lines
4.4 KiB
C#
114 lines
4.4 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Collections.ObjectModel;
|
|
using MultiWheelC.TrajectoryPlanning.Utils;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
|
|
public sealed class QuadraticProgram
|
|
{
|
|
public const double MaximumFiniteBound = 1e30d;
|
|
|
|
public QuadraticProgram(
|
|
SparseCscMatrix upperTriangularP,
|
|
IReadOnlyList<double> q,
|
|
SparseCscMatrix a,
|
|
IReadOnlyList<double> lowerBounds,
|
|
IReadOnlyList<double> upperBounds)
|
|
{
|
|
if (upperTriangularP == null)
|
|
throw new ArgumentNullException(nameof(upperTriangularP));
|
|
if (q == null)
|
|
throw new ArgumentNullException(nameof(q));
|
|
if (a == null)
|
|
throw new ArgumentNullException(nameof(a));
|
|
if (lowerBounds == null)
|
|
throw new ArgumentNullException(nameof(lowerBounds));
|
|
if (upperBounds == null)
|
|
throw new ArgumentNullException(nameof(upperBounds));
|
|
if (upperTriangularP.RowCount != upperTriangularP.ColumnCount)
|
|
throw new ArgumentOutOfRangeException(nameof(upperTriangularP), "The quadratic Hessian must be square.");
|
|
if (q.Count != upperTriangularP.ColumnCount)
|
|
throw new ArgumentOutOfRangeException(nameof(q), "The linear cost length must match the variable count.");
|
|
if (a.ColumnCount != upperTriangularP.ColumnCount)
|
|
throw new ArgumentOutOfRangeException(nameof(a), "Constraint columns must match the variable count.");
|
|
if (lowerBounds.Count != a.RowCount || upperBounds.Count != a.RowCount)
|
|
throw new ArgumentOutOfRangeException(nameof(lowerBounds), "Constraint bounds must match the constraint count.");
|
|
|
|
ValidateUpperTriangle(upperTriangularP);
|
|
ValidateFinite(q, nameof(q));
|
|
ValidateBounds(lowerBounds, upperBounds);
|
|
|
|
UpperTriangularP = CopyMatrix(upperTriangularP);
|
|
LinearCost = Copy(q);
|
|
ConstraintMatrix = CopyMatrix(a);
|
|
LowerBounds = Copy(lowerBounds);
|
|
UpperBounds = Copy(upperBounds);
|
|
VariableCount = UpperTriangularP.ColumnCount;
|
|
ConstraintCount = ConstraintMatrix.RowCount;
|
|
}
|
|
|
|
public SparseCscMatrix UpperTriangularP { get; }
|
|
|
|
public IReadOnlyList<double> LinearCost { get; }
|
|
|
|
public SparseCscMatrix ConstraintMatrix { get; }
|
|
|
|
public IReadOnlyList<double> LowerBounds { get; }
|
|
|
|
public IReadOnlyList<double> UpperBounds { get; }
|
|
|
|
public int VariableCount { get; }
|
|
|
|
public int ConstraintCount { get; }
|
|
|
|
private static void ValidateUpperTriangle(SparseCscMatrix matrix)
|
|
{
|
|
for (int column = 0; column < matrix.ColumnCount; column++)
|
|
{
|
|
for (int index = matrix.ColumnPointers[column]; index < matrix.ColumnPointers[column + 1]; index++)
|
|
{
|
|
if (matrix.RowIndices[index] > column)
|
|
throw new ArgumentOutOfRangeException(nameof(matrix), "The quadratic Hessian must store only its upper triangle.");
|
|
}
|
|
}
|
|
}
|
|
|
|
private static void ValidateFinite(IReadOnlyList<double> values, string parameterName)
|
|
{
|
|
for (int index = 0; index < values.Count; index++)
|
|
{
|
|
if (!NumericGuard.IsFinite(values[index]))
|
|
throw new ArgumentOutOfRangeException(parameterName, "Values must be finite.");
|
|
}
|
|
}
|
|
|
|
private static void ValidateBounds(IReadOnlyList<double> lowerBounds, IReadOnlyList<double> upperBounds)
|
|
{
|
|
for (int index = 0; index < lowerBounds.Count; index++)
|
|
{
|
|
double lower = lowerBounds[index];
|
|
double upper = upperBounds[index];
|
|
if (!NumericGuard.IsFinite(lower) || !NumericGuard.IsFinite(upper) ||
|
|
lower < -MaximumFiniteBound || upper > MaximumFiniteBound || lower > upper)
|
|
{
|
|
throw new ArgumentOutOfRangeException(nameof(lowerBounds),
|
|
"Constraint bounds must be finite, within the supported range, and ordered.");
|
|
}
|
|
}
|
|
}
|
|
|
|
private static SparseCscMatrix CopyMatrix(SparseCscMatrix source)
|
|
{
|
|
return new SparseCscMatrix(source.RowCount, source.ColumnCount, source.Values, source.RowIndices, source.ColumnPointers);
|
|
}
|
|
|
|
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);
|
|
}
|
|
}
|