public static TriDiagonalMatrixF TestTdm()
{
TriDiagonalMatrixF m = new TriDiagonalMatrixF(10);
for (int i = 0; i < m.N; i++)
{
m.A[i] = 1.111111f;
m.B[i] = 2.222222f;
m.C[i] = 3.333333f;
}
Console.WriteLine("Matrix:\n{0}", m.ToDisplayString(",4:0.00", " "));
for (int i = 0; i < m.N; i++)
{
m[i, i] = 4.4444f;
}
Console.WriteLine("Matrix:\n{0}", m.ToDisplayString(",4:0.00", " "));
// Solve
var rand = new System.Random(1);
float[] d = new float[m.N];
for (int i = 0; i < d.Length; i++)
{
d[i] = (float)rand.NextDouble();
}
float[] x = m.Solve(d);
Console.WriteLine("Solve returned: ");
for (int i = 0; i < x.Length; i++)
{
Console.Write("{0:0.0000}, ", x[i]);
}
Console.WriteLine();
return(m);
}
/// <summary>
/// Compute spline coefficients for the specified x,y points.
/// This does the "natural spline" style for ends.
/// This can extrapolate off the ends of the splines.
/// You must provide points in X sort order.
/// </summary>
/// <param name="x">Input. X coordinates to fit.</param>
/// <param name="y">Input. Y coordinates to fit.</param>
/// <param name="debug">Turn on console output. Default is false.</param>
public void Fit(float[] x, float[] y, bool debug = false)
{
// Save x and y for eval
this.xOrig = x;
this.yOrig = y;
int n = x.Length;
float[] r = new float[n]; // the right hand side numbers: wikipedia page overloads b
TriDiagonalMatrixF m = new TriDiagonalMatrixF(n);
float dx1, dx2, dy1, dy2;
// First row is different (equation 16 from the article)
dx1 = x[1] - x[0];
m.C[0] = 1.0f / dx1;
m.B[0] = 2.0f * m.C[0];
r[0] = 3 * (y[1] - y[0]) / (dx1 * dx1);
// Body rows (equation 15 from the article)
for (int i = 1; i < n - 1; i++)
{
dx1 = x[i] - x[i - 1];
dx2 = x[i + 1] - x[i];
m.A[i] = 1.0f / dx1;
m.C[i] = 1.0f / dx2;
m.B[i] = 2.0f * (m.A[i] + m.C[i]);
dy1 = y[i] - y[i - 1];
dy2 = y[i + 1] - y[i];
r[i] = 3 * (dy1 / (dx1 * dx1) + dy2 / (dx2 * dx2));
}
// Last row also different (equation 17 from the article)
dx1 = x[n - 1] - x[n - 2];
dy1 = y[n - 1] - y[n - 2];
m.A[n - 1] = 1.0f / dx1;
m.B[n - 1] = 2.0f * m.A[n - 1];
r[n - 1] = 3 * (dy1 / (dx1 * dx1));
if (debug)
{
Console.WriteLine("Tri-diagonal matrix:\n{0}", m.ToDisplayString(":0.0000", " "));
}
if (debug)
{
Console.WriteLine("r: {0}", ArrayUtil.ToString <float>(r));
}
// k is the solution to the matrix
float[] k = m.Solve(r);
if (debug)
{
Console.WriteLine("k = {0}", ArrayUtil.ToString <float>(k));
}
// a and b are each spline's coefficients
this.a = new float[n - 1];
this.b = new float[n - 1];
for (int i = 1; i < n; i++)
{
dx1 = x[i] - x[i - 1];
dy1 = y[i] - y[i - 1];
a[i - 1] = k[i - 1] * dx1 - dy1; // equation 10 from the article
b[i - 1] = -k[i] * dx1 + dy1; // equation 11 from the article
}
if (debug)
{
Console.WriteLine("a: {0}", ArrayUtil.ToString <float>(a));
}
if (debug)
{
Console.WriteLine("b: {0}", ArrayUtil.ToString <float>(b));
}
}
/// <summary>
/// Compute spline coefficients for the specified x,y points.
/// This does the "natural spline" style for ends.
/// This can extrapolate off the ends of the splines.
/// You must provide points in X sort order.
/// </summary>
/// <param name="x">Input. X coordinates to fit.</param>
/// <param name="y">Input. Y coordinates to fit.</param>
/// <param name="startSlope">Optional slope constraint for the first point. Single.NaN means no constraint.</param>
/// <param name="endSlope">Optional slope constraint for the final point. Single.NaN means no constraint.</param>
/// <param name="debug">Turn on console output. Default is false.</param>
public void Fit(float[] x, Vector4[] y, Vector4 startSlope, Vector4 endSlope, bool debug = false)
{
if (float.IsInfinity(startSlope.x) || float.IsInfinity(startSlope.y) || float.IsInfinity(startSlope.z) || float.IsInfinity(endSlope.x) || float.IsInfinity(endSlope.y) || float.IsInfinity(endSlope.z))
{
throw new Exception("startSlope and endSlope cannot be null.");
}
// Save x and y for eval
this.xOrig = x;
this.yOrigVector = y;
int n = x.Length;
Vector4[] r = new Vector4[n]; // the right hand side numbers: wikipedia page overloads b
TriDiagonalMatrixF m = new TriDiagonalMatrixF(n);
float dx1, dx2;
Vector4 dy1, dy2;
// First row is different (equation 16 from the article)
if (startSlope.x == 0 && startSlope.y == 0 && startSlope.z == 0)
{
dx1 = x[1] - x[0];
m.C[0] = 1.0f / dx1;
m.B[0] = 2.0f * m.C[0];
r[0] = 3 * (y[1] - y[0]) / (dx1 * dx1);
}
else
{
m.B[0] = 1;
r[0] = startSlope;
}
// Body rows (equation 15 from the article)
for (int i = 1; i < n - 1; i++)
{
dx1 = x[i] - x[i - 1];
dx2 = x[i + 1] - x[i];
m.A[i] = 1.0f / dx1;
m.C[i] = 1.0f / dx2;
m.B[i] = 2.0f * (m.A[i] + m.C[i]);
dy1 = y[i] - y[i - 1];
dy2 = y[i + 1] - y[i];
r[i] = 3 * (dy1 / (dx1 * dx1) + dy2 / (dx2 * dx2));
}
// Last row also different (equation 17 from the article)
if (endSlope.x == 0 && endSlope.y == 0 && endSlope.z == 0)
{
dx1 = x[n - 1] - x[n - 2];
dy1 = y[n - 1] - y[n - 2];
m.A[n - 1] = 1.0f / dx1;
m.B[n - 1] = 2.0f * m.A[n - 1];
r[n - 1] = 3 * (dy1 / (dx1 * dx1));
}
else
{
m.B[n - 1] = 1;
r[n - 1] = endSlope;
}
//if (debug) Console.WriteLine("Tri-diagonal matrix:\n{0}", m.ToDisplayString(":0.0000", " "));
//if (debug) Console.WriteLine("r: {0}", ArrayUtil.ToString<float>(r));
// k is the solution to the matrix
Vector4[] k = new Vector4[r.Length];
for (int dim = 0; dim < 4; dim++)
{
float[] currentR = extractFromVectors(r, dim);
float[] currentK = m.Solve(currentR);
for (int j = 0; j < currentK.Length; j++)
{
if (dim == 0)
{
k[j].x = currentK[j];
}
if (dim == 1)
{
k[j].y = currentK[j];
}
if (dim == 2)
{
k[j].z = currentK[j];
}
if (dim == 3)
{
k[j].w = currentK[j];
}
}
}
//if (debug) Console.WriteLine("k = {0}", ArrayUtil.ToString<float>(k));
// a and b are each spline's coefficients
this.aVector = new Vector4[n - 1];
this.bVector = new Vector4[n - 1];
for (int i = 1; i < n; i++)
{
dx1 = x[i] - x[i - 1];
dy1 = y[i] - y[i - 1];
aVector[i - 1] = k[i - 1] * dx1 - dy1; // equation 10 from the article
bVector[i - 1] = -k[i] * dx1 + dy1; // equation 11 from the article
}
//if (debug) Console.WriteLine("a: {0}", ArrayUtil.ToString<float>(a));
//if (debug) Console.WriteLine("b: {0}", ArrayUtil.ToString<float>(b));
}
