CubicSplinesInterpolation/TriDiagonalMatrix.cs at master · E1izabeth/CubicSplinesInterpolation · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
using System;
using System.Diagnostics;
using System.Text;

namespace CubicSplinesInterpolation
{
	/// <summary>
	/// A tri-diagonal matrix has non-zero entries only on the main diagonal, the diagonal above the main (super), and the
	/// diagonal below the main (sub).
	/// </summary>
	/// <remarks>
	/// <para>
	/// This is based on the wikipedia article: http://en.wikipedia.org/wiki/Tridiagonal_matrix_algorithm
	/// </para>
	/// </remarks>
	public class TriDiagonalMatrixF
	{
		/// <summary>
		/// The values for the sub-diagonal. A[0] is never used.
		/// </summary>
		public double[] A;

		/// <summary>
		/// The values for the main diagonal.
		/// </summary>
		public double[] B;

		/// <summary>
		/// The values for the super-diagonal. C[C.Length-1] is never used.
		/// </summary>
		public double[] C;

		/// <summary>
		/// The width and height of this matrix.
		/// </summary>
		public int N
		{
			get { return (A != null ? A.Length : 0); }
		}

		/// <summary>
		/// Indexer. Setter throws an exception if you try to set any not on the super, main, or sub diagonals.
		/// </summary>
		public double this[int row, int col]
		{
			get
			{
				int di = row - col;

				if (di == 0)
				{
					return B[row];
				}
				else if (di == -1)
				{
					Debug.Assert(row < N - 1);
					return C[row];
				}
				else if (di == 1)
				{
					Debug.Assert(row > 0);
					return A[row];
				}
				else return 0;
			}
			set
			{
				int di = row - col;

				if (di == 0)
				{
					B[row] = value;
				}
				else if (di == -1)
				{
					Debug.Assert(row < N - 1);
					C[row] = value;
				}
				else if (di == 1)
				{
					Debug.Assert(row > 0);
					A[row] = value;
				}
				else
				{
					throw new ArgumentException("Only the main, super, and sub diagonals can be set.");
				}
			}
		}

		/// <summary>
		/// Construct an NxN matrix.
		/// </summary>
		public TriDiagonalMatrixF(int n)
		{
			this.A = new double[n];
			this.B = new double[n];
			this.C = new double[n];
		}

		/// <summary>
		/// Solve the system of equations this*x=d given the specified d.
		/// </summary>
		/// <remarks>
		/// Uses the Thomas algorithm described in the wikipedia article: http://en.wikipedia.org/wiki/Tridiagonal_matrix_algorithm
		/// Not optimized. Not destructive.
		/// </remarks>
		/// <param name="d">Right side of the equation.</param>
		public double[] Solve(double[] d)
		{
			int n = this.N;

			if (d.Length != n)
			{
				throw new ArgumentException("The input d is not the same size as this matrix.");
			}

			// cPrime
			double[] cPrime = new double[n];
			cPrime[0] = C[0] / B[0];

			for (int i = 1; i < n; i++)
			{
				cPrime[i] = C[i] / (B[i] - cPrime[i-1] * A[i]);
			}

			// dPrime
			double[] dPrime = new double[n];
			dPrime[0] = d[0] / B[0];

			for (int i = 1; i < n; i++)
			{
				dPrime[i] = (d[i] - dPrime[i-1]*A[i]) / (B[i] - cPrime[i - 1] * A[i]);
			}

			// Back substitution
			double[] x = new double[n];
			x[n - 1] = dPrime[n - 1];

			for (int i = n-2; i >= 0; i--)
			{
				x[i] = dPrime[i] - cPrime[i] * x[i + 1];
			}

			return x;
		}
	}
}