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

systemSolver->runBasicKinematic();

Browse Source
This commit is contained in:
Aik-Siong Koh
2023-06-11 07:15:20 -06:00
parent d848450907
commit 3b08cd72df
182 changed files with 2789 additions and 535 deletions
Download Patch File Download Diff File Expand all files Collapse all files

25
MbDCode/AbsConstraint.cpp
View File

@@ -2,20 +2,19 @@
#include "PartFrame.h"
using namespace MbD;
//
//AbsConstraint::AbsConstraint() {}
//
//AbsConstraint::AbsConstraint(const char* str) : Constraint(str) {}
AbsConstraint::AbsConstraint() {}
AbsConstraint::AbsConstraint(const char* str) : Constraint(str) {}
AbsConstraint::AbsConstraint(size_t i)
AbsConstraint::AbsConstraint(int i)
{
axis = i;
}
void AbsConstraint::initialize()
{
axis = 0;
iqXminusOnePlusAxis = 0;
Constraint::initialize();
}
void MbD::AbsConstraint::calcPostDynCorrectorIteration()
@@ -30,11 +29,17 @@ void MbD::AbsConstraint::calcPostDynCorrectorIteration()
void MbD::AbsConstraint::useEquationNumbers()
{
iqXminusOnePlusAxis = static_cast<PartFrame*>(owner)->iqX - 1 + axis;
iqXminusOnePlusAxis = static_cast<PartFrame*>(owner)->iqX + axis;
}
void MbD::AbsConstraint::fillPosICJacob(SpMatDsptr mat)
{
mat->atijplusNumber(iG, iqXminusOnePlusAxis, 1.0);
mat->atijplusNumber(iqXminusOnePlusAxis, iG, 1.0);
(*(mat->at(iG)))[iqXminusOnePlusAxis] += 1.0;
(*(mat->at(iqXminusOnePlusAxis)))[iG] += 1.0;
}
void MbD::AbsConstraint::fillPosICError(FColDsptr col)
{
Constraint::fillPosICError(col);
col->at(iqXminusOnePlusAxis) += lam;
}

11
MbDCode/AbsConstraint.h
View File

@@ -5,16 +5,17 @@ namespace MbD {
{
//axis iqXminusOnePlusAxis
public:
AbsConstraint();
AbsConstraint(const char* str);
AbsConstraint(size_t axis);
//AbsConstraint();
//AbsConstraint(const char* str);
AbsConstraint(int axis);
void initialize();
void calcPostDynCorrectorIteration() override;
void useEquationNumbers() override;
void fillPosICJacob(SpMatDsptr mat) override;
void fillPosICError(FColDsptr col) override;
size_t axis = -1;
size_t iqXminusOnePlusAxis = -1;
int axis = -1;
int iqXminusOnePlusAxis = -1;
};
}

15
MbDCode/AnyPosICNewtonRaphson.cpp
View File

@@ -1,6 +1,7 @@
#include "AnyPosICNewtonRaphson.h"
#include "SystemSolver.h"
#include "Item.h"
#include <iostream>
using namespace MbD;
@@ -33,17 +34,25 @@ void MbD::AnyPosICNewtonRaphson::fillY()
newMinusOld->equalSelfPlusFullColumnAt(x, 0);
y->zeroSelf();
y->atiminusFullColumn(0, (qsuWeights->timesFullColumn(newMinusOld)));
system->partsJointsMotionsDo([&](std::shared_ptr<Item> item) { item->fillPosICError(y); });
system->partsJointsMotionsDo([&](std::shared_ptr<Item> item) {
item->fillPosICError(y);
//std::cout << *y << std::endl;
});
//std::cout << *y << std::endl;
}
void MbD::AnyPosICNewtonRaphson::fillPyPx()
{
pypx->zeroSelf();
pypx->atijminusDiagonalMatrix(0, 0, qsuWeights);
system->partsJointsMotionsDo([&](std::shared_ptr<Item> item) {item->fillPosICJacob(pypx); });
system->partsJointsMotionsDo([&](std::shared_ptr<Item> item) {
item->fillPosICJacob(pypx);
//std::cout << *(pypx->at(3)) << std::endl;
});
//std::cout << *pypx << std::endl;
}
void MbD::AnyPosICNewtonRaphson::passRootToSystem()
{
system->partsJointsMotionsDo([&](std::shared_ptr<Item> item) {item->setqsulam(x); });
system->partsJointsMotionsDo([&](std::shared_ptr<Item> item) { item->setqsulam(x); });
}

4
MbDCode/AnyPosICNewtonRaphson.h
View File

@@ -16,10 +16,10 @@ namespace MbD {
void fillPyPx() override;
void passRootToSystem() override;
size_t nqsu = -1;
int nqsu = -1;
std::shared_ptr<FullColumn<double>> qsuOld;
std::shared_ptr<DiagonalMatrix<double>> qsuWeights;
size_t nSingularMatrixError = -1;
int nSingularMatrixError = -1;
};
}

24
MbDCode/Array.h
View File

@@ -12,33 +12,37 @@ namespace MbD {
{
public:
Array() {}
Array(size_t count) : std::vector<T>(count) {}
Array(size_t count, const T& value) : std::vector<T>(count, value) {}
Array(std::vector<T> vec) : std::vector<T>(vec) {}
Array(int count) : std::vector<T>(count) {}
Array(int count, const T& value) : std::vector<T>(count, value) {}
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 double sumOfSquares() = 0;
double rootMeanSquare();
virtual size_t numberOfElements() = 0;
virtual int numberOfElements() = 0;
void swapRows(int i, int ii);
};
template<typename T>
inline void Array<T>::copyFrom(std::shared_ptr<Array<T>> x)
{
for (size_t i = 0; i < x->size(); i++) {
for (int i = 0; i < x->size(); i++) {
this->at(i) = x->at(i);
}
}
//template <>
//inline void Array<double>::zeroSelf() {
// for (size_t i = 0; i < this->size(); i++) {
// this->at(i) = 0.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)
{
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>>>;

3
MbDCode/AtPointConstraintIJ.cpp
View File

@@ -4,13 +4,14 @@
using namespace MbD;
AtPointConstraintIJ::AtPointConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi) :
AtPointConstraintIJ::AtPointConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj, int axisi) :
ConstraintIJ(frmi, frmj), axis(axisi)
{
}
void AtPointConstraintIJ::initialize()
{
ConstraintIJ::initialize();
initriIeJeO();
}

4
MbDCode/AtPointConstraintIJ.h
View File

@@ -9,7 +9,7 @@ namespace MbD {
{
//axis riIeJeO
public:
AtPointConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi);
AtPointConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj, int axisi);
void initialize();
void initializeLocally() override;
void initializeGlobally() override;
@@ -20,7 +20,7 @@ namespace MbD {
MbD::ConstraintType type() override;
void postPosICIteration() override;
size_t axis;
int axis;
std::shared_ptr<DispCompIecJecO> riIeJeO;
};
}

20
MbDCode/AtPointConstraintIqcJc.cpp
View File

@@ -5,7 +5,7 @@
using namespace MbD;
AtPointConstraintIqcJc::AtPointConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi) :
AtPointConstraintIqcJc::AtPointConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi) :
AtPointConstraintIJ(frmi, frmj, axisi)
{
}
@@ -29,6 +29,22 @@ void MbD::AtPointConstraintIqcJc::calcPostDynCorrectorIteration()
void MbD::AtPointConstraintIqcJc::useEquationNumbers()
{
iqXIminusOnePlusAxis = std::static_pointer_cast<EndFrameqc>(frmI)->iqX() - 1 + axis;
iqXIminusOnePlusAxis = std::static_pointer_cast<EndFrameqc>(frmI)->iqX() + axis;
iqEI = std::static_pointer_cast<EndFrameqc>(frmI)->iqE();
}
void MbD::AtPointConstraintIqcJc::fillPosICError(FColDsptr col)
{
Constraint::fillPosICError(col);
col->at(iqXIminusOnePlusAxis) -= lam;
col->atiplusFullVectortimes(iqEI, pGpEI, lam);
}
void MbD::AtPointConstraintIqcJc::fillPosICJacob(SpMatDsptr mat)
{
(*(mat->at(iG)))[iqXIminusOnePlusAxis] += -1.0;
(*(mat->at(iqXIminusOnePlusAxis)))[iG] += -1.0;
mat->atijplusFullRow(iG, iqEI, pGpEI);
mat->atijplusFullColumn(iqEI, iG, pGpEI->transpose());
mat->atijplusFullMatrixtimes(iqEI, iqEI, ppGpEIpEI, lam);
}

8
MbDCode/AtPointConstraintIqcJc.h
View File

@@ -7,16 +7,18 @@ namespace MbD {
{
//pGpEI ppGpEIpEI iqXIminusOnePlusAxis iqEI
public:
AtPointConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi);
AtPointConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi);
void initializeGlobally() override;
void initriIeJeO() override;
void calcPostDynCorrectorIteration() override;
void useEquationNumbers() override;
void fillPosICError(FColDsptr col) override;
void fillPosICJacob(SpMatDsptr mat) override;
FRowDsptr pGpEI;
FMatDsptr ppGpEIpEI;
size_t iqXIminusOnePlusAxis = -1;
size_t iqEI = -1;
int iqXIminusOnePlusAxis = -1;
int iqEI = -1;
};
}

21
MbDCode/AtPointConstraintIqcJqc.cpp
View File

@@ -5,7 +5,7 @@
using namespace MbD;
AtPointConstraintIqcJqc::AtPointConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi) :
AtPointConstraintIqcJqc::AtPointConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi) :
AtPointConstraintIqcJc(frmi, frmj, axisi)
{
}
@@ -30,6 +30,23 @@ void MbD::AtPointConstraintIqcJqc::calcPostDynCorrectorIteration()
void MbD::AtPointConstraintIqcJqc::useEquationNumbers()
{
AtPointConstraintIqcJc::useEquationNumbers();
iqXJminusOnePlusAxis = std::static_pointer_cast<EndFrameqc>(frmJ)->iqX() - 1 + axis;
iqXJminusOnePlusAxis = std::static_pointer_cast<EndFrameqc>(frmJ)->iqX() + axis;
iqEJ = std::static_pointer_cast<EndFrameqc>(frmJ)->iqE();
}
void MbD::AtPointConstraintIqcJqc::fillPosICError(FColDsptr col)
{
AtPointConstraintIqcJc::fillPosICError(col);
col->at(iqXJminusOnePlusAxis) += lam;
col->atiplusFullVectortimes(iqEJ, pGpEJ, lam);
}
void MbD::AtPointConstraintIqcJqc::fillPosICJacob(SpMatDsptr mat)
{
AtPointConstraintIqcJc::fillPosICJacob(mat);
(*(mat->at(iG)))[iqXJminusOnePlusAxis] += 1.0;
(*(mat->at(iqXJminusOnePlusAxis)))[iG] += 1.0;
mat->atijplusFullRow(iG, iqEJ, pGpEJ);
mat->atijplusFullColumn(iqEJ, iG, pGpEJ->transpose());
mat->atijplusFullMatrixtimes(iqEJ, iqEJ, ppGpEJpEJ, lam);
}

6
MbDCode/AtPointConstraintIqcJqc.h
View File

@@ -7,15 +7,17 @@ namespace MbD {
{
//pGpEJ ppGpEJpEJ iqXJminusOnePlusAxis iqEJ
public:
AtPointConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi);
AtPointConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi);
void initializeGlobally() override;
void initriIeJeO() override;
void calcPostDynCorrectorIteration() override;
void useEquationNumbers() override;
void fillPosICError(FColDsptr col) override;
void fillPosICJacob(SpMatDsptr mat) override;
FRowDsptr pGpEJ;
FMatDsptr ppGpEJpEJ;
size_t iqXJminusOnePlusAxis = -1, iqEJ = -1;
int iqXJminusOnePlusAxis = -1, iqEJ = -1;
};
}

2
MbDCode/AtPointConstraintIqctJqc.cpp
View File

@@ -4,7 +4,7 @@
using namespace MbD;
AtPointConstraintIqctJqc::AtPointConstraintIqctJqc(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi) :
AtPointConstraintIqctJqc::AtPointConstraintIqctJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi) :
AtPointConstraintIqcJqc(frmi, frmj, axisi)
{
}

2
MbDCode/AtPointConstraintIqctJqc.h
View File

@@ -7,7 +7,7 @@ namespace MbD {
{
//pGpt ppGpEIpt ppGptpt
public:
AtPointConstraintIqctJqc(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi);
AtPointConstraintIqctJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi);
void initializeGlobally() override;
void initriIeJeO() override;
void calcPostDynCorrectorIteration() override;

138
MbDCode/BasicIntegrator.cpp
View File

@@ -1,3 +1,141 @@
#include "BasicIntegrator.h"
#include "CREATE.h"
#include "StableBackwardDifference.h"
#include "IntegratorInterface.h"
using namespace MbD;
void MbD::BasicIntegrator::initializeLocally()
{
_continue = true;
}
void MbD::BasicIntegrator::iStep(int integer)
{
istep = integer;
opBDF->setiStep(integer);
}
void MbD::BasicIntegrator::postFirstStep()
{
}
void MbD::BasicIntegrator::postRun()
{
}
void MbD::BasicIntegrator::postStep()
{
}
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->direction = system->direction;
this->orderMax = system->orderMax();
}
void MbD::BasicIntegrator::setSystem(IntegratorInterface* sys)
{
system = sys;
}
void MbD::BasicIntegrator::calcOperatorMatrix()
{
opBDF->calcOperatorMatrix();
}
void MbD::BasicIntegrator::incrementTime()
{
//| istepNew |
tpast->insert(tpast->begin(), t);
if ((int)tpast->size() > (orderMax + 1)) { tpast->pop_back(); }
auto istepNew = istep + 1;
this->iStep(istepNew);
this->setorder(orderNew);
h = hnew;
this->settnew(t + (direction * h));
this->calcOperatorMatrix();
//system->incrementTime(tnew);
}
void MbD::BasicIntegrator::incrementTry()
{
assert(false);
}
void MbD::BasicIntegrator::initialize()
{
Solver::initialize();
//statistics = IdentityDictionary new.
tpast = std::make_shared<std::vector<double>>();
opBDF = CREATE<StableBackwardDifference>::With();
opBDF->timeNodes = tpast;
}
void MbD::BasicIntegrator::logString(std::string& str)
{
system->logString(str);
}
void MbD::BasicIntegrator::run()
{
this->preRun();
this->initializeLocally();
this->initializeGlobally();
this->firstStep();
this->subsequentSteps();
this->finalize();
this->reportStats();
this->postRun();
}
void MbD::BasicIntegrator::selectOrder()
{
assert(false);
}
void MbD::BasicIntegrator::preFirstStep()
{
system->preFirstStep();
}
void MbD::BasicIntegrator::preRun()
{
}
void MbD::BasicIntegrator::preStep()
{
assert(false);
}
void MbD::BasicIntegrator::reportStats()
{
assert(false);
}
void MbD::BasicIntegrator::firstStep()
{
assert(false);
}
void MbD::BasicIntegrator::setorder(int o)
{
order = o;
//opBD->setorder(o);
}
void MbD::BasicIntegrator::settnew(double t)
{
tnew = t;
//this->time(double);
}
void MbD::BasicIntegrator::subsequentSteps()
{
while (_continue) { this->nextStep(); }
}

