/*-
 *  Copyright 2020 Beuth Hochschule für Technik Berlin, Hochschule für Technik Stuttgart
 * 
 *  This file is part of CityDoctor2.
 *
 *  CityDoctor2 is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU Lesser General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  CityDoctor2 is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU Lesser General Public License for more details.
 *
 *  You should have received a copy of the GNU Lesser General Public License
 *  along with CityDoctor2.  If not, see <https://www.gnu.org/licenses/>.
 */
package de.hft.stuttgart.citydoctor2.edge;


public class Matrix3d {

	private double[][] val = new double[3][3];

	public void set(int i, int j, double value) {
		val[i][j] = value;
	}

	public void gauss(double[] b, double[] x) {
		int[] rowIndex = new int[3];
		int[] colIndex = new int[3];

		// initialization
		// --------------
		for (int i = 0; i < 3; i++) {
			rowIndex[i] = colIndex[i] = i;
		}

		for (int i = 0; i < 2; i++) {

			int rIndex = -1;
			int cIndex = -1;

			// find total pivot
			// ----------------

			double max = 0.0;
			for (int k = i; k < 3; k++) {
				for (int l = i; l < 3; l++) {
					double ab = Math.abs(val[rowIndex[k]][colIndex[l]]);
					if (max < ab) {
						max = ab;
						rIndex = k;
						cIndex = l;
					}
				}
			}

			// check result against eps
			// ------------------------

			if (max < 1e-6) {
				throw new IllegalStateException("gauss: max (" + max + ") below eps (" + 1e-6 + ")");
			}

			// change indicies
			// ---------------

			int u = rowIndex[i];
			int v = colIndex[i];

			rowIndex[i] = rowIndex[rIndex];
			colIndex[i] = colIndex[cIndex];

			rowIndex[rIndex] = u;
			colIndex[cIndex] = v;

			double pivot = val[rowIndex[i]][colIndex[i]];

			// Reduktion
			// ---------
			for (int z = i + 1; z < 3; z++) {
				double q = val[rowIndex[z]][colIndex[i]] / pivot;
				for (int s = i + 1; s < 3; s++) {
					val[rowIndex[z]][colIndex[s]] = val[rowIndex[z]][colIndex[s]]
							- val[rowIndex[i]][colIndex[s]] * q;
				}

				b[rowIndex[z]] = b[rowIndex[z]] - b[rowIndex[i]] * q;
			}
		}

		// solution
		// --------

		for (int i = 2; i >= 0; i--) {
			x[colIndex[i]] = b[rowIndex[i]];
			for (int l = i + 1; l < 3; l++) {
				x[colIndex[i]] = x[colIndex[i]] - x[colIndex[l]] * val[rowIndex[i]][colIndex[l]];
			}
			x[colIndex[i]] = x[colIndex[i]] / val[rowIndex[i]][colIndex[i]];
		}
	}
}
