kindred/solver
1
0
Fork 0
Code Issues 10 Pull Requests 1 Actions Packages Projects Releases Wiki Activity

runVelIC

Browse Source
This commit is contained in:
Aik-Siong Koh
2023-06-18 01:06:39 -06:00
parent 3b08cd72df
commit 371b13a9e0
147 changed files with 2555 additions and 238 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
MbDCode/AbsConstraint.cpp
View File

@@ -34,8 +34,8 @@ void MbD::AbsConstraint::useEquationNumbers()
void MbD::AbsConstraint::fillPosICJacob(SpMatDsptr mat)
{
(*(mat->at(iG)))[iqXminusOnePlusAxis] += 1.0;
(*(mat->at(iqXminusOnePlusAxis)))[iG] += 1.0;
mat->atijplusNumber(iG, iqXminusOnePlusAxis, 1.0);
mat->atijplusNumber(iqXminusOnePlusAxis, iG, 1.0);
}
void MbD::AbsConstraint::fillPosICError(FColDsptr col)
@@ -43,3 +43,8 @@ void MbD::AbsConstraint::fillPosICError(FColDsptr col)
Constraint::fillPosICError(col);
col->at(iqXminusOnePlusAxis) += lam;
}
void MbD::AbsConstraint::fillPosKineJacob(SpMatDsptr mat)
{
mat->atijplusNumber(iG, iqXminusOnePlusAxis, 1.0);
}

1
MbDCode/AbsConstraint.h
View File

@@ -13,6 +13,7 @@ namespace MbD {
void useEquationNumbers() override;
void fillPosICJacob(SpMatDsptr mat) override;
void fillPosICError(FColDsptr col) override;
void fillPosKineJacob(SpMatDsptr mat) override;
int axis = -1;
int iqXminusOnePlusAxis = -1;

70
MbDCode/Array.h
View File

@@ -7,7 +7,7 @@
namespace MbD {
template <typename T>
template<typename T>
class Array : public std::vector<T>
{
public:
@@ -18,11 +18,15 @@ namespace MbD {
Array(std::vector<T>::iterator begin, std::vector<T>::iterator end) : std::vector<T>(begin, end) {}
Array(std::initializer_list<T> list) : std::vector<T>{ list } {}
void copyFrom(std::shared_ptr<Array<T>> x);
virtual void zeroSelf() = 0;
virtual void zeroSelf();
virtual double sumOfSquares() = 0;
double rootMeanSquare();
virtual int numberOfElements() = 0;
void swapRows(int i, int ii);
virtual int numberOfElements();
void swapElems(int i, int ii);
//double maxMagnitude();
double maxMagnitudeOfVector();
void equalArrayAt(std::shared_ptr<Array<T>> array, int i);
};
template<typename T>
inline void Array<T>::copyFrom(std::shared_ptr<Array<T>> x)
@@ -32,19 +36,65 @@ namespace MbD {
}
}
template<typename T>
inline void Array<T>::zeroSelf()
{
for (int i = 0; i < this->size(); i++) {
this->at(i) = (T)0;
}
}
template<typename T>
inline double Array<T>::rootMeanSquare()
{
return std::sqrt(this->sumOfSquares() / this->numberOfElements());
}
template<typename T>
inline void Array<T>::swapRows(int i, int ii)
inline int Array<T>::numberOfElements()
{
return (int)this->size();
}
template<typename T>
inline void Array<T>::swapElems(int i, int ii)
{
auto temp = this->at(i);
this->at(i) = this->at(ii);
this->at(ii) = temp;
}
using ListD = std::initializer_list<double>;
using ListListD = std::initializer_list<std::initializer_list<double>>;
using ListListPairD = std::initializer_list<std::initializer_list<std::initializer_list<double>>>;
}
//template<typename T>
//inline double Array<T>::maxMagnitude()
//{
// if (std::is_arithmetic<T>::value) {
// return this->maxMagnitudeOfVector();
// }
// else {
// auto answer = 0.0;
// for (int i = 0; i < this->size(); i++)
// {
// auto mag = this->at(i)->maxMagnitude();
// if (answer < mag) answer = mag;
// }
// return answer;
// }
//}
template<typename T>
inline double Array<T>::maxMagnitudeOfVector()
{
auto answer = 0.0;
for (int i = 0; i < this->size(); i++)
{
auto mag = std::abs(this->at(i));
if (answer < mag) answer = mag;
}
return answer;
}
template<typename T>
inline void Array<T>::equalArrayAt(std::shared_ptr<Array<T>> array, int i)
{
for (int ii = 0; ii < this->size(); ii++)
{
this->at(ii) = array->at(i + ii);
}
}
using ListD = std::initializer_list<double>;
using ListListD = std::initializer_list<std::initializer_list<double>>;
using ListListPairD = std::initializer_list<std::initializer_list<std::initializer_list<double>>>;
}

6
MbDCode/AtPointConstraintIJ.cpp
View File

@@ -57,3 +57,9 @@ void MbD::AtPointConstraintIJ::postPosICIteration()
riIeJeO->postPosICIteration();
Item::postPosICIteration();
}
void MbD::AtPointConstraintIJ::preVelIC()
{
riIeJeO->preVelIC();
Item::preVelIC();
}

1
MbDCode/AtPointConstraintIJ.h
View File