37
MbDCode/BasicIntegrator.h
View File

@@ -1,13 +1,48 @@
#pragma once
#include <vector>
#include "Integrator.h"
namespace MbD {
class IntegratorInterface;
class DifferenceOperator;
class BasicIntegrator : public Integrator
{
//
//istep iTry maxTry tpast t tnew h hnew order orderNew orderMax opBDF continue
public:
virtual void calcOperatorMatrix();
virtual void incrementTime();
virtual void incrementTry();
void initialize() override;
void initializeGlobally() override;
void initializeLocally() override;
virtual void iStep(int i);
virtual void postFirstStep();
virtual void postRun() override;
virtual void postStep();
virtual void preFirstStep();
virtual void preRun() override;
virtual void preStep();
virtual void reportStats() override;
void run() override;
virtual void selectOrder();
virtual void subsequentSteps();
void setSystem(IntegratorInterface* sys);
void logString(std::string& str) override;
virtual void firstStep();
virtual void nextStep() = 0;
virtual void setorder(int o);
virtual void settnew(double t);
IntegratorInterface* system;
int istep = 0, iTry = 0, maxTry = 0;
std::shared_ptr<std::vector<double>> tpast;
double t = 0, tnew = 0, h = 0, hnew = 0;
int order = 0, orderNew = 0, orderMax = 0;
std::shared_ptr<DifferenceOperator> opBDF;
bool _continue = false;
};
}

57
MbDCode/BasicQuasiIntegrator.cpp
View File

@@ -1,3 +1,60 @@
#include "BasicQuasiIntegrator.h"
#include "IntegratorInterface.h"
using namespace MbD;
void MbD::BasicQuasiIntegrator::firstStep()
{
istep = 0;
this->preFirstStep();
iTry = 1;
orderNew = 1;
this->selectFirstStepSize();
this->incrementTime();
//this->runInitialConditionTypeSolution();
//this->reportTrialStepStats();
//while (this->isRedoingFirstStep())
//{
// this->incrementTry();
// orderNew = 1;
// this->selectFirstStepSize();
// this->changeTime();
// this->runInitialConditionTypeSolution();
// this->reportTrialStepStats();
//}
//this->postFirstStep();
//this->reportStepStats();
}
void MbD::BasicQuasiIntegrator::nextStep()
{
this->preStep();
iTry = 1;
this->selectOrder();
//this->selectStepSize();
//this->incrementTime();
//this->runInitialConditionTypeSolution();
//this->reportTrialStepStats();
//while (this->isRedoingStep()) {
// this->incrementTry();
// this->selectOrder();
// this->selectStepSize();
// this->changeTime();
// this->runInitialConditionTypeSolution();
// this->reportTrialStepStats();
//}
//this->postStep();
//this->reportStepStats();
}
void MbD::BasicQuasiIntegrator::selectFirstStepSize()
{
if (iTry == 1) {
hnew = direction * (system->tout - t);
}
else {
hnew = 0.25 * h;
}
hnew = system->suggestSmallerOrAcceptFirstStepSize(hnew);
}

21
MbDCode/BasicQuasiIntegrator.h
View File

@@ -3,11 +3,20 @@
#include "BasicIntegrator.h"
namespace MbD {
class BasicQuasiIntegrator : public BasicIntegrator
{
//
public:
};
class BasicQuasiIntegrator : public BasicIntegrator
{
//
public:
void firstStep();
//bool isRedoingFirstStep();
//bool isRedoingStep();
void nextStep();
//void reportStepStats();
//void reportTrialStepStats();
//void runInitialConditionTypeSolution();
void selectFirstStepSize();
//void selectStepSize();
//void runInitialConditionTypeSolution();
};
}

12
MbDCode/CREATE.h
View File

