systemSolver->runBasicKinematic();

MbDCode/GESpMatFullPv.cpp

@@ -1,22 +1,84 @@
 #include <cassert>
 
 #include "GESpMatFullPv.h"
+#include "SingularMatrixError.h"
 
 using namespace MbD;
 
-void MbD::GESpMatFullPv::doPivoting(size_t p)
+void MbD::GESpMatFullPv::doPivoting(int p)
 {
 	assert(false);
 }
 
-void MbD::GESpMatFullPv::forwardEliminateWithPivot(size_t p)
+void MbD::GESpMatFullPv::forwardEliminateWithPivot(int p)
 {
-	assert(false);
+	//app is pivot.
+	//i > p, j > p
+	//aip is eliminated.
+	//apj can cause fill - in.
+	//Columns are in place.
+	//"rightHandSideB may be multidimensional."
+
+	auto jp = colOrder->at(p);
+	auto& rowp = matrixA->at(p);
+	auto app = rowp->at(jp);
+	auto elementsInPivotRow = std::make_shared<std::vector<const std::pair<const int, double>*>>(rowp->size() - 1);
+	int index = 0;
+	for (auto const& keyValue : *rowp) {
+		if (keyValue.first != jp) {
+			elementsInPivotRow->at(index) = (&keyValue);
+			index++;
+		}
+	}
+	auto bp = rightHandSideB->at(p);
+	for (int ii = 0; ii < markowitzPivotColCount; ii++)
+	{
+		auto i = rowPositionsOfNonZerosInPivotColumn->at(ii);
+		auto& spRowi = matrixA->at(i);
+		if (spRowi->find(jp) == spRowi->end()) continue;
+		auto aip = spRowi->at(jp);
+		spRowi->erase(jp);
+		auto factor = aip / app;
+		for (auto keyValue : *elementsInPivotRow)
+		{
+			auto j = keyValue->first;
+			auto apj = keyValue->second;
+			(*spRowi)[j] -= factor * apj;
+		}
+		rightHandSideB->at(i) -= bp * factor;
+	}
 }
 
 void MbD::GESpMatFullPv::backSubstituteIntoDU()
 {
-	assert(false);
+	//"Use colOrder to get DU in upper triangular with nonzero diagonals."
+	//"Formula given by Eqn. 9.26 and 9.27 in Chapra's text 2nd Edition."
+
+	double sum, duij, duii;
+	//answerX = rightHandSideB->copyEmpty();
+	assert(m == n);
+	answerX = std::make_shared<FullColumn<double>>(m);
+	auto jn = colOrder->at(n);
+	answerX->at(jn) = rightHandSideB->at(m) / matrixA->at(m)->at(jn);
+	//auto rhsZeroElement = this->rhsZeroElement();
+	for (int i = n - 2; i >= 0; i--)
+	{
+		auto rowi = matrixA->at(i);
+		sum = 0.0; // rhsZeroElement copy.
+		for (auto const& keyValue : *rowi) {
+			auto jj = keyValue.first;
+			auto j = positionsOfOriginalCols->at(jj);
+			if (j > i) {
+				duij = keyValue.second;
+				sum += answerX->at(jj) * duij;
+			}
+			else {
+				duii = keyValue.second;
+			}
+		}
+		auto ji = colOrder->at(i);
+		answerX->at(ji) = rightHandSideB->at(i) - (sum / duii);
+	}
 }
 
 void MbD::GESpMatFullPv::postSolve()
@@ -26,5 +88,38 @@ void MbD::GESpMatFullPv::postSolve()
 
 void MbD::GESpMatFullPv::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal)
 {
-	assert(false);
+	//"A conditioned copy of spMat is solved."
+	if (m != spMat->nrow() || n != spMat->ncol()) {
+		m = spMat->nrow();
+		n = spMat->ncol();
+		matrixA = std::make_shared<SparseMatrix<double>>(m);
+		pivotValues = std::make_shared<FullColumn<double>>(m);
+		rowOrder = std::make_shared<FullColumn<int>>(m);
+		colOrder = std::make_shared<FullColumn<int>>(n);
+		positionsOfOriginalCols = std::make_shared<std::vector<int>>(m);
+		privateIndicesOfNonZerosInPivotRow = std::make_shared<std::vector<int>>();
+		rowPositionsOfNonZerosInColumns = std::make_shared<std::vector<std::shared_ptr<std::vector<int>>>>(n);
+		for (int j = 0; j < n; j++)
+		{
+			rowPositionsOfNonZerosInColumns->at(j) = std::make_shared<std::vector<int>>();
+		}
+	}
+	if (saveOriginal) {
+		rightHandSideB = fullCol->copy();
+	}
+	else {
+		rightHandSideB = fullCol;
+	}
+	for (int i = 0; i < m; i++)
+	{
+		auto& spRowi = spMat->at(i);
+		auto maxRowElement = spRowi->maxElement();
+		if (maxRowElement == 0) {
+			throw SingularMatrixError("");
+		}
+		matrixA->at(i) = spRowi->conditionedWithTol(singularPivotTolerance * maxRowElement);
+		rowOrder->at(i) = i;
+		colOrder->at(i) = i;
+		positionsOfOriginalCols->at(i) = i;
+	}
 }