@@ -19,6 +19,7 @@ namespace MbD {
void prePosIC() override;
MbD::ConstraintType type() override;
void postPosICIteration() override;
void preVelIC() override;
int axis;
std::shared_ptr<DispCompIecJecO> riIeJeO;

10
MbDCode/AtPointConstraintIqcJc.cpp
View File

@@ -42,9 +42,15 @@ void MbD::AtPointConstraintIqcJc::fillPosICError(FColDsptr col)
void MbD::AtPointConstraintIqcJc::fillPosICJacob(SpMatDsptr mat)
{
(*(mat->at(iG)))[iqXIminusOnePlusAxis] += -1.0;
(*(mat->at(iqXIminusOnePlusAxis)))[iG] += -1.0;
mat->atijplusNumber(iG, iqXIminusOnePlusAxis, -1.0);
mat->atijplusNumber(iqXIminusOnePlusAxis, iG, -1.0);
mat->atijplusFullRow(iG, iqEI, pGpEI);
mat->atijplusFullColumn(iqEI, iG, pGpEI->transpose());
mat->atijplusFullMatrixtimes(iqEI, iqEI, ppGpEIpEI, lam);
}
void MbD::AtPointConstraintIqcJc::fillPosKineJacob(SpMatDsptr mat)
{
mat->atijplusNumber(iG, iqXIminusOnePlusAxis, -1.0);
mat->atijplusFullRow(iG, iqEI, pGpEI);
}

1
MbDCode/AtPointConstraintIqcJc.h
View File

@@ -14,6 +14,7 @@ namespace MbD {
void useEquationNumbers() override;
void fillPosICError(FColDsptr col) override;
void fillPosICJacob(SpMatDsptr mat) override;
void fillPosKineJacob(SpMatDsptr mat) override;
FRowDsptr pGpEI;
FMatDsptr ppGpEIpEI;

11
MbDCode/AtPointConstraintIqcJqc.cpp
View File

@@ -44,9 +44,16 @@ void MbD::AtPointConstraintIqcJqc::fillPosICError(FColDsptr col)
void MbD::AtPointConstraintIqcJqc::fillPosICJacob(SpMatDsptr mat)
{
AtPointConstraintIqcJc::fillPosICJacob(mat);
(*(mat->at(iG)))[iqXJminusOnePlusAxis] += 1.0;
(*(mat->at(iqXJminusOnePlusAxis)))[iG] += 1.0;
mat->atijplusNumber(iG, iqXJminusOnePlusAxis, 1.0);
mat->atijplusNumber(iqXJminusOnePlusAxis, iG, 1.0);
mat->atijplusFullRow(iG, iqEJ, pGpEJ);
mat->atijplusFullColumn(iqEJ, iG, pGpEJ->transpose());
mat->atijplusFullMatrixtimes(iqEJ, iqEJ, ppGpEJpEJ, lam);
}
void MbD::AtPointConstraintIqcJqc::fillPosKineJacob(SpMatDsptr mat)
{
AtPointConstraintIqcJc::fillPosKineJacob(mat);
mat->atijplusNumber(iG, iqXJminusOnePlusAxis, 1.0);
mat->atijplusFullRow(iG, iqEJ, pGpEJ);
}

1
MbDCode/AtPointConstraintIqcJqc.h
View File

@@ -14,6 +14,7 @@ namespace MbD {
void useEquationNumbers() override;
void fillPosICError(FColDsptr col) override;
void fillPosICJacob(SpMatDsptr mat) override;
void fillPosKineJacob(SpMatDsptr mat) override;
FRowDsptr pGpEJ;
FMatDsptr ppGpEJpEJ;

8
MbDCode/AtPointConstraintIqctJqc.cpp
View File

@@ -12,7 +12,7 @@ AtPointConstraintIqctJqc::AtPointConstraintIqctJqc(EndFrmcptr frmi, EndFrmcptr f
void MbD::AtPointConstraintIqctJqc::initializeGlobally()
{
riIeJeO->initializeGlobally();
ppGpEJpEJ = (std::static_pointer_cast<DispCompIeqctJeqcO>(riIeJeO))->ppriIeJeOpEJpEJ;
ppGpEJpEJ = std::static_pointer_cast<DispCompIeqctJeqcO>(riIeJeO)->ppriIeJeOpEJpEJ;
}
void AtPointConstraintIqctJqc::initriIeJeO()
@@ -32,3 +32,9 @@ MbD::ConstraintType MbD::AtPointConstraintIqctJqc::type()
{
return MbD::essential;
}
void MbD::AtPointConstraintIqctJqc::preVelIC()
{
AtPointConstraintIJ::preVelIC();
pGpt = std::static_pointer_cast<DispCompIeqctJeqcO>(riIeJeO)->priIeJeOpt;
}

1
MbDCode/AtPointConstraintIqctJqc.h
View File

@@ -12,6 +12,7 @@ namespace MbD {
void initriIeJeO() override;
void calcPostDynCorrectorIteration() override;
MbD::ConstraintType type() override;
void preVelIC() override;
double pGpt;
FRowDsptr ppGpEIpt;

28
MbDCode/BasicIntegrator.cpp
View File

@@ -33,14 +33,14 @@ void MbD::BasicIntegrator::initializeGlobally()
//"Get info from system and prepare for start of simulation."
//"Integrator asks system for info. Not system setting integrator."
this->t = system->tstart;
this->sett(system->tstart);
this->direction = system->direction;
this->orderMax = system->orderMax();
}
void MbD::BasicIntegrator::setSystem(IntegratorInterface* sys)
void MbD::BasicIntegrator::setSystem(Solver* sys)
{
system = sys;
system = static_cast<IntegratorInterface*>(sys);
}
void MbD::BasicIntegrator::calcOperatorMatrix()
@@ -50,7 +50,6 @@ void MbD::BasicIntegrator::calcOperatorMatrix()
void MbD::BasicIntegrator::incrementTime()
{
//| istepNew |
tpast->insert(tpast->begin(), t);
if ((int)tpast->size() > (orderMax + 1)) { tpast->pop_back(); }
@@ -60,7 +59,7 @@ void MbD::BasicIntegrator::incrementTime()
h = hnew;
this->settnew(t + (direction * h));
this->calcOperatorMatrix();
//system->incrementTime(tnew);
system->incrementTime(tnew);
}
void MbD::BasicIntegrator::incrementTry()
@@ -110,7 +109,7 @@ void MbD::BasicIntegrator::preRun()
void MbD::BasicIntegrator::preStep()
{
assert(false);
system->preStep();
}
void MbD::BasicIntegrator::reportStats()
@@ -126,13 +125,24 @@ void MbD::BasicIntegrator::firstStep()
void MbD::BasicIntegrator::setorder(int o)
{
order = o;
//opBD->setorder(o);
opBDF->setorder(o);
}
void MbD::BasicIntegrator::settnew(double t)
{
tnew = t;
//this->time(double);
tnew = t;
this->settime(t);
}
void MbD::BasicIntegrator::sett(double tt)
{
t = tt;
opBDF->settime(tt);
}
void MbD::BasicIntegrator::settime(double tt)
{
opBDF->settime(tt);
}
void MbD::BasicIntegrator::subsequentSteps()

4
MbDCode/BasicIntegrator.h
View File

@@ -28,13 +28,15 @@ namespace MbD {
void run() override;
virtual void selectOrder();
virtual void subsequentSteps();
void setSystem(IntegratorInterface* sys);
void setSystem(Solver* sys) override;
void logString(std::string& str) override;
virtual void firstStep();
virtual void nextStep() = 0;
virtual void setorder(int o);
virtual void settnew(double t);
virtual void sett(double t);
void settime(double t);
IntegratorInterface* system;
int istep = 0, iTry = 0, maxTry = 0;

7
MbDCode/BasicQuasiIntegrator.cpp
View File

@@ -11,7 +11,7 @@ void MbD::BasicQuasiIntegrator::firstStep()
orderNew = 1;
this->selectFirstStepSize();
this->incrementTime();
//this->runInitialConditionTypeSolution();
this->runInitialConditionTypeSolution();
//this->reportTrialStepStats();
//while (this->isRedoingFirstStep())
@@ -48,6 +48,11 @@ void MbD::BasicQuasiIntegrator::nextStep()
//this->reportStepStats();
}
void MbD::BasicQuasiIntegrator::runInitialConditionTypeSolution()
{
system->runInitialConditionTypeSolution();
}
void MbD::BasicQuasiIntegrator::selectFirstStepSize()
{
if (iTry == 1) {

3
MbDCode/BasicQuasiIntegrator.h
View File

@@ -13,10 +13,9 @@ namespace MbD {
void nextStep();
//void reportStepStats();
//void reportTrialStepStats();
//void runInitialConditionTypeSolution();
void runInitialConditionTypeSolution() override;
void selectFirstStepSize();
//void selectStepSize();
//void runInitialConditionTypeSolution();
};
}

18
MbDCode/Constraint.cpp
View File

@@ -45,6 +45,14 @@ void MbD::Constraint::prePosIC()
Item::prePosIC();
}
void MbD::Constraint::prePosKine()
{
//"Preserve lam calculated from AccIC for possible use by DynIntegrator if system is not kinematic."
auto lamOld = lam;
this->prePosIC();
lam = lamOld;
}
void MbD::Constraint::fillEssenConstraints(std::shared_ptr<Constraint> sptr, std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> essenConstraints)
{
if (this->type() == MbD::essential) {
@@ -71,6 +79,11 @@ void MbD::Constraint::fillRedundantConstraints(std::shared_ptr<Constraint> sptr,
}
}
void MbD::Constraint::fillConstraints(std::shared_ptr<Constraint> sptr, std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> allConstraints)
{
allConstraints->push_back(sptr);
}
MbD::ConstraintType MbD::Constraint::type()
{
return MbD::essential;
@@ -123,3 +136,8 @@ void MbD::Constraint::preDyn()
{
mu = 0.0;
}
void MbD::Constraint::fillPosKineError(FColDsptr col)
{
col->atiplusNumber(iG, aG);
}

5
MbDCode/Constraint.h
View File

@@ -16,10 +16,12 @@ namespace MbD {
void setOwner(Item* x);
Item* getOwner();
void prePosIC() override;
void prePosKine() override;
virtual void fillEssenConstraints(std::shared_ptr<Constraint> sptr, std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> essenConstraints);
virtual void fillDispConstraints(std::shared_ptr<Constraint> sptr, std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> dispConstraints);
virtual void fillPerpenConstraints(std::shared_ptr<Constraint> sptr, std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> perpenConstraints);
virtual void fillRedundantConstraints(std::shared_ptr<Constraint> sptr, std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> perpenConstraints);
virtual void fillRedundantConstraints(std::shared_ptr<Constraint> sptr, std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> redunConstraints);
virtual void fillConstraints(std::shared_ptr<Constraint> sptr, std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> allConstraints);
virtual MbD::ConstraintType type();
void fillqsulam(FColDsptr col) override;
void setqsulam(FColDsptr col) override;
@@ -29,6 +31,7 @@ namespace MbD {
virtual bool isRedundant();
void outputStates() override;
void preDyn() override;
void fillPosKineError(FColDsptr col) override;
int iG = -1;
double aG = 0.0; //Constraint function

4
MbDCode/DiagonalMatrix.h
View File

@@ -4,7 +4,7 @@
#include "FullColumn.h"
namespace MbD {
template <typename T>
template<typename T>
class DiagonalMatrix : public Array<T>
{
//
@@ -65,7 +65,7 @@ namespace MbD {
inline void DiagonalMatrix<double>::zeroSelf()
{
for (int i = 0; i < this->size(); i++) {
this->at(i) = 0.0;;
this->at(i) = 0.0;
}
}
}

30
MbDCode/DifferenceOperator.cpp
View File

@@ -1,7 +1,21 @@
#include <cmath>
#include "DifferenceOperator.h"
#include "CREATE.h"
#include "SingularMatrixError.h"
#include "LDUFullMatParPv.h"
#include "FullRow.h"
using namespace MbD;
std::shared_ptr<FullRow<double>> DifferenceOperator::OneOverFactorials = []() {
auto oneOverFactorials = std::make_shared<FullRow<double>>(10);
for (int i = 0; i < oneOverFactorials->size(); i++)
{
oneOverFactorials->at(i) = 1.0 / std::tgamma(i+1);
}
return oneOverFactorials;
}();
void MbD::DifferenceOperator::calcOperatorMatrix()
{
@@ -12,8 +26,7 @@ void MbD::DifferenceOperator::calcOperatorMatrix()
this->formTaylorMatrix();
try {
assert(false);
//operatorMatrix = CREATE<LDUFullMatParPv>::With()->inversesaveOriginal(taylorMatrix, false);
operatorMatrix = CREATE<LDUFullMatParPv>::With()->inversesaveOriginal(taylorMatrix, false);
}
catch (SingularMatrixError ex) {
}
@@ -21,8 +34,6 @@ void MbD::DifferenceOperator::calcOperatorMatrix()
void MbD::DifferenceOperator::initialize()
{
//OneOverFactorials: = StMFullRow new : 10.
//1 to : OneOverFactorials size do : [:i | OneOverFactorials at : i put : 1.0d / i factorial]
}
void MbD::DifferenceOperator::setiStep(int i)
@@ -35,9 +46,9 @@ void MbD::DifferenceOperator::setorder(int o)
order = o;
}
void MbD::DifferenceOperator::instanciateTaylorMatrix()
void MbD::DifferenceOperator::instantiateTaylorMatrix()
{
if (taylorMatrix->empty() || (taylorMatrix->nrow() != (order + 1))) {
if (taylorMatrix == nullptr || (taylorMatrix->nrow() != (order + 1))) {
taylorMatrix = std::make_shared<FullMatrix<double>>(order + 1, order + 1);
}
}
@@ -45,7 +56,7 @@ void MbD::DifferenceOperator::instanciateTaylorMatrix()
void MbD::DifferenceOperator::formTaylorRowwithTimeNodederivative(int i, int ii, int k)
{
//| rowi hi hipower aij |
auto rowi = taylorMatrix->at(i);
auto& rowi = taylorMatrix->at(i);
for (int j = 0; j < k; j++)
{
rowi->at(j) = 0.0;
@@ -61,3 +72,8 @@ void MbD::DifferenceOperator::formTaylorRowwithTimeNodederivative(int i, int ii,
//rowi at : j put : aij
}
}
void MbD::DifferenceOperator::settime(double t)
{
time = t;
}

4
MbDCode/DifferenceOperator.h
View File

@@ -14,13 +14,15 @@ namespace MbD {
virtual void setiStep(int i);
virtual void setorder(int o);
virtual void formTaylorMatrix() = 0;
virtual void instanciateTaylorMatrix();
virtual void instantiateTaylorMatrix();
virtual void formTaylorRowwithTimeNodederivative(int i, int ii, int k);
void settime(double t);
int iStep = 0, order = 0;
std::shared_ptr<FullMatrix<double>> taylorMatrix, operatorMatrix;
double time = 0;
std::shared_ptr<std::vector<double>> timeNodes;
static std::shared_ptr<FullRow<double>> OneOverFactorials;
};
}

6
MbDCode/DirectionCosineConstraintIJ.cpp
View File

@@ -58,3 +58,9 @@ MbD::ConstraintType MbD::DirectionCosineConstraintIJ::type()
{
return MbD::perpendicular;
}
void MbD::DirectionCosineConstraintIJ::preVelIC()
{
aAijIeJe->preVelIC();
Item::preVelIC();
}

1
MbDCode/DirectionCosineConstraintIJ.h
View File

@@ -19,6 +19,7 @@ namespace MbD {
void prePosIC() override;
void postPosICIteration() override;
MbD::ConstraintType type() override;
void preVelIC() override;
int axisI, axisJ;
std::shared_ptr<DirectionCosineIecJec> aAijIeJe;

5
MbDCode/DirectionCosineConstraintIqcJc.cpp
View File

@@ -40,3 +40,8 @@ void MbD::DirectionCosineConstraintIqcJc::fillPosICJacob(SpMatDsptr mat)
mat->atijplusFullColumn(iqEI, iG, pGpEI->transpose());
mat->atijplusFullMatrixtimes(iqEI, iqEI, ppGpEIpEI, lam);
}
void MbD::DirectionCosineConstraintIqcJc::fillPosKineJacob(SpMatDsptr mat)
{
mat->atijplusFullRow(iG, iqEI, pGpEI);
}

1
MbDCode/DirectionCosineConstraintIqcJc.h
View File

@@ -13,6 +13,7 @@ namespace MbD {
void useEquationNumbers() override;
void fillPosICError(FColDsptr col) override;
void fillPosICJacob(SpMatDsptr mat) override;
void fillPosKineJacob(SpMatDsptr mat) override;
FRowDsptr pGpEI;
FMatDsptr ppGpEIpEI;

6
MbDCode/DirectionCosineConstraintIqcJqc.cpp
View File

@@ -46,3 +46,9 @@ void MbD::DirectionCosineConstraintIqcJqc::fillPosICJacob(SpMatDsptr mat)
mat->atijplusTransposeFullMatrix(iqEJ, iqEI, ppGpEIpEJlam);
mat->atijplusFullMatrixtimes(iqEJ, iqEJ, ppGpEJpEJ, lam);
}
void MbD::DirectionCosineConstraintIqcJqc::fillPosKineJacob(SpMatDsptr mat)
{
DirectionCosineConstraintIqcJc::fillPosKineJacob(mat);
mat->atijplusFullRow(iG, iqEJ, pGpEJ);
}

1
MbDCode/DirectionCosineConstraintIqcJqc.h
View File

@@ -13,6 +13,7 @@ namespace MbD {
void useEquationNumbers() override;
void fillPosICError(FColDsptr col) override;
void fillPosICJacob(SpMatDsptr mat) override;
void fillPosKineJacob(SpMatDsptr mat) override;
FRowDsptr pGpEJ;
FMatDsptr ppGpEIpEJ;

6
MbDCode/DirectionCosineConstraintIqctJqc.cpp
View File

@@ -18,3 +18,9 @@ MbD::ConstraintType MbD::DirectionCosineConstraintIqctJqc::type()
{
return MbD::essential;
}
void MbD::DirectionCosineConstraintIqctJqc::preVelIC()
{
DirectionCosineConstraintIJ::preVelIC();
pGpt = std::static_pointer_cast<DirectionCosineIeqctJeqc>(aAijIeJe)->pAijIeJept;
}

1
MbDCode/DirectionCosineConstraintIqctJqc.h
View File

@@ -10,6 +10,7 @@ namespace MbD {
DirectionCosineConstraintIqctJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj);
void initaAijIeJe() override;
MbD::ConstraintType type() override;
void preVelIC() override;
double pGpt;
FRowDsptr ppGpEIpt;

13
MbDCode/DirectionCosineIeqctJeqc.cpp
View File

@@ -1,5 +1,6 @@
#include "DirectionCosineIeqctJeqc.h"
#include "EndFrameqc.h"
#include "EndFrameqct.h"
using namespace MbD;
@@ -31,3 +32,15 @@ void MbD::DirectionCosineIeqctJeqc::calcPostDynCorrectorIteration()
ppAjOIepEIpEI = std::static_pointer_cast<EndFrameqc>(frmI)->ppAjOepEpE(axisI);
DirectionCosineIeqcJeqc::calcPostDynCorrectorIteration();
}
void MbD::DirectionCosineIeqctJeqc::preVelIC()
{
Item::preVelIC();
auto pAjOIept = std::static_pointer_cast<EndFrameqct>(frmI)->pAjOept(axisI);
pAijIeJept = pAjOIept->dot(aAjOJe);
}
double MbD::DirectionCosineIeqctJeqc::pvaluept()
{
return pAijIeJept;
}

2
MbDCode/DirectionCosineIeqctJeqc.h
View File

@@ -12,6 +12,8 @@ namespace MbD {
void initialize();
void initializeGlobally();
void calcPostDynCorrectorIteration() override;
void preVelIC() override;
double pvaluept();
double pAijIeJept;
FRowDsptr ppAijIeJepEIpt;

13
MbDCode/DispCompIeqcJeqcKeqct.cpp
View File

@@ -1,5 +1,6 @@
#include "DispCompIeqcJeqcKeqct.h"
#include "EndFrameqc.h"
#include "EndFrameqct.h"
using namespace MbD;
@@ -28,3 +29,15 @@ void MbD::DispCompIeqcJeqcKeqct::calcPostDynCorrectorIteration()
ppAjOKepEKpEK = efrmKqc->ppAjOepEpE(axisK);
DispCompIeqcJeqcKeqc::calcPostDynCorrectorIteration();
}
void MbD::DispCompIeqcJeqcKeqct::preVelIC()
{
Item::preVelIC();
auto pAjOKept = std::static_pointer_cast<EndFrameqct>(efrmK)->pAjOept(axisK);
priIeJeKept = pAjOKept->dot(rIeJeO);
}
double MbD::DispCompIeqcJeqcKeqct::pvaluept()
{
return priIeJeKept;
}

2
MbDCode/DispCompIeqcJeqcKeqct.h
View File

@@ -11,6 +11,8 @@ namespace MbD {
DispCompIeqcJeqcKeqct(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk);
void initialize() override;
void calcPostDynCorrectorIteration() override;
void preVelIC() override;
double pvaluept();
double priIeJeKept;
FRowDsptr ppriIeJeKepXIpt;

8
MbDCode/DispCompIeqctJeqcKeqct.cpp
View File

@@ -1,4 +1,5 @@
#include "DispCompIeqctJeqcKeqct.h"
#include "EndFrameqct.h"
using namespace MbD;
@@ -9,3 +10,10 @@ MbD::DispCompIeqctJeqcKeqct::DispCompIeqctJeqcKeqct()
MbD::DispCompIeqctJeqcKeqct::DispCompIeqctJeqcKeqct(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk) : DispCompIeqcJeqcKeqct(frmi, frmj, frmk, axisk)
{
}
void MbD::DispCompIeqctJeqcKeqct::preVelIC()
{
DispCompIeqcJeqcKeqct::preVelIC();
auto mprIeJeOpt = std::static_pointer_cast<EndFrameqct>(frmI)->prOeOpt;
priIeJeKept -= aAjOKe->dot(mprIeJeOpt);
}

1
MbDCode/DispCompIeqctJeqcKeqct.h
View File

@@ -9,6 +9,7 @@ namespace MbD {
public:
DispCompIeqctJeqcKeqct();
DispCompIeqctJeqcKeqct(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk);
void preVelIC() override;
};
}

11
MbDCode/DispCompIeqctJeqcO.cpp
View File

@@ -23,3 +23,14 @@ void MbD::DispCompIeqctJeqcO::calcPostDynCorrectorIteration()
DispCompIeqcJeqcO::calcPostDynCorrectorIteration();
ppriIeJeOpEIpEI = std::static_pointer_cast<EndFrameqct>(frmI)->ppriOeOpEpE(axis)->negated();
}
void MbD::DispCompIeqctJeqcO::preVelIC()
{
Item::preVelIC();
priIeJeOpt = -(std::static_pointer_cast<EndFrameqct>(frmI)->priOeOpt(axis));
}
double MbD::DispCompIeqctJeqcO::pvaluept()
{
return priIeJeOpt;
}

2
MbDCode/DispCompIeqctJeqcO.h
View File

@@ -11,6 +11,8 @@ namespace MbD {
DispCompIeqctJeqcO(EndFrmcptr frmi, EndFrmcptr frmj, int axis);
void initializeGlobally() override;
void calcPostDynCorrectorIteration() override;
void preVelIC() override;
double pvaluept();
double priIeJeOpt;
FRowDsptr ppriIeJeOpEIpt;

4
MbDCode/EndFramec.h
View File

@@ -8,9 +8,9 @@
namespace MbD {
class MarkerFrame;
//template <typename T>
//template<typename T>
//class FullColumn;
//template <typename T>
//template<typename T>
//class FullMatrix;
class EndFramec : public CartesianFrame

57
MbDCode/EndFrameqct.cpp
View File

@@ -6,6 +6,7 @@
#include "EulerParameters.h"
#include "CREATE.h"
#include "EulerAngleszxz.h"
#include "EulerAngleszxzDot.h"
using namespace MbD;
@@ -159,3 +160,59 @@ void MbD::EndFrameqct::evalAme()
aAme = phiThePsi->aA;
}
}
void MbD::EndFrameqct::preVelIC()
{
time = System::getInstance().mbdTimeValue();
this->evalrmem();
this->evalAme();
Item::preVelIC();
this->evalprmempt();
this->evalpAmept();
auto aAOm = markerFrame->aAOm;
prOeOpt = aAOm->timesFullColumn(prmempt);
pAOept = aAOm->timesFullMatrix(pAmept);
}
FColDsptr MbD::EndFrameqct::pAjOept(int j)
{
return pAOept->column(j);
}
double MbD::EndFrameqct::priOeOpt(int i)
{
return prOeOpt->at(i);
}
void MbD::EndFrameqct::evalprmempt()
{
if (rmemBlks) {
for (int i = 0; i < 3; i++)
{
auto& derivative = prmemptBlks->at(i);
auto value = derivative->getValue();
prmempt->at(i) = value;
}
}
}
void MbD::EndFrameqct::evalpAmept()
{
if (phiThePsiBlks) {
auto phiThePsi = CREATE<EulerAngleszxz<double>>::With();
auto phiThePsiDot = CREATE<EulerAngleszxzDot<double>>::With();
phiThePsiDot->phiThePsi = phiThePsi;
for (int i = 0; i < 3; i++)
{
auto& expression = phiThePsiBlks->at(i);
auto& derivative = pPhiThePsiptBlks->at(i);
auto value = expression->getValue();
auto valueDot = derivative->getValue();
phiThePsi->at(i) = value;
phiThePsiDot->at(i) = valueDot;
}
phiThePsi->calc();
phiThePsiDot->calc();
pAmept = phiThePsiDot->aAdot;
}
}

12
MbDCode/EndFrameqct.h
View File

@@ -24,14 +24,20 @@ namespace MbD {
void prePosIC() override;
void evalrmem();
void evalAme();
void preVelIC() override;
FColDsptr pAjOept(int j);
double time = 0.0;
double priOeOpt(int i);
void evalprmempt();
void evalpAmept();
std::shared_ptr<FullColumn<Symsptr>> rmemBlks, prmemptBlks, pprmemptptBlks;
std::shared_ptr<FullColumn<Symsptr>> phiThePsiBlks, pPhiThePsiptBlks, ppPhiThePsiptptBlks;
FColDsptr rmem, prmempt, pprmemptpt, pprOeOptpt;
FMatDsptr aAme, pAmept, ppAmeptpt, ppAOeptpt;
FColDsptr rmem, prmempt, pprmemptpt, prOeOpt, pprOeOptpt;
FMatDsptr aAme, pAmept, ppAmeptpt, pAOept, ppAOeptpt;
FMatDsptr pprOeOpEpt;
std::shared_ptr<FullColumn<std::shared_ptr<FullMatrix<double>>>> ppAOepEpt;
};
using EndFrmqctptr = std::shared_ptr<EndFrameqct>;
}

2
MbDCode/EulerAngleszxz.h
View File

@@ -6,7 +6,7 @@
namespace MbD {
template <typename T>
template<typename T>
class EulerAngleszxz : public EulerArray<T>
{
//phiA theA psiA aA

1
MbDCode/EulerAngleszxzDDot.cpp Normal file
View File

@@ -0,0 +1 @@
#include "EulerAngleszxzDDot.h"

18
MbDCode/EulerAngleszxzDDot.h Normal file
View File

@@ -0,0 +1,18 @@
#pragma once
#include "EulerArray.h"
#include "EulerAngleszxzDot.h"
namespace MbD {
template<typename T>
class EulerAngleszxzDDot : public EulerArray<T>
{
//phiThePsiDot phiAddot theAddot psiAddot aAddot
public:
std::shared_ptr<EulerAngleszxzDot<double>> phiThePsiDot;
FMatDsptr phiAddot, theAddot, psiAddot, aAddot;
};
}

1
MbDCode/EulerAngleszxzDot.cpp Normal file
View File

@@ -0,0 +1 @@
#include "EulerAngleszxzDot.h"

73
MbDCode/EulerAngleszxzDot.h Normal file
View File

@@ -0,0 +1,73 @@
#pragma once
#include "EulerArray.h"
#include "EulerAngleszxz.h"
namespace MbD {
template<typename T>
class EulerAngleszxzDot : public EulerArray<T>
{
//phiThePsi phiAdot theAdot psiAdot aAdot
public:
EulerAngleszxzDot() : EulerArray<T>(3) {}
void initialize() override;
void calc();
std::shared_ptr<EulerAngleszxz<T>> phiThePsi;
FMatDsptr phiAdot, theAdot, psiAdot, aAdot;
};
template<typename T>
inline void EulerAngleszxzDot<T>::initialize()
{
phiAdot = std::make_shared<FullMatrix<double>>(3, 3);
phiAdot->zeroSelf();
theAdot = std::make_shared<FullMatrix<double>>(3, 3);
theAdot->zeroSelf();
psiAdot = std::make_shared<FullMatrix<double>>(3, 3);
psiAdot->zeroSelf();
}
template<typename T>
inline void EulerAngleszxzDot<T>::calc()
{
//| zero phi sphi cphi phidot minussphiTimesphidot cphiTimesphidot the sthe cthe thedot minusstheTimesthedot ctheTimesthedot psi spsi cpsi psidot minusspsiTimespsidot cpsiTimespsidot phiA theA psiA |
auto phi = phiThePsi->at(0);
auto sphi = std::sin(phi);
auto cphi = std::cos(phi);
auto phidot = this->at(0);
auto minussphiTimesphidot = -(sphi * phidot);
auto cphiTimesphidot = cphi * phidot;
auto the = phiThePsi->at(1);
auto sthe = std::sin(the);
auto cthe = std::cos(the);
auto thedot = this->at(1);
auto minusstheTimesthedot = -(sthe * thedot);
auto ctheTimesthedot = cthe * thedot;
auto psi = phiThePsi->at(2);
auto spsi = std::sin(psi);
auto cpsi = std::cos(psi);
auto psidot = this->at(2);
auto minusspsiTimespsidot = -(spsi * psidot);
auto cpsiTimespsidot = cpsi * psidot;
phiAdot->at(0)->at(0) = minussphiTimesphidot;
phiAdot->at(0)->at(1) = -cphiTimesphidot;
phiAdot->at(1)->at(0) = cphiTimesphidot;
phiAdot->at(1)->at(1) = minussphiTimesphidot;
theAdot->at(1)->at(1) = minusstheTimesthedot;
theAdot->at(1)->at(2) = -ctheTimesthedot;
theAdot->at(2)->at(1) = ctheTimesthedot;
theAdot->at(2)->at(2) = minusstheTimesthedot;
psiAdot->at(0)->at(0) = minusspsiTimespsidot;
psiAdot->at(0)->at(1) = -cpsiTimespsidot;
psiAdot->at(1)->at(0) = cpsiTimespsidot;
psiAdot->at(1)->at(1) = minusspsiTimespsidot;
auto phiA = phiThePsi->phiA;
auto theA = phiThePsi->theA;
auto psiA = phiThePsi->psiA;
auto term1 = phiAdot->timesFullMatrix(theA->timesFullMatrix(psiA));
auto term2 = phiA->timesFullMatrix(theAdot->timesFullMatrix(psiA));
auto term3 = phiA->timesFullMatrix(theA->timesFullMatrix(psiAdot));
aAdot = (term1->plusFullMatrix(term2))->plusFullMatrix(term3);
}
}

15
MbDCode/EulerArray.h
View File

@@ -3,7 +3,7 @@
namespace MbD {
template <typename T>
template<typename T>
class EulerArray : public FullColumn<T>
{
//
@@ -12,10 +12,23 @@ namespace MbD {
EulerArray(int count, const T& value) : FullColumn<T>(count, value) {}
EulerArray(std::initializer_list<T> list) : FullColumn<T>{ list } {}
virtual void initialize();
void equalFullColumn(std::shared_ptr<FullColumn<T>> fullCol);
void equalFullColumnAt(std::shared_ptr<FullColumn<T>> fullCol, int i);
};
template<typename T>
inline void EulerArray<T>::initialize()
{
}
template<typename T>
inline void EulerArray<T>::equalFullColumn(std::shared_ptr<FullColumn<T>> fullCol)
{
this->equalArrayAt(fullCol, 0);
}
template<typename T>
inline void EulerArray<T>::equalFullColumnAt(std::shared_ptr<FullColumn<T>> fullCol, int i)
{
this->equalArrayAt(fullCol, i);
}
}

7
MbDCode/EulerConstraint.cpp
View File

@@ -49,6 +49,11 @@ void MbD::EulerConstraint::fillPosICJacob(SpMatDsptr mat)
for (int i = 0; i < 4; i++)
{
auto ii = iqE + i;
(*(mat->at(ii)))[ii] += twolam;
mat->atijplusNumber(ii, ii, twolam);
}
}
void MbD::EulerConstraint::fillPosKineJacob(SpMatDsptr mat)
{
mat->atijplusFullRow(iG, iqE, pGpE);
}

1
MbDCode/EulerConstraint.h
View File

@@ -17,6 +17,7 @@ namespace MbD {
void useEquationNumbers() override;
void fillPosICError(FColDsptr col) override;
void fillPosICJacob(SpMatDsptr mat) override;
void fillPosKineJacob(SpMatDsptr mat) override;
FRowDsptr pGpE; //partial derivative of G wrt pE
int iqE = -1;

9
MbDCode/EulerParameters.h
View File

@@ -5,7 +5,7 @@
namespace MbD {
template <typename T>
template<typename T>
class EulerParameters : public EulerArray<T>
{
//aA aB aC pApE
@@ -19,6 +19,7 @@ namespace MbD {
static std::shared_ptr<FullMatrix<std::shared_ptr<FullMatrix<T>>>> ppApEpEtimesMatrix(FMatDsptr mat);
void initialize();
void calc();
void calcABC();
void calcpApE();
@@ -127,6 +128,12 @@ namespace MbD {
pApE->at(i) = std::make_shared<FullMatrix<double>>(3, 3);
}
}
template<typename T>
inline void EulerParameters<T>::calc()
{
this->calcABC();
this->calcpApE();
}
template<>
inline void EulerParameters<double>::calcABC()
{

67
MbDCode/EulerParametersDot.h
View File

@@ -4,28 +4,59 @@
#include "FullColumn.h"
#include "FullMatrix.h"
#include "EulerParameters.h"
#include "CREATE.h"
namespace MbD {
template <typename T>
class EulerParametersDot : public EulerArray<T>
{
//qE aAdot aBdot aCdot pAdotpE
public:
EulerParametersDot(int count) : EulerArray<T>(count) {}
EulerParametersDot(int count, const T& value) : EulerArray<T>(count, value) {}
EulerParametersDot(std::initializer_list<T> list) : EulerArray<T>{ list } {}
static std::shared_ptr<EulerParametersDot<T>> FromqEOpAndOmegaOpO(std::shared_ptr<EulerParameters<T>> qe, FColDsptr omeOpO);
std::shared_ptr<EulerParameters<T>> qE;
FMatDsptr aAdot, aBdot, aCdot;
FColFMatDsptr pAdotpE;
};
template<typename T>
class EulerParametersDot : public EulerArray<T>
{
//qE aAdot aBdot aCdot pAdotpE
public:
EulerParametersDot(int count) : EulerArray<T>(count) {}
EulerParametersDot(int count, const T& value) : EulerArray<T>(count, value) {}
EulerParametersDot(std::initializer_list<T> list) : EulerArray<T>{ list } {}
static std::shared_ptr<EulerParametersDot<T>> FromqEOpAndOmegaOpO(std::shared_ptr<EulerParameters<T>> qe, FColDsptr omeOpO);
void calcAdotBdotCdot();
void calcpAdotpE();
template<typename T>
inline std::shared_ptr<EulerParametersDot<T>> EulerParametersDot<T>::FromqEOpAndOmegaOpO(std::shared_ptr<EulerParameters<T>> qe, FColDsptr omeOpO)
{
return std::shared_ptr<EulerParametersDot<T>>();
}
std::shared_ptr<EulerParameters<T>> qE;
FMatDsptr aAdot, aBdot, aCdot;
FColFMatDsptr pAdotpE;
void calc();
};
template<typename T>
inline std::shared_ptr<EulerParametersDot<T>> EulerParametersDot<T>::FromqEOpAndOmegaOpO(std::shared_ptr<EulerParameters<T>> qEOp, FColDsptr omeOpO)
{
auto answer = CREATE<EulerParametersDot<T>>::With(4);
qEOp->calcABC();
auto aB = qEOp->aB;
answer->equalFullColumn(aB->transposeTimesFullColumn(omeOpO->times(0.5)));
answer->qE = qEOp;
answer->calc();
return answer;
}
template<typename T>
inline void EulerParametersDot<T>::calcAdotBdotCdot()
{
assert(false);
}
template<typename T>
inline void EulerParametersDot<T>::calcpAdotpE()
{
assert(false);
}
template<typename T>
inline void EulerParametersDot<T>::calc()
{
qE->calc();
this->calcAdotBdotCdot();
this->calcpAdotpE();
}
}

33
MbDCode/FullColumn.h
View File

@@ -3,10 +3,13 @@
#include <sstream>
#include "FullVector.h"
//#include "FullRow.h"
namespace MbD {
template<typename T>
class FullRow;
template <typename T>
template<typename T>
class FullColumn : public FullVector<T>
{
public:
@@ -20,11 +23,13 @@ namespace MbD {
std::shared_ptr<FullColumn<T>> times(double a);
std::shared_ptr<FullColumn<T>> negated();
void atiputFullColumn(int i, std::shared_ptr<FullColumn<T>> fullCol);
void atiplusFullColumn(int i, std::shared_ptr<FullColumn<T>> fullCol);
void equalSelfPlusFullColumnAt(std::shared_ptr<FullColumn<T>> fullCol, int i);
void atiminusFullColumn(int i, std::shared_ptr<FullColumn<T>> fullCol);
void equalFullColumnAt(std::shared_ptr<FullColumn<T>> fullCol, int i);
std::shared_ptr<FullColumn<T>> copy();
std::shared_ptr<FullRow<T>> transpose();
virtual std::ostream& printOn(std::ostream& s) const;
friend std::ostream& operator<<(std::ostream& s, const FullColumn& fullCol)
{
@@ -76,6 +81,14 @@ namespace MbD {
}
}
template<typename T>
inline void FullColumn<T>::atiplusFullColumn(int i, std::shared_ptr<FullColumn<T>> fullCol)
{
for (int ii = 0; ii < fullCol->size(); ii++)
{
this->at(i + ii) += fullCol->at(ii);
}
}
template<typename T>
inline void FullColumn<T>::equalSelfPlusFullColumnAt(std::shared_ptr<FullColumn<T>> fullCol, int ii)
{
//self is subcolumn of fullCol
@@ -94,12 +107,13 @@ namespace MbD {
}
}
template<typename T>
inline void FullColumn<T>::equalFullColumnAt(std::shared_ptr<FullColumn<T>> fullCol, int ii)
inline void FullColumn<T>::equalFullColumnAt(std::shared_ptr<FullColumn<T>> fullCol, int i)
{
for (int i = 0; i < this->size(); i++)
{
this->at(i) = fullCol->at(ii + i);
}
this->equalArrayAt(fullCol, i);
//for (int ii = 0; ii < this->size(); ii++)
//{
// this->at(ii) = fullCol->at(i + ii);
//}
}
template<>
inline std::shared_ptr<FullColumn<double>> FullColumn<double>::copy()
@@ -113,6 +127,11 @@ namespace MbD {
return answer;
}
template<typename T>
inline std::shared_ptr<FullRow<T>> FullColumn<T>::transpose()
{
return std::make_shared<FullRow<T>>(*this);
}
template<typename T>
inline std::ostream& FullColumn<T>::printOn(std::ostream& s) const
{
s << "{";

70
MbDCode/FullMatrix.h
View File

@@ -4,17 +4,17 @@
#include "RowTypeMatrix.h"
namespace MbD {
template <typename T>
template<typename T>
class FullColumn;
template <typename T>
template<typename T>
class FullRow;
template <typename T>
template<typename T>
class FullMatrix : public RowTypeMatrix<std::shared_ptr<FullRow<T>>>
{
public:
FullMatrix() {}
FullMatrix(int m) : RowTypeMatrix<std::shared_ptr<FullRow<T>>>(m)
FullMatrix(int m) : RowTypeMatrix<std::shared_ptr<FullRow<T>>>(m)
{
}
FullMatrix(int m, int n) {
@@ -47,10 +47,17 @@ namespace MbD {
std::shared_ptr<FullMatrix<T>> negated();
void symLowerWithUpper();
void atijputFullColumn(int i, int j, std::shared_ptr<FullColumn<T>> fullCol);
void atijplusFullRow(int i, int j, std::shared_ptr<FullRow<T>> fullRow);
void atijplusNumber(int i, int j, double value);
void atijminusNumber(int i, int j, double value);
double sumOfSquares() override;
void zeroSelf() override;
std::shared_ptr<FullMatrix<T>> copy();
std::shared_ptr<FullMatrix<T>> operator+(const std::shared_ptr<FullMatrix<T>> fullMat);
std::shared_ptr<FullColumn<T>> transposeTimesFullColumn(const std::shared_ptr<FullColumn<T>> fullCol);
};
template <>
template<>
inline void FullMatrix<double>::identity() {
this->zeroSelf();
for (int i = 0; i < this->size(); i++) {
@@ -59,7 +66,7 @@ namespace MbD {
}
template<typename T>
inline std::shared_ptr<FullColumn<T>> FullMatrix<T>::column(int j) {
int n = (int) this->size();
int n = (int)this->size();
auto answer = std::make_shared<FullColumn<T>>(n);
for (int i = 0; i < n; i++) {
answer->at(i) = this->at(i)->at(j);
@@ -70,7 +77,7 @@ namespace MbD {
inline std::shared_ptr<FullColumn<T>> FullMatrix<T>::timesFullColumn(std::shared_ptr<FullColumn<T>> fullCol)
{
//"a*b = a(i,j)b(j) sum j."
int n = (int) this->size();
int n = (int)this->size();
auto answer = std::make_shared<FullColumn<T>>(n);
for (int i = 0; i < n; i++)
{
@@ -111,7 +118,7 @@ namespace MbD {
template<typename T>
inline std::shared_ptr<FullMatrix<T>> FullMatrix<T>::plusFullMatrix(std::shared_ptr<FullMatrix<T>> fullMat)
{
int n = (int) this->size();
int n = (int)this->size();
auto answer = std::make_shared<FullMatrix<T>>(n);
for (int i = 0; i < n; i++) {
answer->at(i) = this->at(i)->plusFullRow(fullMat->at(i));
@@ -140,7 +147,7 @@ namespace MbD {
template<typename T>
inline void FullMatrix<T>::symLowerWithUpper()
{
int n = (int) this->size();
int n = (int)this->size();
for (int i = 0; i < n; i++) {
for (int j = i + 1; j < n; j++) {
this->at(j)->at(i) = this->at(i)->at(j);
@@ -150,12 +157,28 @@ namespace MbD {
template<typename T>
inline void FullMatrix<T>::atijputFullColumn(int i1, int j1, std::shared_ptr<FullColumn<T>> fullCol)
{
auto iOffset = i1 - 1;
for (int ii = 0; ii < fullCol->size(); ii++)
{
this->at(iOffset + ii)->at(j1) = fullCol->at(ii);
this->at(i1 + ii)->at(j1) = fullCol->at(ii);
}
}
template<typename T>
inline void FullMatrix<T>::atijplusFullRow(int i, int j, std::shared_ptr<FullRow<T>> fullRow)
{
this->at(i)->atiplusFullRow(j, fullRow);
}
template<typename T>
inline void FullMatrix<T>::atijplusNumber(int i, int j, double value)
{
auto rowi = this->at(i);
rowi->at(j) += value;
}
template<typename T>
inline void FullMatrix<T>::atijminusNumber(int i, int j, double value)
{
auto rowi = this->at(i);
rowi->at(j) -= value;
}
template<>
inline double FullMatrix<double>::sumOfSquares()
{
@@ -184,6 +207,31 @@ namespace MbD {
{
assert(false);
}
template<typename T>
inline std::shared_ptr<FullMatrix<T>> FullMatrix<T>::copy()
{
auto m = (int)this->size();
auto answer = std::make_shared<FullMatrix<T>>(m);
for (int i = 0; i < m; i++)
{
answer->at(i) = this->at(i)->copy();
}
return answer;
}
template<typename T>
inline std::shared_ptr<FullMatrix<T>> FullMatrix<T>::operator+(const std::shared_ptr<FullMatrix<T>> fullMat)
{
return this->plusFullMatrix(fullMat);
}
template<typename T>
inline std::shared_ptr<FullColumn<T>> FullMatrix<T>::transposeTimesFullColumn(std::shared_ptr<FullColumn<T>> fullCol)
{
auto sptr = std::make_shared<FullMatrix<T>>(*this);
//auto aaa = fullCol->transpose();
//auto bbb = aaa->timesFullMatrix(sptr);
//auto ccc = bbb->transpose();
return fullCol->transpose()->timesFullMatrix(sptr)->transpose();
}
using FMatDsptr = std::shared_ptr<FullMatrix<double>>;
using FMatDsptr = std::shared_ptr<FullMatrix<double>>;
using FMatFColDsptr = std::shared_ptr<FullMatrix<std::shared_ptr<FullColumn<double>>>>;

33
MbDCode/FullRow.h
View File

@@ -1,18 +1,21 @@
#pragma once
#include "FullVector.h"
#include "FullColumn.h"
//#include "FullColumn.h"
namespace MbD {
template <typename T>
template<typename T>
class FullMatrix;
template<typename T>
class FullColumn;
template <typename T>
template<typename T>
class FullRow : public FullVector<T>
{
public:
FullRow() {}
FullRow(std::vector<T> vec) : FullVector<T>(vec) {}
FullRow(int count) : FullVector<T>(count) {}
FullRow(int count, const T& value) : FullVector<T>(count, value) {}
FullRow(std::vector<T>::iterator begin, std::vector<T>::iterator end) : FullVector<T>(begin, end) {}
FullRow(std::initializer_list<T> list) : FullVector<T>{ list } {}
std::shared_ptr<FullRow<T>> times(double a);
std::shared_ptr<FullRow<T>> negated();
@@ -23,6 +26,8 @@ namespace MbD {
std::shared_ptr<FullRow<T>> timesTransposeFullMatrix(std::shared_ptr<FullMatrix<T>> fullMat);
void equalSelfPlusFullRowTimes(std::shared_ptr<FullRow<T>> fullRow, double factor);
std::shared_ptr<FullColumn<T>> transpose();
std::shared_ptr<FullRow<T>> copy();
void atiplusFullRow(int j, std::shared_ptr<FullRow<T>> fullRow);
};
template<typename T>
@@ -94,6 +99,26 @@ namespace MbD {
{
return std::make_shared<FullColumn<T>>(*this);
}
template<>
inline std::shared_ptr<FullRow<double>> FullRow<double>::copy()
{
auto n = (int)this->size();
auto answer = std::make_shared<FullRow<double>>(n);
for (int i = 0; i < n; i++)
{
answer->at(i) = this->at(i);
}
return answer;
}
template<typename T>
inline void FullRow<T>::atiplusFullRow(int j1, std::shared_ptr<FullRow<T>> fullRow)
{
for (int jj = 0; jj < fullRow->size(); jj++)
{
auto j = j1 + jj;
this->at(j) += fullRow->at(jj);
}
}
template<typename T>
inline std::shared_ptr<FullRow<T>> FullRow<T>::timesFullMatrix(std::shared_ptr<FullMatrix<T>> fullMat)
{

22
MbDCode/FullVector.h
View File

@@ -2,7 +2,7 @@
#include "Array.h"
namespace MbD {
template <typename T>
template<typename T>
class FullVector : public Array<T>
{
public:
@@ -14,11 +14,13 @@ namespace MbD {
FullVector(std::initializer_list<T> list) : Array<T>{ list } {}
double dot(std::shared_ptr<FullVector<T>> vec);
void atiplusNumber(int i, T value);
void atiminusNumber(int i, T value);
double sumOfSquares() override;
int numberOfElements() override;
void zeroSelf() override;
void atitimes(int i, double factor);
void atiplusFullVectortimes(int i, std::shared_ptr<FullVector> fullVec, double factor);
double maxMagnitude();
};
template<typename T>
@@ -36,6 +38,11 @@ namespace MbD {
{
this->at(i) += value;
}
template<typename T>
inline void FullVector<T>::atiminusNumber(int i, T value)
{
this->at(i) -= value;
}
template<>
inline double FullVector<double>::sumOfSquares()
{
@@ -62,7 +69,7 @@ namespace MbD {
inline void FullVector<double>::zeroSelf()
{
for (int i = 0; i < this->size(); i++) {
this->at(i) = 0.0;;
this->at(i) = 0.0;
}
}
template<typename T>
@@ -84,4 +91,15 @@ namespace MbD {
this->at(i) += fullVec->at(ii) * factor;
}
}
template<>
inline double FullVector<double>::maxMagnitude()
{
auto answer = 0.0;
for (int i = 0; i < this->size(); i++)
{
auto mag = std::abs(this->at(i));
if (answer < mag) answer = mag;
}
return answer;
}
}

42
MbDCode/GEFullMat.cpp
View File

@@ -11,7 +11,18 @@ void MbD::GEFullMat::forwardEliminateWithPivot(int p)
void MbD::GEFullMat::backSubstituteIntoDU()
{
assert(false);
answerX = std::make_shared<FullColumn<double>>(n);
answerX->at(n - 1) = rightHandSideB->at(m - 1) / matrixA->at(m - 1)->at(n - 1);
for (int i = n - 2; i >= 0; i--)
{
auto rowi = matrixA->at(i);
double sum = answerX->at(n) * rowi->at(n);
for (int j = i + 1; j < n - 1; j++)
{
sum += answerX->at(j) * rowi->at(j);
}
answerX->at(i) = (rightHandSideB->at(i) - sum) / rowi->at(i);
}
}
void MbD::GEFullMat::postSolve()
@@ -19,7 +30,36 @@ void MbD::GEFullMat::postSolve()
assert(false);
}
void MbD::GEFullMat::preSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal)
{
assert(false);
}
void MbD::GEFullMat::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal)
{
assert(false);
}
double MbD::GEFullMat::getmatrixArowimaxMagnitude(int i)
{
return matrixA->at(i)->maxMagnitude();
}
FColDsptr MbD::GEFullMat::basicSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal)
{
this->preSolvewithsaveOriginal(fullMat, fullCol, saveOriginal);
for (int p = 0; p < m; p++)
{
this->doPivoting(p);
this->forwardEliminateWithPivot(p);
}
this->backSubstituteIntoDU();
this->postSolve();
return answerX;
}
FColDsptr MbD::GEFullMat::basicSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal)
{
assert(false);
return FColDsptr();
}

7
MbDCode/GEFullMat.h
View File

@@ -7,11 +7,16 @@ namespace MbD {
{
//
public:
std::shared_ptr<FullMatrix<double>> matrixA;
void forwardEliminateWithPivot(int p) override;
void backSubstituteIntoDU() override;
void postSolve() override;
FColDsptr basicSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal) override;
FColDsptr basicSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) override;
void preSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal) override;
void preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) override;
double getmatrixArowimaxMagnitude(int i) override;
std::shared_ptr<FullMatrix<double>> matrixA;
};
}

38
MbDCode/GEFullMatFullPv.cpp
View File

@@ -1,12 +1,48 @@
#include <cassert>
#include "GEFullMatFullPv.h"
#include "SingularMatrixError.h"
using namespace MbD;
void MbD::GEFullMatFullPv::doPivoting(int p)
{
assert(false);
//"Do full pivoting."
//| max pivotRow pivotCol rowi aij mag |
double max = 0.0;
int pivotRow = p;
int pivotCol = p;
for (int i = p; i < m; i++)
{
auto rowi = matrixA->at(i);
for (int j = p; j < n; j++)
{
auto aij = rowi->at(j);
if (aij != 0.0) {
auto mag = aij;
if (mag < 0.0) mag = -mag;
if (max < mag) {
max = mag;
pivotRow = i;
pivotCol = j;
}
}
}
}
if (p != pivotRow) {
matrixA->swapElems(p, pivotRow);
rightHandSideB->swapElems(p, pivotRow);
rowOrder->swapElems(p, pivotRow);
}
if (p != pivotCol) {
for (auto& rowi : *matrixA) {
rowi->swapElems(p, pivotCol);
}
colOrder->swapElems(p, pivotCol);
}
pivotValues->at(p) = max;
if (max < singularPivotTolerance) throw SingularMatrixError("");
}
void MbD::GEFullMatFullPv::postSolve()

31
MbDCode/GEFullMatParPv.cpp
View File

@@ -1,12 +1,39 @@
#include <cassert>
#include "GEFullMatParPv.h"
#include "SingularMatrixError.h"
using namespace MbD;
void MbD::GEFullMatParPv::doPivoting(int p)
{
assert(false);
//"Use scalings. Do row pivoting."
//| app max rowPivot aip mag |
auto app = matrixA->at(p)->at(p);
auto max = app * rowScalings->at(p);
if (max < 0.0) max = -max;
auto rowPivot = p;
for (int i = p + 1; i < m; i++)
{
auto aip = matrixA->at(i)->at(p);
if (aip != 0.0) {
auto mag = aip * rowScalings->at(i);
if (mag < 0) mag = -mag;
if (max < mag) {
max = mag;
rowPivot = i;
}
}
}
if (p != rowPivot) {
matrixA->swapElems(p, rowPivot);
rightHandSideB->swapElems(p, rowPivot);
rowScalings->swapElems(p, rowPivot);
rowOrder->swapElems(p, rowPivot);
}
pivotValues->at(p) = max;
if (max < singularPivotTolerance) throw SingularMatrixError("");
}
void MbD::GEFullMatParPv::postSolve()
@@ -14,7 +41,7 @@ void MbD::GEFullMatParPv::postSolve()
assert(false);
}
void MbD::GEFullMatParPv::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal)
void MbD::GEFullMatParPv::preSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal)
{
assert(false);
}

2
MbDCode/GEFullMatParPv.h
View File

@@ -9,7 +9,7 @@ namespace MbD {
public:
void doPivoting(int p) override;
void postSolve() override;
void preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) override;
void preSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal) override;
};
}

34
MbDCode/GESpMat.cpp
View File

@@ -9,3 +9,37 @@ FColDsptr MbD::GESpMat::solvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCo
this->timedSolvewithsaveOriginal(spMat, fullCol, saveOriginal);
return answerX;
}
FColDsptr MbD::GESpMat::basicSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal)
{
this->preSolvewithsaveOriginal(spMat, fullCol, saveOriginal);
for (int p = 0; p < m; p++)
{
this->doPivoting(p);
this->forwardEliminateWithPivot(p);
}
this->backSubstituteIntoDU();
this->postSolve();
return answerX;
}
FColDsptr MbD::GESpMat::basicSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal)
{
assert(false);
return FColDsptr();
}
void MbD::GESpMat::preSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal)
{
assert(false);
}
void MbD::GESpMat::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal)
{
assert(false);
}
double MbD::GESpMat::getmatrixArowimaxMagnitude(int i)
{
return matrixA->at(i)->maxMagnitude();
}

7
MbDCode/GESpMat.h
View File

@@ -9,10 +9,15 @@ namespace MbD {
//markowitzPivotRowCount markowitzPivotColCount privateIndicesOfNonZerosInPivotRow rowPositionsOfNonZerosInPivotColumn
public:
FColDsptr solvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal);
FColDsptr basicSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) override;
FColDsptr basicSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal) override;
void preSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal) override;
void preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) override;
double getmatrixArowimaxMagnitude(int i) override;
std::shared_ptr<SparseMatrix<double>> matrixA;
int markowitzPivotRowCount, markowitzPivotColCount;
std::shared_ptr<std::vector<int>> privateIndicesOfNonZerosInPivotRow, rowPositionsOfNonZerosInPivotColumn;
std::shared_ptr<std::vector<int>> rowPositionsOfNonZerosInPivotColumn;
};
}

58
MbDCode/GESpMatFullPv.cpp
View File

@@ -7,9 +7,52 @@ using namespace MbD;
void MbD::GESpMatFullPv::doPivoting(int p)
{
assert(false);
}
//"Used by Gauss Elimination only."
//"Do full pivoting."
//"Swap rows but keep columns in place."
//"The elements below the diagonal are removed column by column."
auto max = 0.0;
auto pivotRow = p;
auto pivotCol = p;
for (int j = p; j < n; j++)
{
rowPositionsOfNonZerosInColumns->at(colOrder->at(j))->clear();
}
for (int i = p; i < m; i++)
{
auto& rowi = matrixA->at(i);
for (auto const& kv : *rowi) {
rowPositionsOfNonZerosInColumns->at(kv.first)->push_back(i);
auto aij = kv.second;
auto mag = aij;
if (mag < 0.0) mag = -mag;
if (max < mag) {
max = mag;
pivotRow = i;
pivotCol = positionsOfOriginalCols->at(kv.first);
}
}
}
if (p != pivotRow) {
matrixA->swapElems(p, pivotRow);
rightHandSideB->swapElems(p, pivotRow);
rowOrder->swapElems(p, pivotRow);
}
if (p != pivotCol) {
colOrder->swapElems(p, pivotCol);
positionsOfOriginalCols->at(colOrder->at(p)) = p;
positionsOfOriginalCols->at(colOrder->at(pivotCol)) = pivotCol;
}
pivotValues->at(p) = max;
if (max < singularPivotTolerance) throw SingularMatrixError("");
auto jp = colOrder->at(p);
rowPositionsOfNonZerosInPivotColumn = rowPositionsOfNonZerosInColumns->at(jp);
if (rowPositionsOfNonZerosInPivotColumn->front() == p) {
rowPositionsOfNonZerosInPivotColumn->erase(rowPositionsOfNonZerosInPivotColumn->begin());
}
markowitzPivotColCount = (int)rowPositionsOfNonZerosInPivotColumn->size();
}
void MbD::GESpMatFullPv::forwardEliminateWithPivot(int p)
{
//app is pivot.
@@ -63,7 +106,7 @@ void MbD::GESpMatFullPv::backSubstituteIntoDU()
//auto rhsZeroElement = this->rhsZeroElement();
for (int i = n - 2; i >= 0; i--)
{
auto rowi = matrixA->at(i);
auto& rowi = matrixA->at(i);
sum = 0.0; // rhsZeroElement copy.
for (auto const& keyValue : *rowi) {
auto jj = keyValue.first;
@@ -95,9 +138,8 @@ void MbD::GESpMatFullPv::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fu
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);
colOrder = std::make_shared<FullRow<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++)
{
@@ -113,11 +155,11 @@ void MbD::GESpMatFullPv::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fu
for (int i = 0; i < m; i++)
{
auto& spRowi = spMat->at(i);
auto maxRowElement = spRowi->maxElement();
if (maxRowElement == 0) {
auto maxRowMagnitude = spRowi->maxMagnitude();
if (maxRowMagnitude == 0) {
throw SingularMatrixError("");
}
matrixA->at(i) = spRowi->conditionedWithTol(singularPivotTolerance * maxRowElement);
matrixA->at(i) = spRowi->conditionedWithTol(singularPivotTolerance * maxRowMagnitude);
rowOrder->at(i) = i;
colOrder->at(i) = i;
positionsOfOriginalCols->at(i) = i;

10
MbDCode/GESpMatFullPvPosIC.cpp
View File

@@ -32,7 +32,7 @@ void MbD::GESpMatFullPvPosIC::doPivoting(int p)
{
rowPositionsOfNonZerosInColumns->at(colOrder->at(j))->clear();
}
if (p == pivotRowLimit) {
if (p >= pivotRowLimit) {
pivotRowLimit = *std::find_if(
pivotRowLimits->begin(), pivotRowLimits->end(),
[&](int limit) { return limit > p; });
@@ -53,12 +53,12 @@ void MbD::GESpMatFullPvPosIC::doPivoting(int p)
}
}
if (p != pivotRow) {
matrixA->swapRows(p, pivotRow);
rightHandSideB->swapRows(p, pivotRow);
rowOrder->swapRows(p, pivotRow);
matrixA->swapElems(p, pivotRow);
rightHandSideB->swapElems(p, pivotRow);
rowOrder->swapElems(p, pivotRow);
}
if (p != pivotCol) {
colOrder->swapRows(p, pivotCol);
colOrder->swapElems(p, pivotCol);
positionsOfOriginalCols->at(colOrder->at(p)) = p;
positionsOfOriginalCols->at(colOrder->at(pivotCol)) = pivotCol;
}

60
MbDCode/GESpMatParPvMarko.cpp
View File

@@ -1,12 +1,70 @@
#include <cassert>
#include "GESpMatParPvMarko.h"
#include "SingularMatrixError.h"
using namespace MbD;
void MbD::GESpMatParPvMarko::doPivoting(int p)
{
assert(false);
//"Search from bottom to top."
//"Check for singular pivot."
//"Do scaling. Do partial pivoting."
//"criterion := mag / (2.0d raisedTo: rowiCount)."
//| lookForFirstNonZeroInPivotCol i rowi aip criterionMax rowPivoti criterion max |
int i, rowPivoti;
double aip, mag, max, criterion, criterionMax;
SpRowDsptr spRowi;
rowPositionsOfNonZerosInPivotColumn->clear();
auto lookForFirstNonZeroInPivotCol = true;
i = m - 1;
while (lookForFirstNonZeroInPivotCol) {
spRowi = matrixA->at(i);
if (spRowi->find(p) == spRowi->end()) {
if (i <= p) throw SingularMatrixError("");
}
else {
markowitzPivotColCount = 0;
aip = spRowi->at(p);
mag = aip * rowScalings->at(i);
if (mag < 0) mag = -mag;
max = mag;
criterionMax = mag / std::pow(2.0, spRowi->size());
rowPivoti = i;
lookForFirstNonZeroInPivotCol = false;
}
i--;
}
while (i >= p) {
spRowi = matrixA->at(i);
if (spRowi->find(p) == spRowi->end()) {
aip = std::numeric_limits<double>::min();
}
else {
aip = spRowi->at(p);
markowitzPivotColCount++;
mag = aip * rowScalings->at(i);
if (mag < 0) mag = -mag;
criterion = mag / std::pow(2.0, spRowi->size());
if (criterion > criterionMax) {
max = mag;
criterionMax = criterion;
rowPositionsOfNonZerosInPivotColumn->push_back(rowPivoti);
rowPivoti = i;
}
else {
rowPositionsOfNonZerosInPivotColumn->push_back(i);
}
}
i--;
}
if (p != rowPivoti) {
matrixA->swapElems(p, rowPivoti);
rightHandSideB->swapElems(p, rowPivoti);
rowScalings->swapElems(p, rowPivoti);
if (aip != std::numeric_limits<double>::min()) rowPositionsOfNonZerosInPivotColumn->at(markowitzPivotColCount - 1) = rowPivoti;
}
if (max < singularPivotTolerance) throw SingularMatrixError("");
}
void MbD::GESpMatParPvMarko::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal)

13
MbDCode/GESpMatParPvMarkoFast.cpp
View File

@@ -14,7 +14,6 @@ void MbD::GESpMatParPvMarkoFast::preSolvewithsaveOriginal(SpMatDsptr spMat, FCol
m = spMat->nrow();
n = spMat->ncol();
matrixA = std::make_shared<SparseMatrix<double>>(m);
privateIndicesOfNonZerosInPivotRow = std::make_shared<std::vector<int>>();
rowPositionsOfNonZerosInPivotColumn = std::make_shared<std::vector<int>>();
}
if (saveOriginal) {
@@ -26,11 +25,11 @@ void MbD::GESpMatParPvMarkoFast::preSolvewithsaveOriginal(SpMatDsptr spMat, FCol
for (int i = 0; i < m; i++)
{
auto& spRowi = spMat->at(i);
auto maxRowElement = spRowi->maxElement();
if (maxRowElement == 0) {
auto maxRowMagnitude = spRowi->maxMagnitude();
if (maxRowMagnitude == 0) {
throw SingularMatrixError("");
}
auto scaling = 1.0 / maxRowElement;
auto scaling = 1.0 / maxRowMagnitude;
matrixA->at(i) = spRowi->timesconditionedWithTol(scaling, singularPivotTolerance);
rightHandSideB->atitimes(i, scaling);
}
@@ -58,8 +57,8 @@ void MbD::GESpMatParPvMarkoFast::doPivoting(int p)
if (i <= p) throw SingularMatrixError("");
}
else {
aip = spRowi->at(p);
markowitzPivotColCount = 0;
aip = spRowi->at(p);
if (aip < 0) aip = -aip;
max = aip;
criterionMax = aip / std::pow(2.0, spRowi->size());
@@ -91,8 +90,8 @@ void MbD::GESpMatParPvMarkoFast::doPivoting(int p)
i--;
}
if (p != rowPivoti) {
matrixA->swapRows(p, rowPivoti);
rightHandSideB->swapRows(p, rowPivoti);
matrixA->swapElems(p, rowPivoti);
rightHandSideB->swapElems(p, rowPivoti);
if (aip != std::numeric_limits<double>::min()) rowPositionsOfNonZerosInPivotColumn->at(markowitzPivotColCount - 1) = rowPivoti;
}
if (max < singularPivotTolerance) throw SingularMatrixError("");

86
MbDCode/GESpMatParPvPrecise.cpp
View File

@@ -1,15 +1,97 @@
#include <cassert>
#include "GESpMatParPvPrecise.h"
#include "SingularMatrixError.h"
using namespace MbD;
void MbD::GESpMatParPvPrecise::doPivoting(int p)
{
assert(false);
//"Search from bottom to top."
//"Use scaling vector and partial pivoting with actual swapping of rows."
//"Check for singular pivot."
//"Do scaling. Do partial pivoting."
//| max rowPivot aip mag lookForFirstNonZeroInPivotCol i |
int i, rowPivoti;
double aip, mag, max;
SpRowDsptr spRowi;
rowPositionsOfNonZerosInPivotColumn->clear();
auto lookForFirstNonZeroInPivotCol = true;
i = m - 1;
while (lookForFirstNonZeroInPivotCol) {
spRowi = matrixA->at(i);
if (spRowi->find(p) == spRowi->end()) {
if (i <= p) throw SingularMatrixError("");
}
else {
markowitzPivotColCount = 0;
aip = spRowi->at(p);
mag = aip * rowScalings->at(i);
if (mag < 0) mag = -mag;
max = mag;
rowPivoti = i;
lookForFirstNonZeroInPivotCol = false;
}
i--;
}
while (i >= p) {
spRowi = matrixA->at(i);
if (spRowi->find(p) == spRowi->end()) {
aip = std::numeric_limits<double>::min();
}
else {
aip = spRowi->at(p);
markowitzPivotColCount++;
mag = aip * rowScalings->at(i);
if (mag < 0) mag = -mag;
if (mag > max) {
max = mag;
rowPositionsOfNonZerosInPivotColumn->push_back(rowPivoti);
rowPivoti = i;
}
else {
rowPositionsOfNonZerosInPivotColumn->push_back(i);
}
}
i--;
}
if (p != rowPivoti) {
matrixA->swapElems(p, rowPivoti);
rightHandSideB->swapElems(p, rowPivoti);
rowScalings->swapElems(p, rowPivoti);
rowOrder->swapElems(p, rowPivoti);
if (aip != std::numeric_limits<double>::min()) rowPositionsOfNonZerosInPivotColumn->at(markowitzPivotColCount - 1) = rowPivoti;
}
pivotValues->at(p) = max;
if (max < singularPivotTolerance) throw SingularMatrixError("");
}
void MbD::GESpMatParPvPrecise::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal)
{
assert(false);
//assert(false);
//"A conditioned copy of aMatrix is solved."
if (m != spMat->nrow() || n != spMat->ncol()) {
m = spMat->nrow();
n = spMat->ncol();
matrixA = std::make_shared<SparseMatrix<double>>(m);
rowScalings = std::make_shared<FullColumn<double>>(m);
pivotValues = std::make_shared<FullColumn<double>>(m);
rowOrder = std::make_shared<FullColumn<int>>(m);
rowPositionsOfNonZerosInPivotColumn = 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 maxRowMagnitude = spRowi->maxMagnitude();
if (maxRowMagnitude == 0) throw SingularMatrixError("");
rowScalings->at(i) = 1.0 / maxRowMagnitude;
matrixA->at(i) = spRowi->conditionedWithTol(singularPivotTolerance * maxRowMagnitude);
rowOrder->at(i) = i;
}
}

8
MbDCode/ICKineIntegrator.cpp
View File

@@ -1 +1,9 @@
#include "ICKineIntegrator.h"
#include "SystemSolver.h"
void MbD::ICKineIntegrator::runInitialConditionTypeSolution()
{
system->runPosICKine();
system->runVelICKine();
system->runAccICKine();
}

1
MbDCode/ICKineIntegrator.h
View File

@@ -6,6 +6,7 @@ namespace MbD {
{
//
public:
void runInitialConditionTypeSolution() override;
};
}

4
MbDCode/Integrator.cpp
View File

@@ -1,3 +1,7 @@
#include "Integrator.h"
using namespace MbD;
void MbD::Integrator::setSystem(Solver* sys)
{
}

2
MbDCode/Integrator.h
View File

@@ -9,6 +9,8 @@ namespace MbD {
{
//system direction
public:
void setSystem(Solver* sys) override;
virtual void runInitialConditionTypeSolution() = 0;
double direction = 1;
};

9
MbDCode/IntegratorInterface.cpp
View File

@@ -17,9 +17,9 @@ tend = system->endTime();
direction = (tstart < tend) ? 1.0 : -1.0;
}
void MbD::IntegratorInterface::setSystem(SystemSolver* sys)
void MbD::IntegratorInterface::setSystem(Solver* sys)
{
system = sys;
system = static_cast<SystemSolver*>(sys);
}
void MbD::IntegratorInterface::logString(std::string& str)
@@ -44,3 +44,8 @@ int MbD::IntegratorInterface::orderMax()
{
return system->orderMax;
}
void MbD::IntegratorInterface::incrementTime(double tnew)
{
system->settime(tnew);
}

8
MbDCode/IntegratorInterface.h
View File

@@ -13,13 +13,15 @@ namespace MbD {
void initializeGlobally() override;
virtual void preRun() = 0;
void setSystem(SystemSolver* sys);
void setSystem(Solver* sys) override;
void logString(std::string& str) override;
void run() override;
int orderMax();
virtual void preFirstStep() = 0;
virtual double suggestSmallerOrAcceptFirstStepSize(double hnew) = 0;
virtual void incrementTime(double tnew);
virtual void preFirstStep() = 0;
virtual void preStep() = 0;
virtual double suggestSmallerOrAcceptFirstStepSize(double hnew) = 0;
SystemSolver* system;
double tout = 0, hout = 0, hmin = 0, hmax = 0, tstart = 0, tend = 0;
std::shared_ptr<BasicQuasiIntegrator> integrator;

57
MbDCode/Item.cpp
View File

@@ -59,7 +59,7 @@ void MbD::Item::fillPosKineError(FColDsptr col)
{
}
void MbD::Item::fillPosKineJacob(FMatDsptr mat)
void MbD::Item::fillPosKineJacob(SpMatDsptr mat)
{
}
@@ -71,6 +71,11 @@ void MbD::Item::fillRedundantConstraints(std::shared_ptr<std::vector<std::shared
{
}
void MbD::Item::fillConstraints(std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> allConstraints)
{
assert(false);
}
void MbD::Item::fillqsu(FColDsptr col)
{
}
@@ -116,6 +121,10 @@ void MbD::Item::preDynStep()
{
}
void MbD::Item::storeDynState()
{
}
double MbD::Item::suggestSmallerOrAcceptDynFirstStepSize(double hnew)
{
//"Default is return hnew."
@@ -123,6 +132,41 @@ double MbD::Item::suggestSmallerOrAcceptDynFirstStepSize(double hnew)
return hnew;
}
void MbD::Item::preVelIC()
{
//"Assume positions are valid."
//"Called once before solving for velocity initial conditions."
//"Update all variable dependent instance variables needed for velIC even if they have
//been calculated in postPosIC."
//"Variables dependent on t are updated."
this->calcPostDynCorrectorIteration();
}
void MbD::Item::postVelIC()
{
}
void MbD::Item::fillqsudot(FColDsptr col)
{
}
void MbD::Item::fillqsudotWeights(std::shared_ptr<DiagonalMatrix<double>> diagMat)
{
}
void MbD::Item::fillVelICError(FColDsptr error)
{
}
void MbD::Item::fillVelICJacob(SpMatDsptr jacob)
{
}
void MbD::Item::setqsudotlam(FColDsptr qsudotlam)
{
}
void MbD::Item::constraintsReport()
{
}
@@ -149,16 +193,9 @@ void MbD::Item::prePosIC()
calcPostDynCorrectorIteration();
}
void MbD::Item::prePostIC()
{
}
void MbD::Item::prePostICIteration()
{
}
void MbD::Item::prePostICRestart()
void MbD::Item::prePosKine()
{
this->prePosIC();
}
void MbD::Item::postPosIC()

19
MbDCode/Item.h
View File

@@ -28,21 +28,21 @@ namespace MbD {
virtual void logString(std::string& str);
virtual void prePosIC();
virtual void prePosKine();
virtual void fillPosICError(FColDsptr col);
virtual void fillPosICJacob(FMatDsptr mat);
virtual void fillPosICJacob(SpMatDsptr mat);
virtual void prePostIC();
virtual void prePostICIteration();
virtual void prePostICRestart();
virtual void postPosIC();
virtual void postPosICIteration();
virtual void removeRedundantConstraints(std::shared_ptr<std::vector<int>> redundantEqnNos);
virtual void reactivateRedundantConstraints();
virtual void fillPosKineError(FColDsptr col);
virtual void fillPosKineJacob(FMatDsptr mat);
virtual void fillPosKineJacob(SpMatDsptr mat);
virtual void fillEssenConstraints(std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> essenConstraints);
virtual void fillRedundantConstraints(std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> redunConstraints);
virtual void fillConstraints(std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> allConstraints);
virtual void fillqsu(FColDsptr col);
virtual void fillqsuWeights(std::shared_ptr<DiagonalMatrix<double>> diagMat);
virtual void fillqsulam(FColDsptr col);
@@ -52,8 +52,15 @@ namespace MbD {
virtual std::string classname();
virtual void preDynFirstStep();
virtual void preDynStep();
virtual void storeDynState();
virtual double suggestSmallerOrAcceptDynFirstStepSize(double hnew);
virtual void preVelIC();
virtual void postVelIC();
virtual void fillqsudot(FColDsptr col);
virtual void fillqsudotWeights(std::shared_ptr<DiagonalMatrix<double>> diagMat);
virtual void fillVelICError(FColDsptr error);
virtual void fillVelICJacob(SpMatDsptr jacob);
virtual void setqsudotlam(FColDsptr qsudotlam);
void setName(std::string& str);
const std::string& getName() const;

34
MbDCode/Joint.cpp
View File

@@ -69,6 +69,11 @@ void MbD::Joint::prePosIC()
constraintsDo([](std::shared_ptr<Constraint> constraint) { constraint->prePosIC(); });
}
void MbD::Joint::prePosKine()
{
constraintsDo([](std::shared_ptr<Constraint> constraint) { constraint->prePosKine(); });
}
void MbD::Joint::fillEssenConstraints(std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> essenConstraints)
{
constraintsDo([&](std::shared_ptr<Constraint> con) { con->fillEssenConstraints(con, essenConstraints); });
@@ -89,11 +94,25 @@ void MbD::Joint::fillRedundantConstraints(std::shared_ptr<std::vector<std::share
constraintsDo([&](std::shared_ptr<Constraint> con) { con->fillRedundantConstraints(con, redunConstraints); });
}
void MbD::Joint::fillConstraints(std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> allConstraints)
{
constraintsDo([&](std::shared_ptr<Constraint> con) { con->fillConstraints(con, allConstraints); });
}
void MbD::Joint::fillqsulam(FColDsptr col)
{
constraintsDo([&](std::shared_ptr<Constraint> con) { con->fillqsulam(col); });
}
void MbD::Joint::fillqsudot(FColDsptr col)
{
constraintsDo([&](std::shared_ptr<Constraint> con) { con->fillqsudot(col); });
}
void MbD::Joint::fillqsudotWeights(std::shared_ptr<DiagonalMatrix<double>> diagMat)
{
}
void MbD::Joint::useEquationNumbers()
{
constraintsDo([](std::shared_ptr<Constraint> constraint) { constraint->useEquationNumbers(); });
@@ -182,3 +201,18 @@ void MbD::Joint::preDyn()
{
constraintsDo([](std::shared_ptr<Constraint> constraint) { constraint->preDyn(); });
}
void MbD::Joint::fillPosKineError(FColDsptr col)
{
constraintsDo([&](std::shared_ptr<Constraint> con) { con->fillPosKineError(col); });
}
void MbD::Joint::fillPosKineJacob(SpMatDsptr mat)
{
constraintsDo([&](std::shared_ptr<Constraint> constraint) { constraint->fillPosKineJacob(mat); });
}
void MbD::Joint::preVelIC()
{
constraintsDo([](std::shared_ptr<Constraint> constraint) { constraint->preVelIC(); });
}

7
MbDCode/Joint.h
View File

@@ -23,11 +23,15 @@ namespace MbD {
void postInput() override;
void addConstraint(std::shared_ptr<Constraint> con);
void prePosIC() override;
void prePosKine() override;
void fillEssenConstraints(std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> essenConstraints) override;
virtual void fillDispConstraints(std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> dispConstraints);
virtual void fillPerpenConstraints(std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> perpenConstraints);
void fillRedundantConstraints(std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> redunConstraints) override;
void fillConstraints(std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> allConstraints) override;
void fillqsulam(FColDsptr col) override;
void fillqsudot(FColDsptr col) override;
void fillqsudotWeights(std::shared_ptr<DiagonalMatrix<double>> diagMat) override;
void useEquationNumbers() override;
void setqsulam(FColDsptr col) override;
void postPosICIteration() override;
@@ -39,6 +43,9 @@ namespace MbD {
void postPosIC() override;
void outputStates() override;
void preDyn() override;
void fillPosKineError(FColDsptr col) override;
void fillPosKineJacob(SpMatDsptr mat) override;
void preVelIC() override;
EndFrmcptr frmI;
EndFrmcptr frmJ;

7
MbDCode/KineIntegrator.cpp
View File

@@ -11,3 +11,10 @@ void MbD::KineIntegrator::preRun()
system->logString(str);
QuasiIntegrator::preRun();
}
void MbD::KineIntegrator::runInitialConditionTypeSolution()
{
system->runPosKine();
system->runVelKine();
system->runAccKine();
}

1
MbDCode/KineIntegrator.h
View File

@@ -8,6 +8,7 @@ namespace MbD {
//
public:
void preRun() override;
void runInitialConditionTypeSolution() override;
};
}

160
MbDCode/LDUFullMat.cpp
View File

@@ -1 +1,161 @@
#include "LDUFullMat.h"
using namespace MbD;
FColDsptr MbD::LDUFullMat::basicSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal)
{
this->decomposesaveOriginal(fullMat, saveOriginal);
FColDsptr answer = this->forAndBackSubsaveOriginal(fullCol, saveOriginal);
return answer;
}
FColDsptr MbD::LDUFullMat::basicSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal)
{
assert(false);
return FColDsptr();
}
void MbD::LDUFullMat::preSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal)
{
assert(false);
}
void MbD::LDUFullMat::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal)
{
assert(false);
}
void MbD::LDUFullMat::forwardEliminateWithPivot(int p)
{
//"Save factors in lower triangle for LU decomposition."
//| rowp app rowi aip factor |
auto rowp = matrixA->at(p);
auto app = rowp->at(p);
for (int i = p + 1; i < m; i++)
{
auto rowi = matrixA->at(i);
auto aip = rowi->at(p);
auto factor = aip / app;
rowi->at(p) = factor;
if (factor != 0) {
for (int j = p + 1; j < n; j++)
{
rowi->atiminusNumber(j, factor * rowp->at(j));
}
}
}
}
void MbD::LDUFullMat::postSolve()
{
assert(false);
}
void MbD::LDUFullMat::preSolvesaveOriginal(FMatDsptr fullMat, bool saveOriginal)
{
if (saveOriginal) {
matrixA = fullMat->copy();
}
else {
matrixA = fullMat;
}
if (m != matrixA->nrow() || n != matrixA->ncol()) {
m = matrixA->nrow();
n = matrixA->ncol();
pivotValues = std::make_shared<FullColumn<double>>(m);
rowOrder = std::make_shared<FullColumn<int>>(m);
colOrder = std::make_shared<FullRow<int>>(n);
}
if (m == n) {
for (int i = 0; i < m; i++)
{
rowOrder->at(i) = i;
colOrder->at(i) = i;
}
}
else {
for (int i = 0; i < m; i++)
{
rowOrder->at(i) = i;
}
for (int j = 0; j < n; j++)
{
colOrder->at(j) = j;
}
}
this->findScalingsForRowRange(0, m);
}
void MbD::LDUFullMat::decomposesaveOriginal(FMatDsptr fullMat, bool saveOriginal)
{
this->preSolvesaveOriginal(fullMat, saveOriginal);
for (int p = 0; p < m; p++)
{
this->doPivoting(p);
this->forwardEliminateWithPivot(p);
}
}
void MbD::LDUFullMat::decomposesaveOriginal(SpMatDsptr spMat, bool saveOriginal)
{
assert(false);
}
FMatDsptr MbD::LDUFullMat::inversesaveOriginal(FMatDsptr fullMat, bool saveOriginal)
{
//"ForAndBackSub be optimized for the identity matrix."
this->decomposesaveOriginal(fullMat, saveOriginal);
rightHandSideB = std::make_shared<FullColumn<double>>(m);
auto matrixAinverse = std::make_shared <FullMatrix<double>>(m, n);
for (int j = 0; j < n; j++)
{
rightHandSideB->zeroSelf();
rightHandSideB->at(j) = 1.0;
this->forAndBackSubsaveOriginal(rightHandSideB, saveOriginal);
matrixAinverse->atijputFullColumn(0, j, answerX);
}
return matrixAinverse;
}
double MbD::LDUFullMat::getmatrixArowimaxMagnitude(int i)
{
return matrixA->at(i)->maxMagnitude();
}
void MbD::LDUFullMat::forwardSubstituteIntoL()
{
//"L is lower triangular with nonzero and ones in diagonal."
auto vectorc = std::make_shared<FullColumn<double>>(n);
for (int i = 0; i < n; i++)
{
auto rowi = matrixA->at(i);
double sum = 0.0;
for (int j = 0; j < i - 1; j++)
{
sum += rowi->at(j) * vectorc->at(j);
}
vectorc->at(i) = rightHandSideB->at(i) - sum;
}
rightHandSideB = vectorc;
}
void MbD::LDUFullMat::backSubstituteIntoDU()
{
//"DU is upper triangular with nonzero and arbitrary diagonals."
//| rowi sum |
answerX = std::make_shared<FullColumn<double>>(n);
answerX->at(n - 1) = rightHandSideB->at(m - 1) / matrixA->at(m - 1)->at(n - 1);
for (int i = n-2; i >= 0; i--)
{
auto rowi = matrixA->at(i);
double sum = answerX->at(n) * rowi->at(n);
for (int j = i + 1; j < n - 1; j++)
{
sum += answerX->at(j) * rowi->at(j);
}
answerX->at(i) = (rightHandSideB->at(i) - sum) / rowi->at(i);
}
}

16
MbDCode/LDUFullMat.h
View File

@@ -7,6 +7,22 @@ namespace MbD {
{
//
public:
FColDsptr basicSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal) override;
FColDsptr basicSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) override;
void preSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal) override;
void preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) override;
void forwardEliminateWithPivot(int p) override;
void postSolve() override;
void preSolvesaveOriginal(FMatDsptr fullMat, bool saveOriginal);
void decomposesaveOriginal(SpMatDsptr spMat, bool saveOriginal);
void decomposesaveOriginal(FMatDsptr fullMat, bool saveOriginal);
FMatDsptr inversesaveOriginal(FMatDsptr fullMat, bool saveOriginal);
double getmatrixArowimaxMagnitude(int i) override;
void forwardSubstituteIntoL() override;
void backSubstituteIntoDU() override;
std::shared_ptr<FullMatrix<double>> matrixA;
};
}

46
MbDCode/LDUFullMatParPv.cpp
View File

@@ -1,27 +1,35 @@
#include "LDUFullMatParPv.h"
#include "FullMatrix.h"
#include "SingularMatrixError.h"
using namespace MbD;
FMatDsptr MbD::LDUFullMatParPv::inversesaveOriginal(FMatDsptr fullMat, bool saveOriginal)
void MbD::LDUFullMatParPv::doPivoting(int p)
{
assert(false);
//"ForAndBackSub be optimized for the identity matrix."
//"Use scalings. Do row pivoting."
//| matrixAinverse |
//self decompose : aMatrix saveOriginal : saveOriginal.
//rightHandSideB : = StMFullColumn new : m.
//matrixAinverse : = StMFullMatrix new : m by : n.
//1 to : n
//do :
// [:j |
// rightHandSideB zeroSelf.
// rightHandSideB at : j put : 1.0d.
// self forAndBackSub : rightHandSideB saveOriginal : saveOriginal.
// matrixAinverse
// at : 1
// and : j
// putFullColumn : answerX] .
// ^ matrixAinverse
return FMatDsptr();
//| app max rowPivot aip mag |
auto app = matrixA->at(p)->at(p);
auto max = app * rowScalings->at(p);
if (max < 0.0) max = -max;
auto rowPivot = p;
for (int i = p + 1; i < m; i++)
{
auto aip = matrixA->at(i)->at(p);
if (aip != 0.0) {
auto mag = aip * rowScalings->at(i);
if (mag < 0) mag = -mag;
if (max < mag) {
max = mag;
rowPivot = i;
}
}
}
if (p != rowPivot) {
matrixA->swapElems(p, rowPivot);
rowScalings->swapElems(p, rowPivot);
rowOrder->swapElems(p, rowPivot);
}
pivotValues->at(p) = max;
if (max < singularPivotTolerance) throw SingularMatrixError("");
}

4
MbDCode/LDUFullMatParPv.h
View File

@@ -3,11 +3,11 @@
#include "LDUFullMat.h"
namespace MbD {
class LDUFullMatParPv : public LDUFullMat
class LDUFullMatParPv : public LDUFullMat
{
//
public:
FMatDsptr inversesaveOriginal(FMatDsptr fullMat, bool saveOriginal);
void doPivoting(int p) override;
};
}

75
MbDCode/LDUSpMat.cpp Normal file
View File

@@ -0,0 +1,75 @@
#include "LDUSpMat.h"
#include "FullColumn.h"
using namespace MbD;
FColDsptr MbD::LDUSpMat::basicSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal)
{
this->decomposesaveOriginal(spMat, saveOriginal);
FColDsptr answer = this->forAndBackSubsaveOriginal(fullCol, saveOriginal);
return answer;
}
void MbD::LDUSpMat::decomposesaveOriginal(FMatDsptr fullMat, bool saveOriginal)
{
assert(false);
}
void MbD::LDUSpMat::decomposesaveOriginal(SpMatDsptr spMat, bool saveOriginal)
{
assert(false);
}
FColDsptr MbD::LDUSpMat::forAndBackSubsaveOriginal(FColDsptr fullCol, bool saveOriginal)
{
assert(false);
return FColDsptr();
}
double MbD::LDUSpMat::getmatrixArowimaxMagnitude(int i)
{
return matrixA->at(i)->maxMagnitude();
}
void MbD::LDUSpMat::forwardSubstituteIntoL()
{
//"L is lower triangular with nonzero and ones in diagonal."
auto vectorc = std::make_shared<FullColumn<double>>(n);
vectorc->at(0) = rightHandSideB->at(0);
for (int i = 1; i < n; i++)
{
auto rowi = matrixA->at(i);
double sum = 0.0;
for (auto const& keyValue : *rowi) {
int j = keyValue.first;
double duij = keyValue.second;
sum += duij * vectorc->at(j);
}
vectorc->at(i) = rightHandSideB->at(i) - sum;
}
rightHandSideB = vectorc;
}
void MbD::LDUSpMat::backSubstituteIntoDU()
{
//"DU is upper triangular with nonzero diagonals."
double sum, duij;
for (int i = 0; i < m; i++)
{
rightHandSideB->at(i) = rightHandSideB->at(i) / matrixD->at(i);
}
answerX = std::make_shared<FullColumn<double>>(m);
answerX->at(n - 1) = rightHandSideB->at(m - 1);
for (int i = n - 2; i >= 0; i--)
{
auto rowi = matrixU->at(i);
sum = 0.0;
for (auto const& keyValue : *rowi) {
auto j = keyValue.first;
duij = keyValue.second;
sum += answerX->at(j) * duij;
}
answerX->at(i) = rightHandSideB->at(i) - sum;
}
}

25
MbDCode/LDUSpMat.h Normal file
View File

@@ -0,0 +1,25 @@
#pragma once
#include "MatrixLDU.h"
#include "SparseMatrix.h"
namespace MbD {
class LDUSpMat : public MatrixLDU
{
//matrixL matrixD matrixU markowitzPivotRowCount markowitzPivotColCount privateIndicesOfNonZerosInPivotRow rowPositionsOfNonZerosInPivotColumn
public:
FColDsptr basicSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) override;
void decomposesaveOriginal(FMatDsptr fullMat, bool saveOriginal);
void decomposesaveOriginal(SpMatDsptr spMat, bool saveOriginal);
FColDsptr forAndBackSubsaveOriginal(FColDsptr fullCol, bool saveOriginal);
double getmatrixArowimaxMagnitude(int i) override;
void forwardSubstituteIntoL() override;
void backSubstituteIntoDU() override;
std::shared_ptr<SparseMatrix<double>> matrixA, matrixL, matrixU;
std::shared_ptr<DiagonalMatrix<double>> matrixD;
int markowitzPivotRowCount, markowitzPivotColCount;
std::shared_ptr<std::vector<int>> rowPositionsOfNonZerosInPivotColumn;
};
}

1
MbDCode/LDUSpMatParPv.cpp Normal file
View File

@@ -0,0 +1 @@
#include "LDUSpMatParPv.h"

13
MbDCode/LDUSpMatParPv.h Normal file
View File

@@ -0,0 +1,13 @@
#pragma once
#include "LDUSpMat.h"
namespace MbD {
class LDUSpMatParPv : public LDUSpMat
{
//
public:
};
}

65
MbDCode/LDUSpMatParPvMarko.cpp Normal file
View File

@@ -0,0 +1,65 @@
#include "LDUSpMatParPvMarko.h"
#include "SingularMatrixError.h"
void MbD::LDUSpMatParPvMarko::doPivoting(int p)
{
//"Search from bottom to top."
//"Check for singular pivot."
//"Do scaling. Do partial pivoting."
//"criterion := mag / (2.0d raisedTo: rowiCount)."
//| lookForFirstNonZeroInPivotCol i rowi aip criterionMax rowPivoti criterion max |
int i, rowPivoti;
double aip, mag, max, criterion, criterionMax;
SpRowDsptr spRowi;
rowPositionsOfNonZerosInPivotColumn->clear();
auto lookForFirstNonZeroInPivotCol = true;
i = m - 1;
while (lookForFirstNonZeroInPivotCol) {
spRowi = matrixA->at(i);
if (spRowi->find(p) == spRowi->end()) {
if (i <= p) throw SingularMatrixError("");
}
else {
markowitzPivotColCount = 0;
aip = spRowi->at(p);
mag = aip * rowScalings->at(i);
if (mag < 0) mag = -mag;
max = mag;
criterionMax = mag / std::pow(2.0, spRowi->size());
rowPivoti = i;
lookForFirstNonZeroInPivotCol = false;
}
i--;
}
while (i >= p) {
spRowi = matrixA->at(i);
if (spRowi->find(p) == spRowi->end()) {
aip = std::numeric_limits<double>::min();
}
else {
aip = spRowi->at(p);
markowitzPivotColCount++;
mag = aip * rowScalings->at(i);
if (mag < 0) mag = -mag;
criterion = mag / std::pow(2.0, spRowi->size());
if (criterion > criterionMax) {
max = mag;
criterionMax = criterion;
rowPositionsOfNonZerosInPivotColumn->push_back(rowPivoti);
rowPivoti = i;
}
else {
rowPositionsOfNonZerosInPivotColumn->push_back(i);
}
}
i--;
}
if (p != rowPivoti) {
matrixA->swapElems(p, rowPivoti);
rowScalings->swapElems(p, rowPivoti);
rowOrder->swapElems(p, rowPivoti);
matrixL->swapElems(p, rowPivoti);
if (aip != std::numeric_limits<double>::min()) rowPositionsOfNonZerosInPivotColumn->at(markowitzPivotColCount - 1) = rowPivoti;
}
if (max < singularPivotTolerance) throw SingularMatrixError("");
}

14
MbDCode/LDUSpMatParPvMarko.h Normal file
View File

@@ -0,0 +1,14 @@
#pragma once
#include "LDUSpMatParPv.h"
namespace MbD {
class LDUSpMatParPvMarko : public LDUSpMatParPv
{
//
public:
void doPivoting(int p) override;
};
}

63
MbDCode/LDUSpMatParPvPrecise.cpp Normal file
View File

@@ -0,0 +1,63 @@
#include "LDUSpMatParPvPrecise.h"
#include "SingularMatrixError.h"
void MbD::LDUSpMatParPvPrecise::doPivoting(int p)
{
//"Search from bottom to top."
//"Use scaling vector and partial pivoting with actual swapping of rows."
//"Check for singular pivot."
//"Do scaling. Do partial pivoting."
//| max rowPivot aip mag lookForFirstNonZeroInPivotCol i |
int i, rowPivoti;
double aip, mag, max;
SpRowDsptr spRowi;
rowPositionsOfNonZerosInPivotColumn->clear();
auto lookForFirstNonZeroInPivotCol = true;
i = m - 1;
while (lookForFirstNonZeroInPivotCol) {
spRowi = matrixA->at(i);
if (spRowi->find(p) == spRowi->end()) {
if (i <= p) throw SingularMatrixError("");
}
else {
markowitzPivotColCount = 0;
aip = spRowi->at(p);
mag = aip * rowScalings->at(i);
if (mag < 0) mag = -mag;
max = mag;
rowPivoti = i;
lookForFirstNonZeroInPivotCol = false;
}
i--;
}
while (i >= p) {
spRowi = matrixA->at(i);
if (spRowi->find(p) == spRowi->end()) {
aip = std::numeric_limits<double>::min();
}
else {
aip = spRowi->at(p);
markowitzPivotColCount++;
mag = aip * rowScalings->at(i);
if (mag < 0) mag = -mag;
if (mag > max) {
max = mag;
rowPositionsOfNonZerosInPivotColumn->push_back(rowPivoti);
rowPivoti = i;
}
else {
rowPositionsOfNonZerosInPivotColumn->push_back(i);
}
}
i--;
}
if (p != rowPivoti) {
matrixA->swapElems(p, rowPivoti);
rowScalings->swapElems(p, rowPivoti);
rowOrder->swapElems(p, rowPivoti);
matrixL->swapElems(p, rowPivoti);
if (aip != std::numeric_limits<double>::min()) rowPositionsOfNonZerosInPivotColumn->at(markowitzPivotColCount - 1) = rowPivoti;
}
pivotValues->at(p) = max;
if (max < singularPivotTolerance) throw SingularMatrixError("");
}

14
MbDCode/LDUSpMatParPvPrecise.h Normal file
View File

@@ -0,0 +1,14 @@
#pragma once
#include "LDUSpMatParPv.h"
namespace MbD {
class LDUSpMatParPvPrecise : public LDUSpMatParPv
{
//
public:
void doPivoting(int p) override;
};
}

23
MbDCode/MarkerFrame.cpp
View File

@@ -62,7 +62,7 @@ void MbD::MarkerFrame::calcPostDynCorrectorIteration()
for (int i = 0; i < 4; i++)
{
auto& pAOppEi = pAOppE->at(i);
prOmOpE->atijputFullColumn(1, i, pAOppEi->timesFullColumn(rpmp));
prOmOpE->atijputFullColumn(0, i, pAOppEi->timesFullColumn(rpmp));
pAOmpE->at(i) = pAOppEi->timesFullMatrix(aApm);
}
}
@@ -103,6 +103,16 @@ void MbD::MarkerFrame::fillqsulam(FColDsptr col)
endFramesDo([&](const std::shared_ptr<EndFramec>& endFrame) { endFrame->fillqsulam(col); });
}
void MbD::MarkerFrame::fillqsudot(FColDsptr col)
{
endFramesDo([&](const std::shared_ptr<EndFramec>& endFrame) { endFrame->fillqsudot(col); });
}
void MbD::MarkerFrame::fillqsudotWeights(std::shared_ptr<DiagonalMatrix<double>> diagMat)
{
endFramesDo([&](const std::shared_ptr<EndFramec>& endFrame) { endFrame->fillqsudotWeights(diagMat); });
}
void MbD::MarkerFrame::setqsu(FColDsptr col)
{
endFramesDo([&](const std::shared_ptr<EndFramec>& endFrame) { endFrame->setqsu(col); });
@@ -128,6 +138,17 @@ void MbD::MarkerFrame::preDyn()
endFramesDo([](std::shared_ptr<EndFramec> endFrame) { endFrame->preDyn(); });
}
void MbD::MarkerFrame::storeDynState()
{
endFramesDo([](std::shared_ptr<EndFramec> endFrame) { endFrame->storeDynState(); });
}
void MbD::MarkerFrame::preVelIC()
{
Item::preVelIC();
endFramesDo([](std::shared_ptr<EndFramec> endFrame) { endFrame->preVelIC(); });
}
void MarkerFrame::setPartFrame(PartFrame* partFrm)
{
partFrame = partFrm;

4
MbDCode/MarkerFrame.h
View File

@@ -36,11 +36,15 @@ namespace MbD {
void fillqsu(FColDsptr col) override;
void fillqsuWeights(std::shared_ptr<DiagonalMatrix<double>> diagMat) override;
void fillqsulam(FColDsptr col) override;
void fillqsudot(FColDsptr col) override;
void fillqsudotWeights(std::shared_ptr<DiagonalMatrix<double>> diagMat) override;
void setqsu(FColDsptr col) override;
void setqsulam(FColDsptr col) override;
void postPosICIteration() override;
void postPosIC() override;
void preDyn() override;
void storeDynState() override;
void preVelIC() override;
PartFrame* partFrame; //Use raw pointer when pointing backwards.
FColDsptr rpmp = std::make_shared<FullColumn<double>>(3);

12
MbDCode/MatrixDecomposition.cpp
View File

@@ -1 +1,13 @@
#include "MatrixDecomposition.h"
using namespace MbD;
void MbD::MatrixDecomposition::applyRowOrderOnRightHandSideB()
{
FColDsptr answer = std::make_shared<FullColumn<double>>(m);
for (int i = 0; i < m; i++)
{
answer->at(i) = rightHandSideB->at(rowOrder->at(i));
}
rightHandSideB = answer;
}

9
MbDCode/MatrixDecomposition.h
View File

@@ -7,6 +7,15 @@ namespace MbD {
{
//
public:
virtual FColDsptr forAndBackSubsaveOriginal(FColDsptr fullCol, bool saveOriginal) = 0;
virtual void applyRowOrderOnRightHandSideB();
virtual void forwardSubstituteIntoL() = 0;
//virtual void backSubstituteIntoU();
//virtual FColDsptr basicSolve(aMatrix); with : aVector saveOriginal : saveOriginal
//virtual void forwardSubstituteIntoL();
//virtual void forwardSubstituteIntoLD();
//virtual void postSolve();
//virtual void preSolve : aMatrix saveOriginal : saveOriginal
};
}

14
MbDCode/MatrixGaussElimination.cpp
View File

@@ -1,17 +1,3 @@
#include "MatrixGaussElimination.h"
using namespace MbD;
FColDsptr MbD::MatrixGaussElimination::basicSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal)
{
this->preSolvewithsaveOriginal(spMat, fullCol, saveOriginal);
for (int p = 0; p < m; p++)
{
this->doPivoting(p);
this->forwardEliminateWithPivot(p);
}
this->backSubstituteIntoDU();
this->postSolve();
return answerX;
}

2
MbDCode/MatrixGaussElimination.h
View File

@@ -8,9 +8,7 @@ namespace MbD {
{
//
public:
FColDsptr basicSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) override;
virtual void forwardEliminateWithPivot(int p) = 0;
virtual void backSubstituteIntoDU() = 0;
};
}

16
MbDCode/MatrixLDU.cpp
View File

@@ -1 +1,17 @@
#include "MatrixLDU.h"
using namespace MbD;
FColDsptr MbD::MatrixLDU::forAndBackSubsaveOriginal(FColDsptr fullCol, bool saveOriginal)
{
if (saveOriginal) {
rightHandSideB = fullCol->copy();
}
else {
rightHandSideB = fullCol;
}
this->applyRowOrderOnRightHandSideB();
this->forwardSubstituteIntoL();
this->backSubstituteIntoDU();
return answerX;
}

1
MbDCode/MatrixLDU.h
View File

@@ -7,6 +7,7 @@ namespace MbD {
{
//
public:
FColDsptr forAndBackSubsaveOriginal(FColDsptr fullCol, bool saveOriginal) override;
};
}

17
MbDCode/MatrixSolver.cpp
View File

@@ -6,6 +6,7 @@
#include "MatrixSolver.h"
#include "SparseMatrix.h"
#include "FullMatrix.h"
#include "SingularMatrixError.h"
using namespace MbD;
@@ -15,6 +16,10 @@ void MbD::MatrixSolver::initialize()
singularPivotTolerance = 4 * std::numeric_limits<double>::epsilon();
}
void MbD::MatrixSolver::setSystem(Solver* sys)
{
}
FColDsptr MbD::MatrixSolver::solvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal)
{
this->timedSolvewithsaveOriginal(spMat, fullCol, saveOriginal);
@@ -44,3 +49,15 @@ FColDsptr MbD::MatrixSolver::timedSolvewithsaveOriginal(FMatDsptr fullMat, FColD
{
return FColDsptr();
}
void MbD::MatrixSolver::findScalingsForRowRange(int begin, int end)
{
//"Row element * scaling <= 1.0."
rowScalings = std::make_shared<FullColumn<double>>(m);
for (int i = begin; i < end; i++)
{
auto maxRowMagnitude = this->getmatrixArowimaxMagnitude(i);
if (maxRowMagnitude == 0.0) throw SingularMatrixError("");
rowScalings->at(i) = 1.0 / maxRowMagnitude;
}
}

13
MbDCode/MatrixSolver.h
View File

@@ -13,18 +13,27 @@ namespace MbD {
public:
MatrixSolver(){}
void initialize() override;
void setSystem(Solver* sys) override;
virtual FColDsptr solvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal);
virtual FColDsptr solvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal);
virtual FColDsptr timedSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal);
virtual FColDsptr timedSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal);
virtual FColDsptr timedSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal);
virtual FColDsptr basicSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal) = 0;
virtual FColDsptr basicSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) = 0;
virtual void preSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal) = 0;
virtual void preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) = 0;
virtual void doPivoting(int p) = 0;
virtual void forwardEliminateWithPivot(int p) = 0;
virtual void backSubstituteIntoDU() = 0;
virtual void postSolve() = 0;
virtual void findScalingsForRowRange(int begin, int end);
virtual double getmatrixArowimaxMagnitude(int i) = 0;
int m = 0, n = 0;
FColDsptr answerX, rightHandSideB, rowScalings, pivotValues;
std::shared_ptr<FullColumn<int>> rowOrder, colOrder;
std::shared_ptr<FullColumn<int>> rowOrder;
std::shared_ptr<FullRow<int>> colOrder;
double singularPivotTolerance = 0, millisecondsToRun = 0;
};
}

38
MbDCode/MbDCode.cpp
View File

@@ -55,7 +55,7 @@ int main()
systemSolver->rotationLimit = 0.5;
std::string str;
FColDsptr qX, qE, qXdot, omeOpO;
FColDsptr qX, qE, qXdot, omeOpO, qXddot, qEddot;
FColDsptr rpmp;
FMatDsptr aApm;
FRowDsptr fullRow;
@@ -68,10 +68,14 @@ int main()
qE = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0, 1 });
assembly1->setqX(qX);
assembly1->setqE(qE);
//qXdot = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
//omeOpO = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0, 1 });
//assembly1->setqXdot(qXdot);
//assembly1->setomeOpO(omeOpO);
qXdot = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
omeOpO = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
assembly1->setqXdot(qXdot);
assembly1->setomeOpO(omeOpO);
qXddot = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
qEddot = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
assembly1->setqXddot(qXdot);
assembly1->setqEddot(omeOpO);
std::cout << "assembly1->getqX() " << *assembly1->getqX() << std::endl;
std::cout << "assembly1->getqE() " << *assembly1->getqE() << std::endl;
TheSystem.addPart(assembly1);
@@ -109,6 +113,14 @@ int main()
qE = std::make_shared<FullColumn<double>>(ListD{ 0.0, 0.0, 0.0, 1.0 });
crankPart1->setqX(qX);
crankPart1->setqE(qE);
qXdot = std::make_shared<FullColumn<double>>(ListD{ 0, 0.096568457800423, 0 });
omeOpO = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0.25132741228718 });
crankPart1->setqXdot(qXdot);
crankPart1->setomeOpO(omeOpO);
qXddot = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
qEddot = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
crankPart1->setqXddot(qXdot);
crankPart1->setqEddot(omeOpO);
TheSystem.parts->push_back(crankPart1);
{
auto& partFrame = crankPart1->partFrame;
@@ -143,6 +155,14 @@ int main()
qE = std::make_shared<FullColumn<double>>(ListD{ 0.0, 0.0, 0.89871703427292, 0.43852900965351 });
conrodPart2->setqX(qX);
conrodPart2->setqE(qE);
qXdot = std::make_shared<FullColumn<double>>(ListD{ 0, 0.19313691560085, 0 });
omeOpO = std::make_shared<FullColumn<double>>(ListD{ 1.670970041317e-34, 1.3045598281729e-34, -1.2731200314796e-35 });
crankPart1->setqXdot(qXdot);
crankPart1->setomeOpO(omeOpO);
qXddot = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
qEddot = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
crankPart1->setqXddot(qXdot);
crankPart1->setqEddot(omeOpO);
TheSystem.parts->push_back(conrodPart2);
{
auto& partFrame = conrodPart2->partFrame;
@@ -177,6 +197,14 @@ int main()
qE = std::make_shared<FullColumn<double>>(ListD{ 0.70710678118655, 0.70710678118655, 0.0, 0.0 });
pistonPart3->setqX(qX);
pistonPart3->setqE(qE);
qXdot = std::make_shared<FullColumn<double>>(ListD{ -6.3364526821409e-32, 0.19313691560085, -1.933731897626e-34 });
omeOpO = std::make_shared<FullColumn<double>>(ListD{ 1.670970041317e-34, 1.3045598281729e-34, 1.8896472173894e-50 });
crankPart1->setqXdot(qXdot);
crankPart1->setomeOpO(omeOpO);
qXddot = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
qEddot = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
crankPart1->setqXddot(qXdot);
crankPart1->setqEddot(omeOpO);
TheSystem.parts->push_back(pistonPart3);
{
auto& partFrame = pistonPart3->partFrame;

Some files were not shown because too many files have changed in this diff Show More