@@ -23,7 +23,7 @@ namespace MbD {
inst->initialize();
return inst;
}
static std::shared_ptr<T> With(size_t n) {
static std::shared_ptr<T> With(int n) {
auto inst = std::make_shared<T>(n);
inst->initialize();
return inst;
@@ -33,17 +33,17 @@ namespace MbD {
inst->initialize();
return inst;
}
static std::shared_ptr<T> With(EndFrmcptr frmi, EndFrmcptr frmj, size_t axis) {
static std::shared_ptr<T> With(EndFrmcptr frmi, EndFrmcptr frmj, int axis) {
auto inst = std::make_shared<T>(frmi, frmj, axis);
inst->initialize();
return inst;
}
static std::shared_ptr<T> With(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, size_t axisk) {
static std::shared_ptr<T> With(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk) {
auto inst = std::make_shared<T>(frmi, frmj, frmk, axisk);
inst->initialize();
return inst;
}
static std::shared_ptr<Constraint> ConstraintWith(EndFrmcptr frmi, EndFrmcptr frmj, size_t axis) {
static std::shared_ptr<Constraint> ConstraintWith(EndFrmcptr frmi, EndFrmcptr frmj, int axis) {
std::shared_ptr<Constraint> inst;
std::string str = typeid(T(frmi, frmj, axis)).name();
if (str == "class MbD::AtPointConstraintIJ") {
@@ -65,12 +65,12 @@ namespace MbD {
inst->initialize();
return inst;
}
static std::shared_ptr<T> With(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi, size_t axisj) {
static std::shared_ptr<T> With(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj) {
auto inst = std::make_shared<T>(frmi, frmj, axisi, axisj);
inst->initialize();
return inst;
}
static std::shared_ptr<Constraint> ConstraintWith(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi, size_t axisj) {
static std::shared_ptr<Constraint> ConstraintWith(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj) {
std::shared_ptr<Constraint> inst;
std::string str = typeid(T(frmi, frmj, axisi, axisj)).name();
if (str == "class MbD::DirectionCosineConstraintIJ") {

61
MbDCode/Constraint.cpp
View File

@@ -1,4 +1,8 @@
#include <functional>
#include <chrono>
#include "Constraint.h"
#include "FullColumn.h"
#include "enum.h"
using namespace MbD;
@@ -13,9 +17,9 @@ Constraint::Constraint(const char* str) : Item(str)
void Constraint::initialize()
{
iG = -1;
aG = 0.0;
lam = 0.0;
auto now = std::chrono::high_resolution_clock::now();
auto nanoseconds = now.time_since_epoch().count();
name = std::to_string(nanoseconds);
}
void MbD::Constraint::postInput()
@@ -41,25 +45,32 @@ void MbD::Constraint::prePosIC()
Item::prePosIC();
}
void MbD::Constraint::fillEssenConstraints(std::shared_ptr<Constraint>sptr, std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> essenConstraints)
void MbD::Constraint::fillEssenConstraints(std::shared_ptr<Constraint> sptr, std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> essenConstraints)
{
if (this->type() == MbD::essential) {
essenConstraints->push_back(sptr);
}
}
void MbD::Constraint::fillDispConstraints(std::shared_ptr<Constraint>sptr, std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> dispConstraints)
void MbD::Constraint::fillDispConstraints(std::shared_ptr<Constraint> sptr, std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> dispConstraints)
{
if (this->type() == MbD::displacement) {
dispConstraints->push_back(sptr);
}
}
void MbD::Constraint::fillPerpenConstraints(std::shared_ptr<Constraint>sptr, std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> perpenConstraints)
void MbD::Constraint::fillPerpenConstraints(std::shared_ptr<Constraint> sptr, std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> perpenConstraints)
{
if (this->type() == MbD::perpendicular) {
perpenConstraints->push_back(sptr);
}
}
void MbD::Constraint::fillRedundantConstraints(std::shared_ptr<Constraint> sptr, std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> redunConstraints)
{
if (this->type() == MbD::redundant) {
redunConstraints->push_back(sptr);
}
}
MbD::ConstraintType MbD::Constraint::type()
{
return MbD::essential;
@@ -74,3 +85,41 @@ void MbD::Constraint::setqsulam(FColDsptr col)
{
lam = col->at(iG);
}
void MbD::Constraint::fillPosICError(FColDsptr col)
{
col->at(iG) += aG;
}
void MbD::Constraint::removeRedundantConstraints(std::shared_ptr<std::vector<int>> redundantEqnNos)
{
//My owner should handle this.
assert(false);
}
void MbD::Constraint::reactivateRedundantConstraints()
{
//My owner should handle this.
assert(false);
}
bool MbD::Constraint::isRedundant()
{
return false;
}
void MbD::Constraint::outputStates()
{
Item::outputStates();
std::stringstream ss;
ss << "iG = " << iG << std::endl;
ss << "aG = " << aG << std::endl;
ss << "lam = " << lam << std::endl;
auto str = ss.str();
this->logString(str);
}
void MbD::Constraint::preDyn()
{
mu = 0.0;
}

22
MbDCode/Constraint.h
View File

@@ -16,16 +16,24 @@ namespace MbD {
void setOwner(Item* x);
Item* getOwner();
void prePosIC() 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 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 MbD::ConstraintType type();
void fillqsulam(FColDsptr col) override;
void setqsulam(FColDsptr col) override;
size_t iG;
double aG; //Constraint function
double lam; //Lambda is Lagrange Multiplier
void fillPosICError(FColDsptr col) override;
void removeRedundantConstraints(std::shared_ptr<std::vector<int>> redundantEqnNos) override;
void reactivateRedundantConstraints() override;
virtual bool isRedundant();
void outputStates() override;
void preDyn() override;
int iG = -1;
double aG = 0.0; //Constraint function
double lam = 0.0; //Lambda is Lagrange Multiplier
double mu = 0, lamDeriv = 0;
Item* owner; //A Joint or PartFrame owns the constraint. //Use raw pointer when pointing backwards.
};
}

30
MbDCode/DiagonalMatrix.h
View File

@@ -9,24 +9,24 @@ namespace MbD {
{
//
public:
DiagonalMatrix(size_t count) : Array<T>(count) {}
DiagonalMatrix(int count) : Array<T>(count) {}
DiagonalMatrix(std::initializer_list<T> list) : Array<T>{ list } {}
void atiputDiagonalMatrix(size_t i, std::shared_ptr < DiagonalMatrix<T>> diagMat);
void atiputDiagonalMatrix(int i, std::shared_ptr < DiagonalMatrix<T>> diagMat);
std::shared_ptr<FullColumn<T>> timesFullColumn(std::shared_ptr<FullColumn<T>> fullCol);
size_t nrow() {
return this->size();
int nrow() {
return (int) this->size();
}
size_t ncol() {
return this->size();
int ncol() {
return (int) this->size();
}
double sumOfSquares() override;
size_t numberOfElements() override;
int numberOfElements() override;
void zeroSelf() override;
};
template<typename T>
inline void DiagonalMatrix<T>::atiputDiagonalMatrix(size_t i, std::shared_ptr<DiagonalMatrix<T>> diagMat)
inline void DiagonalMatrix<T>::atiputDiagonalMatrix(int i, std::shared_ptr<DiagonalMatrix<T>> diagMat)
{
for (size_t ii = 0; ii < diagMat->size(); ii++)
for (int ii = 0; ii < diagMat->size(); ii++)
{
this->at(i + ii) = diagMat->at(ii);
}
@@ -36,9 +36,9 @@ namespace MbD {
{
//"a*b = a(i,j)b(j) sum j."
auto nrow = this->size();
auto nrow = (int) this->size();
auto answer = std::make_shared<FullColumn<T>>(nrow);
for (size_t i = 0; i < nrow; i++)
for (int i = 0; i < nrow; i++)
{
answer->at(i) = this->at(i) * fullCol->at(i);
}
@@ -48,7 +48,7 @@ namespace MbD {
inline double DiagonalMatrix<double>::sumOfSquares()
{
double sum = 0.0;
for (size_t i = 0; i < this->size(); i++)
for (int i = 0; i < this->size(); i++)
{
double element = this->at(i);
sum += element * element;
@@ -56,15 +56,15 @@ namespace MbD {
return sum;
}
template<typename T>
inline size_t DiagonalMatrix<T>::numberOfElements()
inline int DiagonalMatrix<T>::numberOfElements()
{
auto n = this->size();
auto n = (int) this->size();
return n * n;
}
template<>
inline void DiagonalMatrix<double>::zeroSelf()
{
for (size_t i = 0; i < this->size(); i++) {
for (int i = 0; i < this->size(); i++) {
this->at(i) = 0.0;;
}
}

63
MbDCode/DifferenceOperator.cpp Normal file
View File

@@ -0,0 +1,63 @@
#include "DifferenceOperator.h"
#include "CREATE.h"
#include "SingularMatrixError.h"
#include "LDUFullMatParPv.h"
void MbD::DifferenceOperator::calcOperatorMatrix()
{
//Compute operatorMatrix such that
//value(time) : = (operatorMatrix at : 1) timesColumn : series.
//valuedot(time) : = (operatorMatrix at : 2) timesColumn : series.
//valueddot(time) : = (operatorMatrix at : 3) timesColumn : series.
this->formTaylorMatrix();
try {
assert(false);
//operatorMatrix = CREATE<LDUFullMatParPv>::With()->inversesaveOriginal(taylorMatrix, false);
}
catch (SingularMatrixError ex) {
}
}
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)
{
iStep = i;
}
void MbD::DifferenceOperator::setorder(int o)
{
order = o;
}
void MbD::DifferenceOperator::instanciateTaylorMatrix()
{
if (taylorMatrix->empty() || (taylorMatrix->nrow() != (order + 1))) {
taylorMatrix = std::make_shared<FullMatrix<double>>(order + 1, order + 1);
}
}
void MbD::DifferenceOperator::formTaylorRowwithTimeNodederivative(int i, int ii, int k)
{
//| rowi hi hipower aij |
auto rowi = taylorMatrix->at(i);
for (int j = 0; j < k; j++)
{
rowi->at(j) = 0.0;
}
rowi->at(k) = 1.0;
auto hi = timeNodes->at(ii) - time;
auto hipower = 1.0;
for (int j = k + 1; j < order + 1; j++)
{
hipower = hipower * hi;
assert(false);
//aij = hipower * (OneOverFactorials at : j - k - 1);
//rowi at : j put : aij
}
}

26
MbDCode/DifferenceOperator.h Normal file
View File

@@ -0,0 +1,26 @@
#pragma once
#include <memory>
#include "FullMatrix.h"
namespace MbD {
class DifferenceOperator
{
//iStep order taylorMatrix operatorMatrix time timeNodes
public:
void calcOperatorMatrix();
void initialize();
virtual void setiStep(int i);
virtual void setorder(int o);
virtual void formTaylorMatrix() = 0;
virtual void instanciateTaylorMatrix();
virtual void formTaylorRowwithTimeNodederivative(int i, int ii, int k);
int iStep = 0, order = 0;
std::shared_ptr<FullMatrix<double>> taylorMatrix, operatorMatrix;
double time = 0;
std::shared_ptr<std::vector<double>> timeNodes;
};
}

3
MbDCode/DirectionCosineConstraintIJ.cpp
View File

@@ -5,13 +5,14 @@
using namespace MbD;
DirectionCosineConstraintIJ::DirectionCosineConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi, size_t axisj) :
DirectionCosineConstraintIJ::DirectionCosineConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj) :
ConstraintIJ(frmi, frmj), axisI(axisi), axisJ(axisj)
{
}
void DirectionCosineConstraintIJ::initialize()
{
ConstraintIJ::initialize();
initaAijIeJe();
}

4
MbDCode/DirectionCosineConstraintIJ.h
View File

@@ -9,7 +9,7 @@ namespace MbD {
{
//axisI axisJ aAijIeJe
public:
DirectionCosineConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi, size_t axisj);
DirectionCosineConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj);
void initialize();
void initializeLocally() override;
void initializeGlobally() override;
@@ -20,7 +20,7 @@ namespace MbD {
void postPosICIteration() override;
MbD::ConstraintType type() override;
size_t axisI, axisJ;
int axisI, axisJ;
std::shared_ptr<DirectionCosineIecJec> aAijIeJe;
};
}

15
MbDCode/DirectionCosineConstraintIqcJc.cpp
View File

@@ -5,7 +5,7 @@
using namespace MbD;
DirectionCosineConstraintIqcJc::DirectionCosineConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi, size_t axisj) :
DirectionCosineConstraintIqcJc::DirectionCosineConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj) :
DirectionCosineConstraintIJ(frmi, frmj, axisi, axisj)
{
}
@@ -27,3 +27,16 @@ void MbD::DirectionCosineConstraintIqcJc::useEquationNumbers()
{
iqEI = std::static_pointer_cast<EndFrameqc>(frmI)->iqE();
}
void MbD::DirectionCosineConstraintIqcJc::fillPosICError(FColDsptr col)
{
Constraint::fillPosICError(col);
col->atiplusFullVectortimes(iqEI, pGpEI, lam);
}
void MbD::DirectionCosineConstraintIqcJc::fillPosICJacob(SpMatDsptr mat)
{
mat->atijplusFullRow(iG, iqEI, pGpEI);
mat->atijplusFullColumn(iqEI, iG, pGpEI->transpose());
mat->atijplusFullMatrixtimes(iqEI, iqEI, ppGpEIpEI, lam);
}

6
MbDCode/DirectionCosineConstraintIqcJc.h
View File

@@ -7,14 +7,16 @@ namespace MbD {
{
//pGpEI ppGpEIpEI iqEI
public:
DirectionCosineConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi, size_t axisj);
DirectionCosineConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj);
void initaAijIeJe() override;
void calcPostDynCorrectorIteration() override;
void useEquationNumbers() override;
void fillPosICError(FColDsptr col) override;
void fillPosICJacob(SpMatDsptr mat) override;
FRowDsptr pGpEI;
FMatDsptr ppGpEIpEI;
size_t iqEI = -1;
int iqEI = -1;
};
}

19
MbDCode/DirectionCosineConstraintIqcJqc.cpp
View File

@@ -5,7 +5,7 @@
using namespace MbD;
DirectionCosineConstraintIqcJqc::DirectionCosineConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi, size_t axisj) :
DirectionCosineConstraintIqcJqc::DirectionCosineConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj) :
DirectionCosineConstraintIqcJc(frmi, frmj, axisi, axisj)
{
}
@@ -29,3 +29,20 @@ void MbD::DirectionCosineConstraintIqcJqc::useEquationNumbers()
DirectionCosineConstraintIqcJc::useEquationNumbers();
iqEJ = std::static_pointer_cast<EndFrameqc>(frmJ)->iqE();
}
void MbD::DirectionCosineConstraintIqcJqc::fillPosICError(FColDsptr col)
{
DirectionCosineConstraintIqcJc::fillPosICError(col);
col->atiplusFullVectortimes(iqEJ, pGpEJ, lam);
}
void MbD::DirectionCosineConstraintIqcJqc::fillPosICJacob(SpMatDsptr mat)
{
DirectionCosineConstraintIqcJc::fillPosICJacob(mat);
mat->atijplusFullRow(iG, iqEJ, pGpEJ);
mat->atijplusFullColumn(iqEJ, iG, pGpEJ->transpose());
auto ppGpEIpEJlam = ppGpEIpEJ->times(lam);
mat->atijplusFullMatrix(iqEI, iqEJ, ppGpEIpEJlam);
mat->atijplusTransposeFullMatrix(iqEJ, iqEI, ppGpEIpEJlam);
mat->atijplusFullMatrixtimes(iqEJ, iqEJ, ppGpEJpEJ, lam);
}

6
MbDCode/DirectionCosineConstraintIqcJqc.h
View File

@@ -7,15 +7,17 @@ namespace MbD {
{
//pGpEJ ppGpEIpEJ ppGpEJpEJ iqEJ
public:
DirectionCosineConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi, size_t axisj);
DirectionCosineConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj);
void initaAijIeJe() override;
void calcPostDynCorrectorIteration() override;
void useEquationNumbers() override;
void fillPosICError(FColDsptr col) override;
void fillPosICJacob(SpMatDsptr mat) override;
FRowDsptr pGpEJ;
FMatDsptr ppGpEIpEJ;
FMatDsptr ppGpEJpEJ;
size_t iqEJ = -1;
int iqEJ = -1;
};
}

2
MbDCode/DirectionCosineConstraintIqctJqc.cpp
View File

@@ -4,7 +4,7 @@
using namespace MbD;
DirectionCosineConstraintIqctJqc::DirectionCosineConstraintIqctJqc(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi, size_t axisj) :
DirectionCosineConstraintIqctJqc::DirectionCosineConstraintIqctJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj) :
DirectionCosineConstraintIqcJqc(frmi, frmj, axisi, axisj)
{
}

2
MbDCode/DirectionCosineConstraintIqctJqc.h
View File

@@ -7,7 +7,7 @@ namespace MbD {
{
//pGpt ppGpEIpt ppGpEJpt ppGptpt
public:
DirectionCosineConstraintIqctJqc(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi, size_t axisj);
DirectionCosineConstraintIqctJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj);
void initaAijIeJe() override;
MbD::ConstraintType type() override;

32
MbDCode/DirectionCosineIecJec.cpp
View File

@@ -9,7 +9,7 @@ DirectionCosineIecJec::DirectionCosineIecJec()
{
}
DirectionCosineIecJec::DirectionCosineIecJec(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi, size_t axisj) :
DirectionCosineIecJec::DirectionCosineIecJec(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj) :
KinematicIeJe(frmi, frmj), axisI(axisi), axisJ(axisj)
{
@@ -21,3 +21,33 @@ void MbD::DirectionCosineIecJec::calcPostDynCorrectorIteration()
aAjOJe = frmJ->aAjOe(axisJ);
aAijIeJe = aAjOIe->dot(aAjOJe);
}
FRowDsptr MbD::DirectionCosineIecJec::pvaluepXJ()
{
assert(false);
return FRowDsptr();
}
FRowDsptr MbD::DirectionCosineIecJec::pvaluepEJ()
{
assert(false);
return FRowDsptr();
}
FMatDsptr MbD::DirectionCosineIecJec::ppvaluepXJpEK()
{
assert(false);
return FMatDsptr();
}
FMatDsptr MbD::DirectionCosineIecJec::ppvaluepEJpEK()
{
assert(false);
return FMatDsptr();
}
FMatDsptr MbD::DirectionCosineIecJec::ppvaluepEJpEJ()
{
assert(false);
return FMatDsptr();
}

9
MbDCode/DirectionCosineIecJec.h
View File

@@ -12,10 +12,15 @@ namespace MbD {
//aAijIeJe axisI axisJ aAjOIe aAjOJe
public:
DirectionCosineIecJec();
DirectionCosineIecJec(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi, size_t axisj);
DirectionCosineIecJec(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj);
void calcPostDynCorrectorIteration() override;
FRowDsptr pvaluepXJ() override;
FRowDsptr pvaluepEJ() override;
FMatDsptr ppvaluepXJpEK() override;
FMatDsptr ppvaluepEJpEK() override;
FMatDsptr ppvaluepEJpEJ() override;
size_t axisI, axisJ; //0, 1, 2 = x, y, z
int axisI, axisJ; //0, 1, 2 = x, y, z
double aAijIeJe;
std::shared_ptr<FullColumn<double>> aAjOIe, aAjOJe;
};

8
MbDCode/DirectionCosineIeqcJec.cpp
View File

@@ -7,7 +7,7 @@ DirectionCosineIeqcJec::DirectionCosineIeqcJec()
{
}
DirectionCosineIeqcJec::DirectionCosineIeqcJec(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi, size_t axisj) :
DirectionCosineIeqcJec::DirectionCosineIeqcJec(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj) :
DirectionCosineIecJec(frmi, frmj, axisi, axisj)
{
}
@@ -28,15 +28,15 @@ void MbD::DirectionCosineIeqcJec::calcPostDynCorrectorIteration()
{
DirectionCosineIecJec::calcPostDynCorrectorIteration();
pAjOIepEIT = std::static_pointer_cast<EndFrameqc>(frmI)->pAjOepET(axisI);
for (size_t i = 0; i < 4; i++)
for (int i = 0; i < 4; i++)
{
pAijIeJepEI->at(i) = pAjOIepEIT->at(i)->dot(aAjOJe);
}
for (size_t i = 0; i < 4; i++)
for (int i = 0; i < 4; i++)
{
auto& ppAijIeJepEIipEI = ppAijIeJepEIpEI->at(i);
auto& ppAjOIepEIipEI = ppAjOIepEIpEI->at(i);
for (size_t j = 0; j < 4; j++)
for (int j = 0; j < 4; j++)
{
ppAijIeJepEIipEI->at(j) = ppAjOIepEIipEI->at(j)->dot(aAjOJe);
}

2
MbDCode/DirectionCosineIeqcJec.h
View File

@@ -8,7 +8,7 @@ namespace MbD {
//pAijIeJepEI ppAijIeJepEIpEI pAjOIepEIT ppAjOIepEIpEI
public:
DirectionCosineIeqcJec();
DirectionCosineIeqcJec(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi, size_t axisj);
DirectionCosineIeqcJec(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj);
void initialize();
void initializeGlobally();
void calcPostDynCorrectorIteration() override;

22
MbDCode/DirectionCosineIeqcJeqc.cpp
View File

@@ -7,7 +7,7 @@ DirectionCosineIeqcJeqc::DirectionCosineIeqcJeqc()
{
}
DirectionCosineIeqcJeqc::DirectionCosineIeqcJeqc(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi, size_t axisj) :
DirectionCosineIeqcJeqc::DirectionCosineIeqcJeqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj) :
DirectionCosineIeqcJec(frmi, frmj, axisi, axisj)
{
}
@@ -30,26 +30,36 @@ void MbD::DirectionCosineIeqcJeqc::calcPostDynCorrectorIteration()
{
DirectionCosineIeqcJec::calcPostDynCorrectorIteration();
pAjOJepEJT = std::static_pointer_cast<EndFrameqc>(frmJ)->pAjOepET(axisJ);
for (size_t i = 0; i < 4; i++)
for (int i = 0; i < 4; i++)
{
pAijIeJepEJ->at(i) = aAjOIe->dot(pAjOJepEJT->at(i));
}
for (size_t i = 0; i < 4; i++)
for (int i = 0; i < 4; i++)
{
auto& ppAijIeJepEIipEJ = ppAijIeJepEIpEJ->at(i);
for (size_t j = 0; j < 4; j++)
for (int j = 0; j < 4; j++)
{
ppAijIeJepEIipEJ->at(j) = pAjOIepEIT->at(i)->dot(pAjOJepEJT->at(j));
}
}
for (size_t i = 0; i < 4; i++)
for (int i = 0; i < 4; i++)
{
auto& ppAijIeJepEJipEJ = ppAijIeJepEJpEJ->at(i);
auto& ppAjOJepEJipEJ = ppAjOJepEJpEJ->at(i);
for (size_t j = 0; j < 4; j++)
for (int j = 0; j < 4; j++)
{
ppAijIeJepEJipEJ->at(j) = aAjOIe->dot(ppAjOJepEJipEJ->at(j));
}
}
ppAijIeJepEJpEJ->symLowerWithUpper();
}
FRowDsptr MbD::DirectionCosineIeqcJeqc::pvaluepEJ()
{
return pAijIeJepEJ;
}
FMatDsptr MbD::DirectionCosineIeqcJeqc::ppvaluepEJpEJ()
{
return ppAijIeJepEJpEJ;
}

4
MbDCode/DirectionCosineIeqcJeqc.h
View File

@@ -8,10 +8,12 @@ namespace MbD {
//pAijIeJepEJ ppAijIeJepEIpEJ ppAijIeJepEJpEJ pAjOJepEJT ppAjOJepEJpEJ
public:
DirectionCosineIeqcJeqc();
DirectionCosineIeqcJeqc(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi, size_t axisj);
DirectionCosineIeqcJeqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj);
void initialize();
void initializeGlobally();
void calcPostDynCorrectorIteration() override;
FRowDsptr pvaluepEJ() override;
FMatDsptr ppvaluepEJpEJ() override;
FRowDsptr pAijIeJepEJ;
FMatDsptr ppAijIeJepEIpEJ;

2
MbDCode/DirectionCosineIeqctJeqc.cpp
View File

@@ -7,7 +7,7 @@ DirectionCosineIeqctJeqc::DirectionCosineIeqctJeqc()
{
}
DirectionCosineIeqctJeqc::DirectionCosineIeqctJeqc(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi, size_t axisj) :
DirectionCosineIeqctJeqc::DirectionCosineIeqctJeqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj) :
DirectionCosineIeqcJeqc(frmi, frmj, axisi, axisj)
{
}

2
MbDCode/DirectionCosineIeqctJeqc.h
View File

@@ -8,7 +8,7 @@ namespace MbD {
//pAijIeJept ppAijIeJepEIpt ppAijIeJepEJpt ppAijIeJeptpt
public:
DirectionCosineIeqctJeqc();
DirectionCosineIeqctJeqc(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi, size_t axisj);
DirectionCosineIeqctJeqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj);
void initialize();
void initializeGlobally();
void calcPostDynCorrectorIteration() override;

7
MbDCode/DiscontinuityError.cpp Normal file
View File

@@ -0,0 +1,7 @@
#include "DiscontinuityError.h"
using namespace MbD;
MbD::DiscontinuityError::DiscontinuityError(const std::string& msg) : std::runtime_error(msg)
{
}

15
MbDCode/DiscontinuityError.h Normal file
View File

@@ -0,0 +1,15 @@
#pragma once
#include <stdexcept>
#include <memory>
#include <vector>
namespace MbD {
class DiscontinuityError : virtual public std::runtime_error
{
public:
//DiscontinuityError();
explicit DiscontinuityError(const std::string& msg);
virtual ~DiscontinuityError() noexcept {}
};
}

32
MbDCode/DispCompIecJecKec.cpp
View File

@@ -6,10 +6,40 @@ MbD::DispCompIecJecKec::DispCompIecJecKec()
{
}
MbD::DispCompIecJecKec::DispCompIecJecKec(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, size_t axisk): KinematicIeJe(frmi, frmj), efrmK(frmk), axisK(axisk)
MbD::DispCompIecJecKec::DispCompIecJecKec(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk): KinematicIeJe(frmi, frmj), efrmK(frmk), axisK(axisk)
{
}
FRowDsptr MbD::DispCompIecJecKec::pvaluepXJ()
{
assert(false);
return FRowDsptr();
}
FRowDsptr MbD::DispCompIecJecKec::pvaluepEJ()
{
assert(false);
return FRowDsptr();
}
FMatDsptr MbD::DispCompIecJecKec::ppvaluepXJpEK()
{
assert(false);
return FMatDsptr();
}
FMatDsptr MbD::DispCompIecJecKec::ppvaluepEJpEK()
{
assert(false);
return FMatDsptr();
}
FMatDsptr MbD::DispCompIecJecKec::ppvaluepEJpEJ()
{
assert(false);
return FMatDsptr();
}
double MbD::DispCompIecJecKec::value()
{
return riIeJeKe;

11
MbDCode/DispCompIecJecKec.h
View File

@@ -8,11 +8,16 @@ namespace MbD {
//efrmK axisK riIeJeKe aAjOKe rIeJeO
public:
DispCompIecJecKec();
DispCompIecJecKec(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, size_t axisk);
DispCompIecJecKec(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk);
FRowDsptr pvaluepXJ() override;
FRowDsptr pvaluepEJ() override;
FMatDsptr ppvaluepXJpEK() override;
FMatDsptr ppvaluepEJpEK() override;
FMatDsptr ppvaluepEJpEJ() override;
virtual double value();
EndFrmcptr efrmK;
size_t axisK;
int axisK;
double riIeJeKe;
FColDsptr aAjOKe;
FColDsptr rIeJeO;

6
MbDCode/DispCompIecJecKeqc.cpp
View File

@@ -7,7 +7,7 @@ MbD::DispCompIecJecKeqc::DispCompIecJecKeqc()
{
}
MbD::DispCompIecJecKeqc::DispCompIecJecKeqc(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, size_t axisk) : DispCompIecJecKec(frmi, frmj, frmk, axisk)
MbD::DispCompIecJecKeqc::DispCompIecJecKeqc(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk) : DispCompIecJecKec(frmi, frmj, frmk, axisk)
{
}
@@ -32,13 +32,13 @@ void MbD::DispCompIecJecKeqc::calcPostDynCorrectorIteration()
riIeJeKe = aAjOKe->dot(rIeJeO);
pAjOKepEKT = efrmKqc->pAjOepET(axisK);
ppAjOKepEKpEK = efrmKqc->ppAjOepEpE(axisK);
for (size_t i = 0; i < 4; i++)
for (int i = 0; i < 4; i++)
{
priIeJeKepEK->at(i) = ((pAjOKepEKT->at(i))->dot(rIeJeO));
auto& ppAjOKepEKipEK = ppAjOKepEKpEK->at(i);
auto& ppriIeJeKepEKipEK = ppriIeJeKepEKpEK->at(i);
ppriIeJeKepEKipEK->at(i) = ((ppAjOKepEKipEK->at(i))->dot(rIeJeO));
for (size_t j = i + 1; j < 4; j++)
for (int j = i + 1; j < 4; j++)
{
auto ppriIeJeKepEKipEKj = (ppAjOKepEKipEK->at(i))->dot(rIeJeO);
ppriIeJeKepEKipEK->at(j) = ppriIeJeKepEKipEKj;

2
MbDCode/DispCompIecJecKeqc.h
View File

@@ -8,7 +8,7 @@ namespace MbD {
//priIeJeKepEK ppriIeJeKepEKpEK pAjOKepEKT ppAjOKepEKpEK
public:
DispCompIecJecKeqc();
DispCompIecJecKeqc(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, size_t axisk);
DispCompIecJecKeqc(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk);
void initialize() override;
void initializeGlobally() override;
void calcPostDynCorrectorIteration() override;

32
MbDCode/DispCompIecJecO.cpp
View File

@@ -6,7 +6,7 @@ MbD::DispCompIecJecO::DispCompIecJecO()
{
}
MbD::DispCompIecJecO::DispCompIecJecO(EndFrmcptr frmi, EndFrmcptr frmj, size_t axis) : KinematicIeJe(frmi, frmj), axis(axis)
MbD::DispCompIecJecO::DispCompIecJecO(EndFrmcptr frmi, EndFrmcptr frmj, int axis) : KinematicIeJe(frmi, frmj), axis(axis)
{
}
@@ -14,3 +14,33 @@ void MbD::DispCompIecJecO::calcPostDynCorrectorIteration()
{
riIeJeO = frmJ->riOeO(axis) - frmI->riOeO(axis);
}
FRowDsptr MbD::DispCompIecJecO::pvaluepXJ()
{
assert(false);
return FRowDsptr();
}
FRowDsptr MbD::DispCompIecJecO::pvaluepEJ()
{
assert(false);
return FRowDsptr();
}
FMatDsptr MbD::DispCompIecJecO::ppvaluepXJpEK()
{
assert(false);
return FMatDsptr();
}
FMatDsptr MbD::DispCompIecJecO::ppvaluepEJpEK()
{
assert(false);
return FMatDsptr();
}
FMatDsptr MbD::DispCompIecJecO::ppvaluepEJpEJ()
{
assert(false);
return FMatDsptr();
}

9
MbDCode/DispCompIecJecO.h
View File

@@ -8,10 +8,15 @@ namespace MbD {
//axis riIeJeO
public:
DispCompIecJecO();
DispCompIecJecO(EndFrmcptr frmi, EndFrmcptr frmj, size_t axis);
DispCompIecJecO(EndFrmcptr frmi, EndFrmcptr frmj, int axis);
void calcPostDynCorrectorIteration() override;
FRowDsptr pvaluepXJ() override;
FRowDsptr pvaluepEJ() override;
FMatDsptr ppvaluepXJpEK() override;
FMatDsptr ppvaluepEJpEK() override;
FMatDsptr ppvaluepEJpEJ() override;
size_t axis = -1;
int axis = -1;
double riIeJeO;
};
}

18
MbDCode/DispCompIeqcJecKeqc.cpp
View File

@@ -7,7 +7,7 @@ MbD::DispCompIeqcJecKeqc::DispCompIeqcJecKeqc()
{
}
MbD::DispCompIeqcJecKeqc::DispCompIeqcJecKeqc(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, size_t axisk) : DispCompIecJecKeqc(frmi, frmj, frmk, axisk)
MbD::DispCompIeqcJecKeqc::DispCompIeqcJecKeqc(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk) : DispCompIecJecKeqc(frmi, frmj, frmk, axisk)
{
}
@@ -27,39 +27,39 @@ void MbD::DispCompIeqcJecKeqc::calcPostDynCorrectorIteration()
auto frmIqc = std::static_pointer_cast<EndFrameqc>(frmI);
auto mprIeJeOpEIT = frmIqc->prOeOpE->transpose();
auto mpprIeJeOpEIpEI = frmIqc->pprOeOpEpE;
for (size_t i = 0; i < 3; i++)
for (int i = 0; i < 3; i++)
{
priIeJeKepXI->at(i) = 0.0 - (aAjOKe->at(i));
}
for (size_t i = 0; i < 4; i++)
for (int i = 0; i < 4; i++)
{
priIeJeKepEI->at(i) = 0.0 - (aAjOKe->dot(mprIeJeOpEIT->at(i)));
}
for (size_t i = 0; i < 3; i++)
for (int i = 0; i < 3; i++)
{
auto& ppriIeJeKepXIipEK = ppriIeJeKepXIpEK->at(i);
for (size_t j = 0; j < 4; j++)
for (int j = 0; j < 4; j++)
{
ppriIeJeKepXIipEK->at(j) = 0.0 - (pAjOKepEKT->at(j)->at(i));
}
}
for (size_t i = 0; i < 4; i++)
for (int i = 0; i < 4; i++)
{
auto& mpprIeJeOpEIipEI = mpprIeJeOpEIpEI->at(i);
auto& ppriIeJeKepEIipEI = ppriIeJeKepEIpEI->at(i);
ppriIeJeKepEIipEI->at(i) = 0.0 - (aAjOKe->dot(mpprIeJeOpEIipEI->at(i)));
for (size_t j = 0; j < 4; j++)
for (int j = 0; j < 4; j++)
{
auto ppriIeJeKepEIipEIj = 0.0 - (aAjOKe->dot(mpprIeJeOpEIipEI->at(j)));
ppriIeJeKepEIipEI->at(j) = ppriIeJeKepEIipEIj;
ppriIeJeKepEIpEI->at(j)->at(i) = ppriIeJeKepEIipEIj;
}
}
for (size_t i = 0; i < 4; i++)
for (int i = 0; i < 4; i++)
{
auto& mprIeJeOpEITi = mprIeJeOpEIT->at(i);
auto& ppriIeJeKepEIipEK = ppriIeJeKepEIpEK->at(i);
for (size_t j = 0; j < 4; j++)
for (int j = 0; j < 4; j++)
{
ppriIeJeKepEIipEK->at(j) = 0.0 - (pAjOKepEKT->at(j)->dot(mprIeJeOpEITi));
}

2
MbDCode/DispCompIeqcJecKeqc.h
View File

@@ -8,7 +8,7 @@ namespace MbD {
//priIeJeKepXI priIeJeKepEI ppriIeJeKepXIpEK ppriIeJeKepEIpEI ppriIeJeKepEIpEK
public:
DispCompIeqcJecKeqc();
DispCompIeqcJecKeqc(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, size_t axisk);
DispCompIeqcJecKeqc(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk);
void initialize() override;
void calcPostDynCorrectorIteration() override;

2
MbDCode/DispCompIeqcJecO.cpp
View File

@@ -7,7 +7,7 @@ MbD::DispCompIeqcJecO::DispCompIeqcJecO()
{
}
MbD::DispCompIeqcJecO::DispCompIeqcJecO(EndFrmcptr frmi, EndFrmcptr frmj, size_t axis) : DispCompIecJecO(frmi, frmj, axis)
MbD::DispCompIeqcJecO::DispCompIeqcJecO(EndFrmcptr frmi, EndFrmcptr frmj, int axis) : DispCompIecJecO(frmi, frmj, axis)
{
}

2
MbDCode/DispCompIeqcJecO.h
View File

@@ -8,7 +8,7 @@ namespace MbD {
//priIeJeOpXI priIeJeOpEI ppriIeJeOpEIpEI
public:
DispCompIeqcJecO();
DispCompIeqcJecO(EndFrmcptr frmi, EndFrmcptr frmj, size_t axis);
DispCompIeqcJecO(EndFrmcptr frmi, EndFrmcptr frmj, int axis);
void initializeGlobally() override;
void calcPostDynCorrectorIteration() override;

43
MbDCode/DispCompIeqcJeqcKeqc.cpp
View File

@@ -7,7 +7,7 @@ MbD::DispCompIeqcJeqcKeqc::DispCompIeqcJeqcKeqc()
{
}
MbD::DispCompIeqcJeqcKeqc::DispCompIeqcJeqcKeqc(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, size_t axisk) : DispCompIeqcJecKeqc(frmi, frmj, frmk, axisk)
MbD::DispCompIeqcJeqcKeqc::DispCompIeqcJeqcKeqc(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk) : DispCompIeqcJecKeqc(frmi, frmj, frmk, axisk)
{
}
@@ -27,41 +27,66 @@ void MbD::DispCompIeqcJeqcKeqc::calcPostDynCorrectorIteration()
auto frmJqc = std::static_pointer_cast<EndFrameqc>(frmJ);
auto prIeJeOpEJT = frmJqc->prOeOpE->transpose();
auto pprIeJeOpEJpEJ = frmJqc->pprOeOpEpE;
for (size_t i = 0; i < 3; i++)
for (int i = 0; i < 3; i++)
{
priIeJeKepXJ->at(i) = (aAjOKe->at(i));
}
for (size_t i = 0; i < 4; i++)
for (int i = 0; i < 4; i++)
{
priIeJeKepEJ->at(i) = (aAjOKe->dot(prIeJeOpEJT->at(i)));
}
for (size_t i = 0; i < 3; i++)
for (int i = 0; i < 3; i++)
{
auto& ppriIeJeKepXJipEK = ppriIeJeKepXJpEK->at(i);
for (size_t j = 0; j < 4; j++)
for (int j = 0; j < 4; j++)
{
ppriIeJeKepXJipEK->at(j) = (pAjOKepEKT->at(j)->at(i));
}
}
for (size_t i = 0; i < 4; i++)
for (int i = 0; i < 4; i++)
{
auto& pprIeJeOpEJipEJ = pprIeJeOpEJpEJ->at(i);
auto& ppriIeJeKepEJipEJ = ppriIeJeKepEJpEJ->at(i);
ppriIeJeKepEJipEJ->at(i) = (aAjOKe->dot(pprIeJeOpEJipEJ->at(i)));
for (size_t j = 0; j < 4; j++)
for (int j = 0; j < 4; j++)
{
auto ppriIeJeKepEJipEJj = (aAjOKe->dot(pprIeJeOpEJipEJ->at(j)));
ppriIeJeKepEJipEJ->at(j) = ppriIeJeKepEJipEJj;
ppriIeJeKepEJpEJ->at(j)->at(i) = ppriIeJeKepEJipEJj;
}
}
for (size_t i = 0; i < 4; i++)
for (int i = 0; i < 4; i++)
{
auto& prIeJeOpEJTi = prIeJeOpEJT->at(i);
auto& ppriIeJeKepEJipEK = ppriIeJeKepEJpEK->at(i);
for (size_t j = 0; j < 4; j++)
for (int j = 0; j < 4; j++)
{
ppriIeJeKepEJipEK->at(j) = (pAjOKepEKT->at(j)->dot(prIeJeOpEJTi));
}
}
}
FRowDsptr MbD::DispCompIeqcJeqcKeqc::pvaluepXJ()
{
return priIeJeKepXJ;
}
FRowDsptr MbD::DispCompIeqcJeqcKeqc::pvaluepEJ()
{
return priIeJeKepEJ;
}
FMatDsptr MbD::DispCompIeqcJeqcKeqc::ppvaluepXJpEK()
{
return ppriIeJeKepXJpEK;
}
FMatDsptr MbD::DispCompIeqcJeqcKeqc::ppvaluepEJpEK()
{
return ppriIeJeKepEJpEK;
}
FMatDsptr MbD::DispCompIeqcJeqcKeqc::ppvaluepEJpEJ()
{
return ppriIeJeKepEJpEJ;
}

7
MbDCode/DispCompIeqcJeqcKeqc.h
View File

@@ -8,9 +8,14 @@ namespace MbD {
//priIeJeKepXJ priIeJeKepEJ ppriIeJeKepXJpEK ppriIeJeKepEJpEJ ppriIeJeKepEJpEK
public:
DispCompIeqcJeqcKeqc();
DispCompIeqcJeqcKeqc(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, size_t axisk);
DispCompIeqcJeqcKeqc(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk);
void initialize() override;
void calcPostDynCorrectorIteration() override;
FRowDsptr pvaluepXJ() override;
FRowDsptr pvaluepEJ() override;
FMatDsptr ppvaluepXJpEK();
FMatDsptr ppvaluepEJpEK();
FMatDsptr ppvaluepEJpEJ() override;
FRowDsptr priIeJeKepXJ;
FRowDsptr priIeJeKepEJ;

2
MbDCode/DispCompIeqcJeqcKeqct.cpp
View File

@@ -7,7 +7,7 @@ MbD::DispCompIeqcJeqcKeqct::DispCompIeqcJeqcKeqct()
{
}
MbD::DispCompIeqcJeqcKeqct::DispCompIeqcJeqcKeqct(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, size_t axisk) : DispCompIeqcJeqcKeqc(frmi, frmj, frmk, axisk)
MbD::DispCompIeqcJeqcKeqct::DispCompIeqcJeqcKeqct(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk) : DispCompIeqcJeqcKeqc(frmi, frmj, frmk, axisk)
{
}

2
MbDCode/DispCompIeqcJeqcKeqct.h
View File

@@ -8,7 +8,7 @@ namespace MbD {
//priIeJeKept ppriIeJeKepXIpt ppriIeJeKepEIpt ppriIeJeKepXJpt ppriIeJeKepEJpt ppriIeJeKepEKpt ppriIeJeKeptpt
public:
DispCompIeqcJeqcKeqct();
DispCompIeqcJeqcKeqct(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, size_t axisk);
DispCompIeqcJeqcKeqct(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk);
void initialize() override;
void calcPostDynCorrectorIteration() override;

17
MbDCode/DispCompIeqcJeqcO.cpp
View File

@@ -7,7 +7,7 @@ MbD::DispCompIeqcJeqcO::DispCompIeqcJeqcO()
{
}
MbD::DispCompIeqcJeqcO::DispCompIeqcJeqcO(EndFrmcptr frmi, EndFrmcptr frmj, size_t axis) : DispCompIeqcJecO(frmi, frmj, axis)
MbD::DispCompIeqcJeqcO::DispCompIeqcJeqcO(EndFrmcptr frmi, EndFrmcptr frmj, int axis) : DispCompIeqcJecO(frmi, frmj, axis)
{
}
@@ -24,3 +24,18 @@ void MbD::DispCompIeqcJeqcO::calcPostDynCorrectorIteration()
DispCompIeqcJecO::calcPostDynCorrectorIteration();
priIeJeOpEJ = std::static_pointer_cast<EndFrameqc>(frmJ)->priOeOpE(axis);
}
FRowDsptr MbD::DispCompIeqcJeqcO::pvaluepXJ()
{
return priIeJeOpXJ;
}
FRowDsptr MbD::DispCompIeqcJeqcO::pvaluepEJ()
{
return priIeJeOpEJ;
}
FMatDsptr MbD::DispCompIeqcJeqcO::ppvaluepEJpEJ()
{
return ppriIeJeOpEJpEJ;
}

5
MbDCode/DispCompIeqcJeqcO.h
View File

@@ -8,9 +8,12 @@ namespace MbD {
//priIeJeOpXJ priIeJeOpEJ ppriIeJeOpEJpEJ
public:
DispCompIeqcJeqcO();
DispCompIeqcJeqcO(EndFrmcptr frmi, EndFrmcptr frmj, size_t axis);
DispCompIeqcJeqcO(EndFrmcptr frmi, EndFrmcptr frmj, int axis);
void initializeGlobally() override;
void calcPostDynCorrectorIteration() override;
FRowDsptr pvaluepXJ() override;
FRowDsptr pvaluepEJ() override;
FMatDsptr ppvaluepEJpEJ() override;
FRowDsptr priIeJeOpXJ;
FRowDsptr priIeJeOpEJ;

2
MbDCode/DispCompIeqctJeqcKeqct.cpp
View File

@@ -6,6 +6,6 @@ MbD::DispCompIeqctJeqcKeqct::DispCompIeqctJeqcKeqct()
{
}
MbD::DispCompIeqctJeqcKeqct::DispCompIeqctJeqcKeqct(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, size_t axisk) : DispCompIeqcJeqcKeqct(frmi, frmj, frmk, axisk)
MbD::DispCompIeqctJeqcKeqct::DispCompIeqctJeqcKeqct(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk) : DispCompIeqcJeqcKeqct(frmi, frmj, frmk, axisk)
{
}

2
MbDCode/DispCompIeqctJeqcKeqct.h
View File

@@ -8,7 +8,7 @@ namespace MbD {
//
public:
DispCompIeqctJeqcKeqct();
DispCompIeqctJeqcKeqct(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, size_t axisk);
DispCompIeqctJeqcKeqct(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk);
};
}

2
MbDCode/DispCompIeqctJeqcO.cpp
View File

@@ -7,7 +7,7 @@ MbD::DispCompIeqctJeqcO::DispCompIeqctJeqcO()
{
}
MbD::DispCompIeqctJeqcO::DispCompIeqctJeqcO(EndFrmcptr frmi, EndFrmcptr frmj, size_t axis) : DispCompIeqcJeqcO(frmi, frmj, axis)
MbD::DispCompIeqctJeqcO::DispCompIeqctJeqcO(EndFrmcptr frmi, EndFrmcptr frmj, int axis) : DispCompIeqcJeqcO(frmi, frmj, axis)
{
}

2
MbDCode/DispCompIeqctJeqcO.h
View File

@@ -8,7 +8,7 @@ namespace MbD {
//priIeJeOpt ppriIeJeOpEIpt ppriIeJeOptpt
public:
DispCompIeqctJeqcO();
DispCompIeqctJeqcO(EndFrmcptr frmi, EndFrmcptr frmj, size_t axis);
DispCompIeqctJeqcO(EndFrmcptr frmi, EndFrmcptr frmj, int axis);
void initializeGlobally() override;
void calcPostDynCorrectorIteration() override;

8
MbDCode/EndFramec.cpp
View File

@@ -30,10 +30,6 @@ void EndFramec::initializeLocally()
{
}
void EndFramec::initializeGlobally()
{
}
void EndFramec::initEndFrameqct()
{
assert(false);
@@ -45,12 +41,12 @@ void MbD::EndFramec::calcPostDynCorrectorIteration()
aAOe = markerFrame->aAOm;
}
FColDsptr MbD::EndFramec::aAjOe(size_t j)
FColDsptr MbD::EndFramec::aAjOe(int j)
{
return aAOe->column(j);
}
double MbD::EndFramec::riOeO(size_t i)
double MbD::EndFramec::riOeO(int i)
{
return rOeO->at(i);
}

5
MbDCode/EndFramec.h
View File

@@ -23,11 +23,10 @@ namespace MbD {
void setMarkerFrame(MarkerFrame* markerFrm);
MarkerFrame* getMarkerFrame();
void initializeLocally() override;
void initializeGlobally() override;
virtual void initEndFrameqct();
void calcPostDynCorrectorIteration() override;
FColDsptr aAjOe(size_t j);
double riOeO(size_t i);
FColDsptr aAjOe(int j);
double riOeO(int i);
MarkerFrame* markerFrame; //Use raw pointer when pointing backwards.
FColDsptr rOeO = std::make_shared<FullColumn<double>>(3);

24
MbDCode/EndFrameqc.cpp
View File

@@ -38,13 +38,13 @@ void EndFrameqc::initEndFrameqct()
endFrameqct->setMarkerFrame(markerFrame);
}
FMatFColDsptr MbD::EndFrameqc::ppAjOepEpE(size_t jj)
FMatFColDsptr MbD::EndFrameqc::ppAjOepEpE(int jj)
{
auto answer = std::make_shared<FullMatrix<std::shared_ptr<FullColumn<double>>>>(4, 4);
for (size_t i = 0; i < 4; i++) {
for (int i = 0; i < 4; i++) {
auto& answeri = answer->at(i);
auto& ppAOepEipE = ppAOepEpE->at(i);
for (size_t j = i; j < 4; j++) {
for (int j = i; j < 4; j++) {
answeri->at(j) = ppAOepEipE->at(j)->column(jj);
}
}
@@ -59,13 +59,13 @@ void MbD::EndFrameqc::calcPostDynCorrectorIteration()
pAOepE = markerFrame->pAOmpE;
}
FMatDsptr MbD::EndFrameqc::pAjOepET(size_t axis)
FMatDsptr MbD::EndFrameqc::pAjOepET(int axis)
{
auto answer = std::make_shared<FullMatrix<double>>(4, 3);
for (size_t i = 0; i < 4; i++) {
for (int i = 0; i < 4; i++) {
auto& answeri = answer->at(i);
auto& pAOepEi = pAOepE->at(i);
for (size_t j = 0; j < 3; j++) {
for (int j = 0; j < 3; j++) {
auto& answerij = pAOepEi->at(j)->at(axis);
answeri->at(j) = answerij;
}
@@ -73,13 +73,13 @@ FMatDsptr MbD::EndFrameqc::pAjOepET(size_t axis)
return answer;
}
FMatDsptr MbD::EndFrameqc::ppriOeOpEpE(size_t ii)
FMatDsptr MbD::EndFrameqc::ppriOeOpEpE(int ii)
{
auto answer = std::make_shared<FullMatrix<double>>(4, 4);
for (size_t i = 0; i < 4; i++) {
for (int i = 0; i < 4; i++) {
auto& answeri = answer->at(i);
auto& pprOeOpEipE = pprOeOpEpE->at(i);
for (size_t j = 0; j < 4; j++) {
for (int j = 0; j < 4; j++) {
auto& answerij = pprOeOpEipE->at(j)->at(ii);
answeri->at(j) = answerij;
}
@@ -87,17 +87,17 @@ FMatDsptr MbD::EndFrameqc::ppriOeOpEpE(size_t ii)
return answer;
}
size_t MbD::EndFrameqc::iqX()
int MbD::EndFrameqc::iqX()
{
return markerFrame->iqX();
}
size_t MbD::EndFrameqc::iqE()
int MbD::EndFrameqc::iqE()
{
return markerFrame->iqE();
}
FRowDsptr MbD::EndFrameqc::priOeOpE(size_t i)
FRowDsptr MbD::EndFrameqc::priOeOpE(int i)
{
return prOeOpE->at(i);
}

12
MbDCode/EndFrameqc.h
View File

@@ -15,13 +15,13 @@ namespace MbD {
void initialize();
void initializeGlobally() override;
void initEndFrameqct() override;
FMatFColDsptr ppAjOepEpE(size_t j);
FMatFColDsptr ppAjOepEpE(int j);
void calcPostDynCorrectorIteration() override;
FMatDsptr pAjOepET(size_t j);
FMatDsptr ppriOeOpEpE(size_t i);
size_t iqX();
size_t iqE();
FRowDsptr priOeOpE(size_t i);
FMatDsptr pAjOepET(int j);
FMatDsptr ppriOeOpEpE(int i);
int iqX();
int iqE();
FRowDsptr priOeOpE(int i);
FMatDsptr prOeOpE;
std::shared_ptr<FullMatrix<std::shared_ptr<FullColumn<double>>>> pprOeOpEpE;

18
MbDCode/EndFrameqct.cpp
View File

@@ -60,7 +60,7 @@ void EndFrameqct::initprmemptBlks()
{
auto& mbdTime = System::getInstance().time;
prmemptBlks = std::make_shared< FullColumn<Symsptr>>(3);
for (size_t i = 0; i < 3; i++) {
for (int i = 0; i < 3; i++) {
auto& disp = rmemBlks->at(i);
auto var = disp->differentiateWRT(disp, mbdTime);
auto vel = var->simplified(var);
@@ -76,12 +76,12 @@ void EndFrameqct::initpPhiThePsiptBlks()
{
auto& mbdTime = System::getInstance().time;
pPhiThePsiptBlks = std::make_shared< FullColumn<Symsptr>>(3);
for (size_t i = 0; i < 3; i++) {
for (int i = 0; i < 3; i++) {
auto& angle = phiThePsiBlks->at(i);
auto var = angle->differentiateWRT(angle, mbdTime);
std::cout << "var " << *var << std::endl;
//std::cout << "var " << *var << std::endl;
auto vel = var->simplified(var);
std::cout << "vel " << *vel << std::endl;
//std::cout << "vel " << *vel << std::endl;
pPhiThePsiptBlks->at(i) = vel;
}
}
@@ -104,11 +104,11 @@ void MbD::EndFrameqct::calcPostDynCorrectorIteration()
rOeO = rOmO->plusFullColumn(aAOm->timesFullColumn(rmem));
auto& prOmOpE = markerFrame->prOmOpE;
auto& pAOmpE = markerFrame->pAOmpE;
for (size_t i = 0; i < 3; i++)
for (int i = 0; i < 3; i++)
{
auto& prOmOpEi = prOmOpE->at(i);
auto& prOeOpEi = prOeOpE->at(i);
for (size_t j = 0; j < 4; j++)
for (int j = 0; j < 4; j++)
{
auto prOeOpEij = prOmOpEi->at(j) + pAOmpE->at(j)->at(i)->timesFullColumn(rmem);
prOeOpEi->at(j) = prOeOpEij;
@@ -117,7 +117,7 @@ void MbD::EndFrameqct::calcPostDynCorrectorIteration()
auto rpep = markerFrame->rpmp->plusFullColumn(markerFrame->aApm->timesFullColumn(rmem));
pprOeOpEpE = EulerParameters<double>::ppApEpEtimesColumn(rpep);
aAOe = aAOm->timesFullMatrix(aAme);
for (size_t i = 0; i < 4; i++)
for (int i = 0; i < 4; i++)
{
pAOepE->at(i) = pAOmpE->at(i)->timesFullMatrix(aAme);
}
@@ -136,7 +136,7 @@ void MbD::EndFrameqct::prePosIC()
void MbD::EndFrameqct::evalrmem()
{
if (rmemBlks) {
for (size_t i = 0; i < 3; i++)
for (int i = 0; i < 3; i++)
{
auto& expression = rmemBlks->at(i);
double value = expression->getValue();
@@ -149,7 +149,7 @@ void MbD::EndFrameqct::evalAme()
{
if (phiThePsiBlks) {
auto phiThePsi = CREATE<EulerAngleszxz<double>>::With();
for (size_t i = 0; i < 3; i++)
for (int i = 0; i < 3; i++)
{
auto& expression = phiThePsiBlks->at(i);
auto value = expression->getValue();

4
MbDCode/EulerArray.h
View File

@@ -8,8 +8,8 @@ namespace MbD {
{
//
public:
EulerArray(size_t count) : FullColumn<T>(count) {}
EulerArray(size_t count, const T& value) : FullColumn<T>(count, value) {}
EulerArray(int count) : FullColumn<T>(count) {}
EulerArray(int count, const T& value) : FullColumn<T>(count, value) {}
EulerArray(std::initializer_list<T> list) : FullColumn<T>{ list } {}
virtual void initialize();
};

22
MbDCode/EulerConstraint.cpp
View File

@@ -15,6 +15,7 @@ EulerConstraint::EulerConstraint(const char* str) : Constraint(str)
void EulerConstraint::initialize()
{
Constraint::initialize();
pGpE = std::make_shared<FullRow<double>>(4);
}
@@ -22,7 +23,7 @@ void MbD::EulerConstraint::calcPostDynCorrectorIteration()
{
auto& qE = static_cast<PartFrame*>(owner)->qE;
aG = qE->sumOfSquares() - 1.0;
for (size_t i = 0; i < 4; i++)
for (int i = 0; i < 4; i++)
{
pGpE->at(i) = 2.0 * qE->at(i);
}
@@ -32,3 +33,22 @@ void MbD::EulerConstraint::useEquationNumbers()
{
iqE = static_cast<PartFrame*>(owner)->iqE;
}
void MbD::EulerConstraint::fillPosICError(FColDsptr col)
{
Constraint::fillPosICError(col);
col->atiplusFullVectortimes(iqE, pGpE, lam);
}
void MbD::EulerConstraint::fillPosICJacob(SpMatDsptr mat)
{
//"ppGpEpE is a diag(2,2,2,2)."
mat->atijplusFullRow(iG, iqE, pGpE);
mat->atijplusFullColumn(iqE, iG, pGpE->transpose());
auto twolam = 2.0 * lam;
for (int i = 0; i < 4; i++)
{
auto ii = iqE + i;
(*(mat->at(ii)))[ii] += twolam;
}
}

4
MbDCode/EulerConstraint.h
View File

@@ -15,9 +15,11 @@ namespace MbD {
void initialize();
void calcPostDynCorrectorIteration() override;
void useEquationNumbers() override;
void fillPosICError(FColDsptr col) override;
void fillPosICJacob(SpMatDsptr mat) override;
FRowDsptr pGpE; //partial derivative of G wrt pE
size_t iqE = -1;
int iqE = -1;
};
}

6
MbDCode/EulerParameters.h
View File

@@ -11,8 +11,8 @@ namespace MbD {
//aA aB aC pApE
public:
EulerParameters() : EulerArray<T>(4) {}
EulerParameters(size_t count) : EulerArray<T>(count) {}
EulerParameters(size_t count, const T& value) : EulerArray<T>(count, value) {}
EulerParameters(int count) : EulerArray<T>(count) {}
EulerParameters(int count, const T& value) : EulerArray<T>(count, value) {}
EulerParameters(std::initializer_list<T> list) : EulerArray<T>{ list } {}
static std::shared_ptr<FullMatrix<std::shared_ptr<FullColumn<T>>>> ppApEpEtimesColumn(FColDsptr col);
@@ -122,7 +122,7 @@ namespace MbD {
aB = std::make_shared<FullMatrix<double>>(3, 4);
aC = std::make_shared<FullMatrix<double>>(3, 4);
pApE = std::make_shared<FullColumn<std::shared_ptr<FullMatrix<double>>>>(4);
for (size_t i = 0; i < 4; i++)
for (int i = 0; i < 4; i++)
{
pApE->at(i) = std::make_shared<FullMatrix<double>>(3, 3);
}

4
MbDCode/EulerParametersDot.h
View File

@@ -12,8 +12,8 @@ namespace MbD {
{
//qE aAdot aBdot aCdot pAdotpE
public:
EulerParametersDot(size_t count) : EulerArray<T>(count) {}
EulerParametersDot(size_t count, const T& value) : EulerArray<T>(count, value) {}
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;

73
MbDCode/FullColumn.h
View File

@@ -10,39 +10,34 @@ namespace MbD {
class FullColumn : public FullVector<T>
{
public:
FullColumn(size_t count) : FullVector<T>(count) {}
FullColumn(size_t count, const T& value) : FullVector<T>(count, value) {}
FullColumn(std::vector<T> vec) : FullVector<T>(vec) {}
FullColumn(int count) : FullVector<T>(count) {}
FullColumn(int count, const T& value) : FullVector<T>(count, value) {}
FullColumn(std::vector<T>::iterator begin, std::vector<T>::iterator end) : FullVector<T>(begin, end) {}
FullColumn(std::initializer_list<T> list) : FullVector<T>{ list } {}
std::shared_ptr<FullColumn<T>> plusFullColumn(std::shared_ptr<FullColumn<T>> fullCol);
std::shared_ptr<FullColumn<T>> minusFullColumn(std::shared_ptr<FullColumn<T>> fullCol);
std::shared_ptr<FullColumn<T>> times(double a);
std::shared_ptr<FullColumn<T>> negated();
void atiputFullColumn(size_t i, std::shared_ptr<FullColumn<T>> fullCol);
void equalSelfPlusFullColumnAt(std::shared_ptr<FullColumn<T>> fullCol, size_t i);
void atiminusFullColumn(size_t i, std::shared_ptr<FullColumn<T>> fullCol);
void equalFullColumnAt(std::shared_ptr<FullColumn<T>> fullCol, size_t i);
void atiputFullColumn(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::string toString()
virtual std::ostream& printOn(std::ostream& s) const;
friend std::ostream& operator<<(std::ostream& s, const FullColumn& fullCol)
{
std::stringstream ss;
ss << "FullColumn { ";
for (size_t i = 0; i < this->size() - 1; i++) {
ss << this->at(i) << ", ";
}
ss << this->back() << " }";
return ss.str();
return fullCol.printOn(s);
}
};
using FColDsptr = std::shared_ptr<FullColumn<double>>;
template<typename T>
inline std::shared_ptr<FullColumn<T>> FullColumn<T>::plusFullColumn(std::shared_ptr<FullColumn<T>> fullCol)
{
size_t n = this->size();
int n = (int) this->size();
auto answer = std::make_shared<FullColumn<T>>(n);
for (size_t i = 0; i < n; i++) {
for (int i = 0; i < n; i++) {
answer->at(i) = this->at(i) + fullCol->at(i);
}
return answer;
@@ -50,9 +45,9 @@ namespace MbD {
template<typename T>
inline std::shared_ptr<FullColumn<T>> FullColumn<T>::minusFullColumn(std::shared_ptr<FullColumn<T>> fullCol)
{
size_t n = this->size();
int n = (int) this->size();
auto answer = std::make_shared<FullColumn<T>>(n);
for (size_t i = 0; i < n; i++) {
for (int i = 0; i < n; i++) {
answer->at(i) = this->at(i) - fullCol->at(i);
}
return answer;
@@ -60,9 +55,9 @@ namespace MbD {
template<typename T>
inline std::shared_ptr<FullColumn<T>> FullColumn<T>::times(double a)
{
size_t n = this->size();
int n = (int) this->size();
auto answer = std::make_shared<FullColumn>(n);
for (size_t i = 0; i < n; i++) {
for (int i = 0; i < n; i++) {
answer->at(i) = this->at(i) * a;
}
return answer;
@@ -73,35 +68,35 @@ namespace MbD {
return this->times(-1.0);
}
template<typename T>
inline void FullColumn<T>::atiputFullColumn(size_t i, std::shared_ptr<FullColumn<T>> fullCol)
inline void FullColumn<T>::atiputFullColumn(int i, std::shared_ptr<FullColumn<T>> fullCol)
{
for (size_t ii = 0; ii < fullCol->size(); ii++)
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, size_t ii)
inline void FullColumn<T>::equalSelfPlusFullColumnAt(std::shared_ptr<FullColumn<T>> fullCol, int ii)
{
//self is subcolumn of fullCol
for (size_t i = 0; i < this->size(); i++)
for (int i = 0; i < this->size(); i++)
{
this->at(i) += fullCol->at(ii + i);
}
}
template<typename T>
inline void FullColumn<T>::atiminusFullColumn(size_t i1, std::shared_ptr<FullColumn<T>> fullCol)
inline void FullColumn<T>::atiminusFullColumn(int i1, std::shared_ptr<FullColumn<T>> fullCol)
{
for (size_t ii = 0; ii < fullCol->size(); ii++)
for (int ii = 0; ii < fullCol->size(); ii++)
{
size_t i = i1 + ii;
int i = i1 + ii;
this->at(i) -= fullCol->at(ii);
}
}
template<typename T>
inline void FullColumn<T>::equalFullColumnAt(std::shared_ptr<FullColumn<T>> fullCol, size_t ii)
inline void FullColumn<T>::equalFullColumnAt(std::shared_ptr<FullColumn<T>> fullCol, int ii)
{
for (size_t i = 0; i < this->size(); i++)
for (int i = 0; i < this->size(); i++)
{
this->at(i) = fullCol->at(ii + i);
}
@@ -109,13 +104,25 @@ namespace MbD {
template<>
inline std::shared_ptr<FullColumn<double>> FullColumn<double>::copy()
{
auto n = this->size();
auto n = (int) this->size();
auto answer = std::make_shared<FullColumn<double>>(n);
for (size_t i = 0; i < n; i++)
for (int i = 0; i < n; i++)
{
answer->at(i) = this->at(i);
}
return answer;
}
template<typename T>
inline std::ostream& FullColumn<T>::printOn(std::ostream& s) const
{
s << "{";
s << this->at(0);
for (int i = 1; i < this->size(); i++)
{
s << ", " << this->at(i);
}
s << "}";
return s;
}
}

69
MbDCode/FullMatrix.h
View File

@@ -14,11 +14,11 @@ namespace MbD {
{
public:
FullMatrix() {}
FullMatrix(size_t m) : RowTypeMatrix<std::shared_ptr<FullRow<T>>>(m)
FullMatrix(int m) : RowTypeMatrix<std::shared_ptr<FullRow<T>>>(m)
{
}
FullMatrix(size_t m, size_t n) {
for (size_t i = 0; i < m; i++) {
FullMatrix(int m, int n) {
for (int i = 0; i < m; i++) {
auto row = std::make_shared<FullRow<T>>(n);
this->push_back(row);
}
@@ -37,7 +37,7 @@ namespace MbD {
}
}
void identity();
std::shared_ptr<FullColumn<T>> column(size_t j);
std::shared_ptr<FullColumn<T>> column(int j);
std::shared_ptr<FullColumn<T>> timesFullColumn(std::shared_ptr<FullColumn<T>> fullCol);
std::shared_ptr<FullMatrix<T>> timesFullMatrix(std::shared_ptr<FullMatrix<T>> fullMat);
std::shared_ptr<FullMatrix<T>> timesTransposeFullMatrix(std::shared_ptr<FullMatrix<T>> fullMat);
@@ -46,23 +46,22 @@ namespace MbD {
std::shared_ptr<FullMatrix<T>> transpose();
std::shared_ptr<FullMatrix<T>> negated();
void symLowerWithUpper();
void atijputFullColumn(size_t i, size_t j, std::shared_ptr<FullColumn<T>> fullCol);
void atijputFullColumn(int i, int j, std::shared_ptr<FullColumn<T>> fullCol);
double sumOfSquares() override;
void atijplusNumber(size_t i, size_t j, double value);
void zeroSelf() override;
};
template <>
inline void FullMatrix<double>::identity() {
this->zeroSelf();
for (size_t i = 0; i < this->size(); i++) {
for (int i = 0; i < this->size(); i++) {
this->at(i)->at(i) = 1.0;
}
}
template<typename T>
inline std::shared_ptr<FullColumn<T>> FullMatrix<T>::column(size_t j) {
size_t n = this->size();
inline std::shared_ptr<FullColumn<T>> FullMatrix<T>::column(int j) {
int n = (int) this->size();
auto answer = std::make_shared<FullColumn<T>>(n);
for (size_t i = 0; i < n; i++) {
for (int i = 0; i < n; i++) {
answer->at(i) = this->at(i)->at(j);
}
return answer;
@@ -71,9 +70,9 @@ 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."
size_t n = this->size();
int n = (int) this->size();
auto answer = std::make_shared<FullColumn<T>>(n);
for (size_t i = 0; i < n; i++)
for (int i = 0; i < n; i++)
{
answer->at(i) = this->at(i)->timesFullColumn(fullCol);
}
@@ -82,9 +81,9 @@ namespace MbD {
template<typename T>
inline std::shared_ptr<FullMatrix<T>> FullMatrix<T>::timesFullMatrix(std::shared_ptr<FullMatrix<T>> fullMat)
{
size_t m = this->nrow();
int m = this->nrow();
auto answer = std::make_shared<FullMatrix<T>>(m);
for (size_t i = 0; i < m; i++) {
for (int i = 0; i < m; i++) {
answer->at(i) = this->at(i)->timesFullMatrix(fullMat);
}
return answer;
@@ -92,9 +91,9 @@ namespace MbD {
template<typename T>
inline std::shared_ptr<FullMatrix<T>> FullMatrix<T>::timesTransposeFullMatrix(std::shared_ptr<FullMatrix<T>> fullMat)
{
size_t nrow = this->nrow();
int nrow = this->nrow();
auto answer = std::make_shared<FullMatrix<T>>(nrow);
for (size_t i = 0; i < nrow; i++) {
for (int i = 0; i < nrow; i++) {
answer->at(i) = this->at(i)->timesTransposeFullMatrix(fullMat);
}
return answer;
@@ -102,9 +101,9 @@ namespace MbD {
template<typename T>
inline std::shared_ptr<FullMatrix<T>> FullMatrix<T>::times(double a)
{
size_t m = this->nrow();
int m = this->nrow();
auto answer = std::make_shared<FullMatrix<T>>(m);
for (size_t i = 0; i < m; i++) {
for (int i = 0; i < m; i++) {
answer->at(i) = this->at(i)->times(a);
}
return answer;
@@ -112,9 +111,9 @@ namespace MbD {
template<typename T>
inline std::shared_ptr<FullMatrix<T>> FullMatrix<T>::plusFullMatrix(std::shared_ptr<FullMatrix<T>> fullMat)
{
size_t n = this->size();
int n = (int) this->size();
auto answer = std::make_shared<FullMatrix<T>>(n);
for (size_t i = 0; i < n; i++) {
for (int i = 0; i < n; i++) {
answer->at(i) = this->at(i)->plusFullRow(fullMat->at(i));
}
return answer;
@@ -122,12 +121,12 @@ namespace MbD {
template<typename T>
inline std::shared_ptr<FullMatrix<T>> FullMatrix<T>::transpose()
{
size_t nrow = this->nrow();
int nrow = this->nrow();
auto ncol = this->ncol();
auto answer = std::make_shared<FullMatrix<T>>(ncol, nrow);
for (size_t i = 0; i < nrow; i++) {
for (int i = 0; i < nrow; i++) {
auto& row = this->at(i);
for (size_t j = 0; j < ncol; j++) {
for (int j = 0; j < ncol; j++) {
answer->at(j)->at(i) = row->at(j);
}
}
@@ -141,18 +140,18 @@ namespace MbD {
template<typename T>
inline void FullMatrix<T>::symLowerWithUpper()
{
size_t n = this->size();
for (size_t i = 0; i < n; i++) {
for (size_t j = i + 1; j < n; j++) {
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);
}
}
}
template<typename T>
inline void FullMatrix<T>::atijputFullColumn(size_t i1, size_t j1, std::shared_ptr<FullColumn<T>> fullCol)
inline void FullMatrix<T>::atijputFullColumn(int i1, int j1, std::shared_ptr<FullColumn<T>> fullCol)
{
auto iOffset = i1 - 1;
for (size_t ii = 0; ii < fullCol->size(); ii++)
for (int ii = 0; ii < fullCol->size(); ii++)
{
this->at(iOffset + ii)->at(j1) = fullCol->at(ii);
}
@@ -161,7 +160,7 @@ namespace MbD {
inline double FullMatrix<double>::sumOfSquares()
{
double sum = 0.0;
for (size_t i = 0; i < this->size(); i++)
for (int i = 0; i < this->size(); i++)
{
sum += this->at(i)->sumOfSquares();
}
@@ -174,19 +173,9 @@ namespace MbD {
return 0.0;
}
template<>
inline void FullMatrix<double>::atijplusNumber(size_t i, size_t j, double value)
{
this->at(i)->atiplusNumber(j, value);
}
template<typename T>
inline void FullMatrix<T>::atijplusNumber(size_t i, size_t j, double value)
{
assert(false);
}
template<>
inline void FullMatrix<double>::zeroSelf()
{
for (size_t i = 0; i < this->size(); i++) {
for (int i = 0; i < this->size(); i++) {
this->at(i)->zeroSelf();
}
}

32
MbDCode/FullRow.h
View File

@@ -11,8 +11,8 @@ namespace MbD {
{
public:
FullRow() {}
FullRow(size_t count) : FullVector<T>(count) {}
FullRow(size_t count, const T& value) : FullVector<T>(count, value) {}
FullRow(int count) : FullVector<T>(count) {}
FullRow(int count, const T& value) : FullVector<T>(count, value) {}
FullRow(std::initializer_list<T> list) : FullVector<T>{ list } {}
std::shared_ptr<FullRow<T>> times(double a);
std::shared_ptr<FullRow<T>> negated();
@@ -22,14 +22,15 @@ namespace MbD {
std::shared_ptr<FullRow<T>> timesFullMatrix(std::shared_ptr<FullMatrix<T>> fullMat);
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();
};
template<typename T>
inline std::shared_ptr<FullRow<T>> FullRow<T>::times(double a)
{
size_t n = this->size();
int n = (int) this->size();
auto answer = std::make_shared<FullRow>(n);
for (size_t i = 0; i < n; i++) {
for (int i = 0; i < n; i++) {
answer->at(i) = this->at(i) * a;
}
return answer;
@@ -42,9 +43,9 @@ namespace MbD {
template<typename T>
inline std::shared_ptr<FullRow<T>> FullRow<T>::plusFullRow(std::shared_ptr<FullRow<T>> fullRow)
{
size_t n = this->size();
int n = (int) this->size();
auto answer = std::make_shared<FullRow<T>>(n);
for (size_t i = 0; i < n; i++) {
for (int i = 0; i < n; i++) {
answer->at(i) = this->at(i) + fullRow->at(i);
}
return answer;
@@ -52,9 +53,9 @@ namespace MbD {
template<typename T>
inline std::shared_ptr<FullRow<T>> FullRow<T>::minusFullRow(std::shared_ptr<FullRow<T>> fullRow)
{
size_t n = this->size();
int n = (int) this->size();
auto answer = std::make_shared<FullRow<T>>(n);
for (size_t i = 0; i < n; i++) {
for (int i = 0; i < n; i++) {
answer->at(i) = this->at(i) - fullRow->at(i);
}
return answer;
@@ -63,7 +64,7 @@ namespace MbD {
inline T FullRow<T>::timesFullColumn(std::shared_ptr<FullColumn<T>> fullCol)
{
auto answer = this->at(0) * fullCol->at(0);
for (size_t i = 1; i < this->size(); i++)
for (int i = 1; i < this->size(); i++)
{
answer += this->at(i) * fullCol->at(i);
}
@@ -73,9 +74,9 @@ namespace MbD {
inline std::shared_ptr<FullRow<T>> FullRow<T>::timesTransposeFullMatrix(std::shared_ptr<FullMatrix<T>> fullMat)
{
//"a*bT = a(1,j)b(k,j)"
size_t ncol = fullMat->nrow();
int ncol = fullMat->nrow();
auto answer = std::make_shared<FullRow<T>>(ncol);
for (size_t k = 0; k < ncol; k++) {
for (int k = 0; k < ncol; k++) {
answer->at(k) = this->dot(fullMat->at(k));
}
return answer;
@@ -83,16 +84,21 @@ namespace MbD {
template<typename T>
inline void FullRow<T>::equalSelfPlusFullRowTimes(std::shared_ptr<FullRow<T>> fullRow, double factor)
{
for (size_t i = 0; i < this->size(); i++)
for (int i = 0; i < this->size(); i++)
{
this->at(i) += fullRow->at(i) * factor;
}
}
template<typename T>
inline std::shared_ptr<FullColumn<T>> FullRow<T>::transpose()
{
return std::make_shared<FullColumn<T>>(*this);
}
template<typename T>
inline std::shared_ptr<FullRow<T>> FullRow<T>::timesFullMatrix(std::shared_ptr<FullMatrix<T>> fullMat)
{
std::shared_ptr<FullRow<T>> answer = fullMat->at(0)->times(this->at(0));
for (size_t j = 1; j < this->size(); j++)
for (int j = 1; j < (int) this->size(); j++)
{
answer->equalSelfPlusFullRowTimes(fullMat->at(j), this->at(j));
}

39
MbDCode/FullVector.h
View File

@@ -7,28 +7,32 @@ namespace MbD {
{
public:
FullVector() {}
FullVector(size_t count) : Array<T>(count) {}
FullVector(size_t count, const T& value) : Array<T>(count, value) {}
FullVector(std::vector<T> vec) : Array<T>(vec) {}
FullVector(int count) : Array<T>(count) {}
FullVector(int count, const T& value) : Array<T>(count, value) {}
FullVector(std::vector<T>::iterator begin, std::vector<T>::iterator end) : Array<T>(begin, end) {}
FullVector(std::initializer_list<T> list) : Array<T>{ list } {}
double dot(std::shared_ptr<FullVector<T>> vec);
void atiplusNumber(size_t i, T value);
void atiplusNumber(int i, T value);
double sumOfSquares() override;
size_t numberOfElements() override;
int numberOfElements() override;
void zeroSelf() override;
void atitimes(size_t i, double factor);
void atitimes(int i, double factor);
void atiplusFullVectortimes(int i, std::shared_ptr<FullVector> fullVec, double factor);
};
template<typename T>
inline double FullVector<T>::dot(std::shared_ptr<FullVector<T>> vec)
{
size_t n = this->size();
int n = (int) this->size();
double answer = 0.0;
for (size_t i = 0; i < n; i++) {
for (int i = 0; i < n; i++) {
answer += this->at(i) * vec->at(i);
}
return answer;
}
template<typename T>
inline void FullVector<T>::atiplusNumber(size_t i, T value)
inline void FullVector<T>::atiplusNumber(int i, T value)
{
this->at(i) += value;
}
@@ -36,7 +40,7 @@ namespace MbD {
inline double FullVector<double>::sumOfSquares()
{
double sum = 0.0;
for (size_t i = 0; i < this->size(); i++)
for (int i = 0; i < this->size(); i++)
{
double element = this->at(i);
sum += element * element;
@@ -50,14 +54,14 @@ namespace MbD {
return 0.0;
}
template<typename T>
inline size_t FullVector<T>::numberOfElements()
inline int FullVector<T>::numberOfElements()
{
return this->size();
return (int) this->size();
}
template<>
inline void FullVector<double>::zeroSelf()
{
for (size_t i = 0; i < this->size(); i++) {
for (int i = 0; i < this->size(); i++) {
this->at(i) = 0.0;;
}
}
@@ -67,8 +71,17 @@ namespace MbD {
assert(false);
}
template<typename T>
inline void FullVector<T>::atitimes(size_t i, double factor)
inline void FullVector<T>::atitimes(int i, double factor)
{
this->at(i) *= factor;
}
template<typename T>
inline void FullVector<T>::atiplusFullVectortimes(int i1, std::shared_ptr<FullVector> fullVec, double factor)
{
for (int ii = 0; ii < fullVec->size(); ii++)
{
auto i = i1 + ii;
this->at(i) += fullVec->at(ii) * factor;
}
}
}

2
MbDCode/FunctionWithManyArgs.cpp
View File

@@ -25,7 +25,7 @@ FunctionWithManyArgs::FunctionWithManyArgs(Symsptr term, Symsptr term1, Symsptr
FunctionWithManyArgs::FunctionWithManyArgs(std::shared_ptr<std::vector<Symsptr>> _terms) {
terms = std::make_shared<std::vector<Symsptr>>();
for (size_t i = 0; i < _terms->size(); i++)
for (int i = 0; i < _terms->size(); i++)
terms->push_back(_terms->at(i));
}

2
MbDCode/GEFullMat.cpp
View File

@@ -4,7 +4,7 @@
using namespace MbD;
void MbD::GEFullMat::forwardEliminateWithPivot(size_t p)
void MbD::GEFullMat::forwardEliminateWithPivot(int p)
{
assert(false);
}

2
MbDCode/GEFullMat.h
View File

@@ -8,7 +8,7 @@ namespace MbD {
//
public:
std::shared_ptr<FullMatrix<double>> matrixA;
void forwardEliminateWithPivot(size_t p) override;
void forwardEliminateWithPivot(int p) override;
void backSubstituteIntoDU() override;
void postSolve() override;
void preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) override;

2
MbDCode/GEFullMatFullPv.cpp
View File

@@ -4,7 +4,7 @@
using namespace MbD;
void MbD::GEFullMatFullPv::doPivoting(size_t p)
void MbD::GEFullMatFullPv::doPivoting(int p)
{
assert(false);
}

2
MbDCode/GEFullMatFullPv.h
View File

@@ -7,7 +7,7 @@ namespace MbD {
{
//
public:
void doPivoting(size_t p) override;
void doPivoting(int p) override;
void postSolve() override;
};
}

2
MbDCode/GEFullMatParPv.cpp
View File

@@ -4,7 +4,7 @@
using namespace MbD;
void MbD::GEFullMatParPv::doPivoting(size_t p)
void MbD::GEFullMatParPv::doPivoting(int p)
{
assert(false);
}

2
MbDCode/GEFullMatParPv.h
View File

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

2
MbDCode/GESpMat3.cpp → MbDCode/GESpMat.cpp
View File

@@ -1,4 +1,4 @@
#include "GESpMat3.h"
#include "GESpMat.h"
#include "FullColumn.h"
#include "SparseMatrix.h"

4
MbDCode/GESpMat3.h → MbDCode/GESpMat.h
View File

@@ -11,8 +11,8 @@ namespace MbD {
FColDsptr solvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal);
std::shared_ptr<SparseMatrix<double>> matrixA;
size_t markowitzPivotRowCount, markowitzPivotColCount;
std::shared_ptr<std::vector<size_t>> privateIndicesOfNonZerosInPivotRow, rowPositionsOfNonZerosInPivotColumn;
int markowitzPivotRowCount, markowitzPivotColCount;
std::shared_ptr<std::vector<int>> privateIndicesOfNonZerosInPivotRow, rowPositionsOfNonZerosInPivotColumn;
};
}

105
MbDCode/GESpMatFullPv.cpp
View File

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

10
MbDCode/GESpMatFullPv.h
View File

@@ -1,20 +1,20 @@
#pragma once
#include "GESpMat3.h"
#include "GESpMat.h"
namespace MbD {
class GESpMatFullPv : public GESpMat
{
//positionsOfOriginalCols rowPositionsOfNonZerosInColumns
public:
void doPivoting(size_t p) override;
void forwardEliminateWithPivot(size_t p) override;
void doPivoting(int p) override;
void forwardEliminateWithPivot(int p) override;
void backSubstituteIntoDU() override;
void postSolve() override;
void preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) override;
std::shared_ptr<std::vector<size_t>> positionsOfOriginalCols;
std::shared_ptr<std::vector<std::shared_ptr<std::vector<size_t>>>> rowPositionsOfNonZerosInColumns;
std::shared_ptr<std::vector<int>> positionsOfOriginalCols;
std::shared_ptr<std::vector<std::shared_ptr<std::vector<int>>>> rowPositionsOfNonZerosInColumns;
};
}

68
MbDCode/GESpMatFullPvPosIC.cpp
View File

@@ -2,8 +2,7 @@
#include <cassert>
#include "GESpMatFullPvPosIC.h"
//#include "FullColumn.h"
//#include "SparseMatrix.h"
#include "SingularMatrixError.h"
#include "PosICNewtonRaphson.h"
using namespace MbD;
@@ -12,7 +11,7 @@ void MbD::GESpMatFullPvPosIC::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsp
{
GESpMatFullPv::preSolvewithsaveOriginal(spMat, fullCol, saveOriginal);
if (system == nullptr) {
pivotRowLimits = std::make_shared<std::vector<size_t>>();
pivotRowLimits = std::make_shared<std::vector<int>>();
}
else {
pivotRowLimits = system->pivotRowLimits;
@@ -20,27 +19,76 @@ void MbD::GESpMatFullPvPosIC::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsp
pivotRowLimit = -1;
}
void MbD::GESpMatFullPvPosIC::doPivoting(size_t p)
void MbD::GESpMatFullPvPosIC::doPivoting(int p)
{
//"Used by Gauss Elimination only."
//"Swap rows but keep columns in place."
//"The elements below the diagonal are removed column by column."
assert(false);
auto max = 0.0;
auto pivotRow = p;
auto pivotCol = p;
for (size_t j = p; j < n; j++)
for (int j = p; j < n; j++)
{
rowPositionsOfNonZerosInColumns->at(colOrder->at(j))->clear();
}
if (p > pivotRowLimit) {
if (p == pivotRowLimit) {
pivotRowLimit = *std::find_if(
pivotRowLimits->begin(), pivotRowLimits->end(),
[&](size_t limit) { return limit >= p; });
[&](int limit) { return limit > p; });
}
for (size_t i = p; i < pivotRowLimit; i++)
for (int i = p; i < pivotRowLimit; 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->swapRows(p, pivotRow);
rightHandSideB->swapRows(p, pivotRow);
rowOrder->swapRows(p, pivotRow);
}
if (p != pivotCol) {
colOrder->swapRows(p, pivotCol);
positionsOfOriginalCols->at(colOrder->at(p)) = p;
positionsOfOriginalCols->at(colOrder->at(pivotCol)) = pivotCol;
}
pivotValues->at(p) = max;
if (max < singularPivotTolerance) {
auto itr = std::find_if(
pivotRowLimits->begin(), pivotRowLimits->end(),
[&](int limit) { return limit > pivotRowLimit; });
if (itr == pivotRowLimits->end()) {
auto begin = rowOrder->begin() + p;
auto end = rowOrder->begin() + pivotRowLimit;
auto redundantEqnNos = std::make_shared<FullColumn<int>>(begin, end);
throw SingularMatrixError("", redundantEqnNos);
}
else {
pivotRowLimit = *itr;
}
return this->doPivoting(p);
}
auto jp = colOrder->at(p);
rowPositionsOfNonZerosInPivotColumn = rowPositionsOfNonZerosInColumns->at(jp);
for (int i = pivotRowLimit; i < m; i++)
{
auto& spRowi = matrixA->at(i);
if (spRowi->find(jp) != spRowi->end()) {
rowPositionsOfNonZerosInPivotColumn->push_back(i);
}
}
if (rowPositionsOfNonZerosInPivotColumn->front() == p) {
rowPositionsOfNonZerosInPivotColumn->erase(rowPositionsOfNonZerosInPivotColumn->begin());
}
markowitzPivotColCount = (int) rowPositionsOfNonZerosInPivotColumn->size();
}

6
MbDCode/GESpMatFullPvPosIC.h
View File

@@ -10,11 +10,11 @@ namespace MbD {
//system pivotRowLimits pivotRowLimit
public:
void preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) override;
void doPivoting(size_t p) override;
void doPivoting(int p) override;
PosICNewtonRaphson* system; //Use raw pointer when pointing backwards.
std::shared_ptr<std::vector<size_t>> pivotRowLimits;
size_t pivotRowLimit;
std::shared_ptr<std::vector<int>> pivotRowLimits;
int pivotRowLimit;
};
}

57
MbDCode/GESpMatParPv.cpp
View File

@@ -4,17 +4,66 @@
using namespace MbD;
void MbD::GESpMatParPv::forwardEliminateWithPivot(size_t p)
void MbD::GESpMatParPv::forwardEliminateWithPivot(int p)
{
assert(false);
//"rightHandSideB may be multidimensional."
auto& rowp = matrixA->at(p);
auto app = rowp->at(p);
auto elementsInPivotRow = std::make_shared<std::vector<const std::pair<const int, double>*>>(rowp->size() - 1);
int index = 0;
for (auto const& keyValue : *rowp) {
if (keyValue.first != p) {
elementsInPivotRow->at(index) = (&keyValue);
index++;
}
}
auto bp = rightHandSideB->at(p);
for (int ii = 0; ii < markowitzPivotColCount; ii++)
{
auto i = rowPositionsOfNonZerosInPivotColumn->at(ii);
auto& rowi = matrixA->at(i);
auto aip = rowi->at(p);
rowi->erase(p);
auto factor = aip / app;
for (auto keyValue : *elementsInPivotRow)
{
auto j = keyValue->first;
auto apj = keyValue->second;
(*rowi)[j] -= factor * apj;
}
rightHandSideB->at(i) -= bp * factor;
}
}
void MbD::GESpMatParPv::backSubstituteIntoDU()
{
assert(false);
//"DU is upper triangular with nonzero diagonals."
double sum, duij, duii;
//answerX = rightHandSideB->copyEmpty();
assert(m == n);
answerX = std::make_shared<FullColumn<double>>(m);
answerX->at(n - 1) = rightHandSideB->at(m - 1) / matrixA->at(m - 1)->at(n - 1);
//auto rhsZeroElement = this->rhsZeroElement();
for (int i = n - 2; i >= 0; i--)
{
auto rowi = matrixA->at(i);
sum = 0.0; // rhsZeroElement copy.
for (auto const& keyValue : *rowi) {
auto j = keyValue.first;
if (j > i) {
duij = keyValue.second;
sum += answerX->at(j) * duij;
}
else {
duii = keyValue.second;
}
}
answerX->at(i) = rightHandSideB->at(i) - (sum / duii);
}
}
void MbD::GESpMatParPv::postSolve()
{
assert(false);
}

4
MbDCode/GESpMatParPv.h
View File

@@ -1,13 +1,13 @@
#pragma once
#include "GESpMat3.h"
#include "GESpMat.h"
namespace MbD {
class GESpMatParPv : public GESpMat
{
//
public:
void forwardEliminateWithPivot(size_t p) override;
void forwardEliminateWithPivot(int p) override;
void backSubstituteIntoDU() override;
void postSolve() override;

2
MbDCode/GESpMatParPvMarko.cpp
View File

@@ -4,7 +4,7 @@
using namespace MbD;
void MbD::GESpMatParPvMarko::doPivoting(size_t p)
void MbD::GESpMatParPvMarko::doPivoting(int p)
{
assert(false);
}

2
MbDCode/GESpMatParPvMarko.h
View File

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

67
MbDCode/GESpMatParPvMarkoFast.cpp
View File

@@ -14,8 +14,8 @@ 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<size_t>>();
rowPositionsOfNonZerosInPivotColumn = std::make_shared<std::vector<size_t>>();
privateIndicesOfNonZerosInPivotRow = std::make_shared<std::vector<int>>();
rowPositionsOfNonZerosInPivotColumn = std::make_shared<std::vector<int>>();
}
if (saveOriginal) {
rightHandSideB = fullCol->copy();
@@ -23,7 +23,7 @@ void MbD::GESpMatParPvMarkoFast::preSolvewithsaveOriginal(SpMatDsptr spMat, FCol
else {
rightHandSideB = fullCol;
}
for (size_t i = 0; i < m; i++)
for (int i = 0; i < m; i++)
{
auto& spRowi = spMat->at(i);
auto maxRowElement = spRowi->maxElement();
@@ -36,7 +36,64 @@ void MbD::GESpMatParPvMarkoFast::preSolvewithsaveOriginal(SpMatDsptr spMat, FCol
}
}
void MbD::GESpMatParPvMarkoFast::doPivoting(size_t p)
void MbD::GESpMatParPvMarkoFast::doPivoting(int p)
{
assert(false);
//"Search from bottom to top."
//"Optimized for speed. No check for singular pivot."
//"Do partial pivoting."
//"criterion := mag / (2.0d raisedTo: rowiCount)."
//"Rows are explicitly scaled in preSolve:."
//"Pivot size are nieither checked nor stored."
//| lookForFirstNonZeroInPivotCol i rowi aip criterionMax rowPivoti criterion max |
int i, rowPivoti;
double aip, 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 {
aip = spRowi->at(p);
markowitzPivotColCount = 0;
if (aip < 0) aip = -aip;
max = aip;
criterionMax = aip / 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++;
if (aip < 0) aip = -aip;
criterion = aip / std::pow(2.0, spRowi->size());
if (criterion > criterionMax) {
max = aip;
criterionMax = criterion;
rowPositionsOfNonZerosInPivotColumn->push_back(rowPivoti);
rowPivoti = i;
}
else {
rowPositionsOfNonZerosInPivotColumn->push_back(i);
}
}
i--;
}
if (p != rowPivoti) {
matrixA->swapRows(p, rowPivoti);
rightHandSideB->swapRows(p, rowPivoti);
if (aip != std::numeric_limits<double>::min()) rowPositionsOfNonZerosInPivotColumn->at(markowitzPivotColCount - 1) = rowPivoti;
}
if (max < singularPivotTolerance) throw SingularMatrixError("");
}

2
MbDCode/GESpMatParPvMarkoFast.h
View File

@@ -8,7 +8,7 @@ namespace MbD {
//
public:
void preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) override;
void doPivoting(size_t p) override;
void doPivoting(int p) override;
};
}

2
MbDCode/GESpMatParPvPrecise.cpp
View File

@@ -4,7 +4,7 @@
using namespace MbD;
void MbD::GESpMatParPvPrecise::doPivoting(size_t p)
void MbD::GESpMatParPvPrecise::doPivoting(int p)
{
assert(false);
}

2
MbDCode/GESpMatParPvPrecise.h
View File

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

1
MbDCode/ICKineIntegrator.cpp Normal file
View File

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

12
MbDCode/ICKineIntegrator.h Normal file
View File

@@ -0,0 +1,12 @@
#pragma once
#include "QuasiIntegrator.h"
namespace MbD {
class ICKineIntegrator : public QuasiIntegrator
{
//
public:
};
}

7
MbDCode/Integrator.h
View File

@@ -1,13 +1,16 @@
#pragma once
#include <memory>
#include "Solver.h"
namespace MbD {
class SystemSolver;
class Integrator : public Solver
{
//
//system direction
public:
double direction = 1;
};
}

43
MbDCode/IntegratorInterface.cpp
View File

@@ -1,3 +1,46 @@
#include <string>
#include "IntegratorInterface.h"
#include "SystemSolver.h"
#include "BasicQuasiIntegrator.h"
using namespace MbD;
void MbD::IntegratorInterface::initializeGlobally()
{
tstart = system->startTime();
hout = system->outputStepSize();
hmax = system->maxStepSize();
hmin = system->minStepSize();
tout = system->firstOutputTime();
tend = system->endTime();
direction = (tstart < tend) ? 1.0 : -1.0;
}
void MbD::IntegratorInterface::setSystem(SystemSolver* sys)
{
system = sys;
}
void MbD::IntegratorInterface::logString(std::string& str)
{
system->logString(str);
}
void MbD::IntegratorInterface::run()
{
this->preRun();
this->initializeLocally();
this->initializeGlobally();
if (hout > (4 * std::numeric_limits<double>::epsilon()) && (direction * tout < (direction * (tend + (0.1 * direction * hout))))) {
integrator->run();
}
this->finalize();
this->reportStats();
this->postRun();
}
int MbD::IntegratorInterface::orderMax()
{
return system->orderMax;
}

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