3952f45945
* fixes mac errors and warnings; check on TODO items * renamed function as override was not of virtual * removed another using * experimental adjustment * move fcDot to public * renaming things
72 lines
2.1 KiB
C++
72 lines
2.1 KiB
C++
/***************************************************************************
|
|
* Copyright (c) 2023 Ondsel, Inc. *
|
|
* *
|
|
* This file is part of OndselSolver. *
|
|
* *
|
|
* See LICENSE file for details about copyright. *
|
|
***************************************************************************/
|
|
|
|
#include "SystemNewtonRaphson.h"
|
|
#include "SystemSolver.h"
|
|
#include "SparseMatrix.h"
|
|
#include "MatrixSolver.h"
|
|
#include "GESpMatParPvMarkoFast.h"
|
|
#include "CREATE.h"
|
|
#include "GESpMatParPvPrecise.h"
|
|
|
|
using namespace MbD;
|
|
|
|
void SystemNewtonRaphson::initializeGlobally()
|
|
{
|
|
this->assignEquationNumbers();
|
|
system->partsJointsMotionsForcesTorquesDo([&](std::shared_ptr<Item> item) { item->useEquationNumbers(); });
|
|
this->createVectorsAndMatrices();
|
|
matrixSolver = this->matrixSolverClassNew();
|
|
}
|
|
|
|
void SystemNewtonRaphson::createVectorsAndMatrices()
|
|
{
|
|
x = std::make_shared<FullColumn<double>>(n);
|
|
y = std::make_shared<FullColumn<double>>(n);
|
|
pypx = std::make_shared <SparseMatrix<double>>(n, n);
|
|
}
|
|
|
|
std::shared_ptr<MatrixSolver> SystemNewtonRaphson::matrixSolverClassNew()
|
|
{
|
|
return CREATE<GESpMatParPvMarkoFast>::With();
|
|
}
|
|
|
|
void SystemNewtonRaphson::calcdxNorm()
|
|
{
|
|
VectorNewtonRaphson::calcdxNorm();
|
|
std::string str("MbD: Convergence = ");
|
|
str += std::to_string(dxNorm);
|
|
system->logString(str);
|
|
}
|
|
|
|
void SystemNewtonRaphson::basicSolveEquations()
|
|
{
|
|
dx = matrixSolver->solvewithsaveOriginal(pypx, y->negated(), false);
|
|
}
|
|
|
|
void SystemNewtonRaphson::handleSingularMatrix()
|
|
{
|
|
auto& r = *matrixSolver;
|
|
std::string str = typeid(r).name();
|
|
if (str.find("GESpMatParPvMarkoFast") != std::string::npos) {
|
|
matrixSolver = CREATE<GESpMatParPvPrecise>::With();
|
|
this->solveEquations();
|
|
}
|
|
else {
|
|
str = typeid(r).name();
|
|
if (str.find("GESpMatParPvPrecise") != std::string::npos) {
|
|
str = "MbD: Singular Matrix Error. ";
|
|
system->logString(str);
|
|
matrixSolver = this->matrixSolverClassNew();
|
|
}
|
|
else {
|
|
assert(false);
|
|
}
|
|
}
|
|
}
|