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

EndFrameqct done

Browse Source
This commit is contained in:
Aik-Siong Koh
2023-05-12 21:50:11 -06:00
parent 9ddca433a1
commit c147f2a33d
65 changed files with 514 additions and 166 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
MbDCode/Array.h
View File

@@ -12,6 +12,7 @@ namespace MbD {
Array(size_t count, const T& value) : std::vector<T>(count, value) {}
Array(std::initializer_list<T> list) : std::vector<T>{ list } {}
void copy(std::shared_ptr<Array<T>> x);
void zeroSelf();
};
template<typename T>
inline void Array<T>::copy(std::shared_ptr<Array<T>> x)
@@ -20,5 +21,14 @@ namespace MbD {
this->at(i) = x->at(i);
}
}
template <>
inline void Array<double>::zeroSelf() {
for (int i = 0; i < this->size(); i++) {
this->at(i) = 0.0;;
}
}
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>>>;
}

1
MbDCode/CartesianFrame.cpp
View File

@@ -12,5 +12,4 @@ CartesianFrame::CartesianFrame(const char* str) : Item(str)
void CartesianFrame::initialize()
{
Item::initialize();
}

5
MbDCode/Constant.cpp
View File

@@ -9,3 +9,8 @@ Constant::Constant()
Constant::Constant(double val) : Variable(val)
{
}
std::shared_ptr<Symbolic> MbD::Constant::differentiateWRT(std::shared_ptr<Symbolic> var)
{
return std::make_shared<Constant>(0.0);
}

1
MbDCode/Constant.h
View File

@@ -8,6 +8,7 @@ namespace MbD {
public:
Constant();
Constant(double val);
std::shared_ptr<Symbolic> differentiateWRT(std::shared_ptr<Symbolic> var) override;
};
}

2
MbDCode/ConstraintIJ.cpp
View File

@@ -3,7 +3,7 @@
using namespace MbD;
MbD::ConstraintIJ::ConstraintIJ(std::shared_ptr<EndFramec> frmi, std::shared_ptr<EndFramec> frmj) : frmI(frmi), frmJ(frmj)
MbD::ConstraintIJ::ConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj) : frmI(frmi), frmJ(frmj)
{
aConstant = 0.0;
}

5
MbDCode/ConstraintIJ.h
View File

@@ -1,5 +1,6 @@
#pragma once
//#include "typedef.h"
#include "Constraint.h"
#include "EndFramec.h"
@@ -8,9 +9,9 @@ namespace MbD {
{
//frmI frmJ aConstant
public:
ConstraintIJ(std::shared_ptr<EndFramec> frmi, std::shared_ptr<EndFramec> frmj);
ConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj);
std::shared_ptr<EndFramec> frmI, frmJ;
EndFrmcptr frmI, frmJ;
double aConstant;
};
}

2
MbDCode/DirectionCosineConstraintIJ.cpp
View File

@@ -3,7 +3,7 @@
using namespace MbD;
DirectionCosineConstraintIJ::DirectionCosineConstraintIJ(std::shared_ptr<EndFramec> frmi, std::shared_ptr<EndFramec> frmj, int axisi, int axisj) :
DirectionCosineConstraintIJ::DirectionCosineConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj) :
ConstraintIJ(frmI, frmJ), axisI(axisi), axisJ(axisj)
{
aAijIeJe = std::make_shared<DirectionCosineIecJec>(frmI, frmJ, axisI, axisJ);

2
MbDCode/DirectionCosineConstraintIJ.h
View File

@@ -9,7 +9,7 @@ namespace MbD {
//axisI axisJ aAijIeJe
public:
// self owns : (MbDDirectionCosineConstraintIJ withFrmI : frmI frmJ : frmJ axisI : 2 axisJ : 1).
DirectionCosineConstraintIJ(std::shared_ptr<EndFramec> frmI, std::shared_ptr<EndFramec> frmJ, int axisI, int axisJ);
DirectionCosineConstraintIJ(EndFrmcptr frmI, EndFrmcptr frmJ, int axisI, int axisJ);
int axisI, axisJ;
std::shared_ptr<DirectionCosineIecJec> aAijIeJe;

2
MbDCode/DirectionCosineIecJec.cpp
View File

@@ -9,7 +9,7 @@ MbD::DirectionCosineIecJec::DirectionCosineIecJec()
{
}
MbD::DirectionCosineIecJec::DirectionCosineIecJec(std::shared_ptr<EndFramec> frmi, std::shared_ptr<EndFramec> frmj, int axisi, int axisj) :
MbD::DirectionCosineIecJec::DirectionCosineIecJec(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj) :
KinematicIeJe(frmi, frmj), axisI(axisi), axisJ(axisj)
{
aAijIeJe = 0.0;

2
MbDCode/DirectionCosineIecJec.h
View File

@@ -11,7 +11,7 @@ namespace MbD {
//aAijIeJe axisI axisJ aAjOIe aAjOJe
public:
DirectionCosineIecJec();
DirectionCosineIecJec(std::shared_ptr<EndFramec> frmI, std::shared_ptr<EndFramec> frmJ, int axisI, int axisJ);
DirectionCosineIecJec(EndFrmcptr frmI, EndFrmcptr frmJ, int axisI, int axisJ);
int axisI, axisJ;
double aAijIeJe;

2
MbDCode/DirectionCosineIeqcJec.h
View File

@@ -13,7 +13,7 @@ namespace MbD {
FRowDsptr pAijIeJepEI;
FMatDsptr ppAijIeJepEIpEI;
FMatDsptr pAjOIepEIT;
std::shared_ptr<FullMatrix<FullColumn<double>>> ppAjOIepEIpEI;
std::shared_ptr<FullMatrix<std::shared_ptr<FullColumn<double>>>> ppAjOIepEIpEI;
};
}

2
MbDCode/DirectionCosineIeqcJeqc.h
View File

@@ -14,7 +14,7 @@ namespace MbD {
FMatDsptr ppAijIeJepEIpEJ;
FMatDsptr ppAijIeJepEJpEJ;
FMatDsptr pAjOJepEJT;
std::shared_ptr<FullMatrix<FullColumn<double>>> ppAjOJepEJpEJ;
std::shared_ptr<FullMatrix<std::shared_ptr<FullColumn<double>>>> ppAjOJepEJpEJ;
};
}

1
MbDCode/EndFramec.cpp
View File

@@ -14,7 +14,6 @@ EndFramec::EndFramec(const char* str) : CartesianFrame(str) {
void EndFramec::initialize()
{
CartesianFrame::initialize();
}
void EndFramec::setMarkerFrame(MarkerFrame* markerFrm)

6
MbDCode/EndFramec.h
View File

@@ -1,6 +1,9 @@
#pragma once
#include <memory>
#include "CartesianFrame.h"
#include "MarkerFrame.h"
//#include "MarkerFrame.h"
#include "FullColumn.h"
#include "FullMatrix.h"
@@ -24,5 +27,6 @@ namespace MbD {
FColDsptr rOeO = std::make_shared<FullColumn<double>>(3);
FMatDsptr aAOe = std::make_shared<FullMatrix<double>>(3, 3);
};
using EndFrmcptr = std::shared_ptr<EndFramec>;
}

39
MbDCode/EndFrameqc.cpp
View File

@@ -2,6 +2,8 @@
#include "EndFrameqc.h"
#include "EndFrameqct.h"
#include "Variable.h"
#include "MarkerFrame.h"
using namespace MbD;
@@ -15,26 +17,41 @@ EndFrameqc::EndFrameqc(const char* str) : EndFramec(str) {
void EndFrameqc::initialize()
{
EndFramec::initialize();
prOeOpE = std::make_unique<FullMatrix<double>>(3, 4);
pprOeOpEpE = std::make_unique<FullMatrix<std::shared_ptr<FullColumn<double>>>>(4, 4);
pAOepE = std::make_unique<FullColumn<std::shared_ptr<FullMatrix<double>>>>(4);
ppAOepEpE = std::make_unique<FullMatrix<std::shared_ptr<FullMatrix<double>>>>(4, 4);
prOeOpE = std::make_shared<FullMatrix<double>>(3, 4);
pprOeOpEpE = std::make_shared<FullMatrix<std::shared_ptr<FullColumn<double>>>>(4, 4);
pAOepE = std::make_shared<FullColumn<std::shared_ptr<FullMatrix<double>>>>(4);
ppAOepEpE = std::make_shared<FullMatrix<std::shared_ptr<FullMatrix<double>>>>(4, 4);
}
void EndFrameqc::initializeLocally()
{
if (endFrameqct) {
endFrameqct->initializeLocally();
}
}
void EndFrameqc::initializeGlobally()
{
if (endFrameqct) {
endFrameqct->initializeGlobally();
}
else {
pprOeOpEpE = markerFrame->pprOmOpEpE;
ppAOepEpE = markerFrame->ppAOmpEpE;
}
}
void MbD::EndFrameqc::EndFrameqctFrom(std::shared_ptr<EndFramec>& frm)
void MbD::EndFrameqc::EndFrameqctFrom(EndFrmcptr& frm)
{
std::shared_ptr<EndFramec> newFrm;
newFrm = std::make_shared<EndFrameqct>(frm->getName().c_str());
newFrm->setMarkerFrame(frm->getMarkerFrame());
//frm.swap(newFrm);
std::swap(*(frm.get()), *(newFrm.get()));
endFrameqct = std::make_shared<EndFrameqct>();
}
void MbD::EndFrameqc::setrmemBlks(std::shared_ptr<FullColumn<std::shared_ptr<Symbolic>>> xyzBlks)
{
std::static_pointer_cast<EndFrameqct>(endFrameqct)->rmemBlks = xyzBlks;
}
void MbD::EndFrameqc::setphiThePsiBlks(std::shared_ptr<FullColumn<std::shared_ptr<Symbolic>>> xyzRotBlks)
{
std::static_pointer_cast<EndFrameqct>(endFrameqct)->phiThePsiBlks = xyzRotBlks;
}

18
MbDCode/EndFrameqc.h
View File

@@ -1,10 +1,9 @@
#pragma once
#include "EndFramec.h"
#include "FullColumn.h"
#include "FullMatrix.h"
namespace MbD {
class EndFramec;
class Symbolic;
class EndFrameqc : public EndFramec
{
@@ -15,12 +14,15 @@ namespace MbD {
void initialize();
void initializeLocally() override;
void initializeGlobally() override;
void EndFrameqctFrom(std::shared_ptr<EndFramec>& frm) override;
void EndFrameqctFrom(EndFrmcptr& frm) override;
void setrmemBlks(std::shared_ptr<FullColumn<std::shared_ptr<Symbolic>>> xyzBlks);
void setphiThePsiBlks(std::shared_ptr<FullColumn<std::shared_ptr<Symbolic>>> xyzRotBlks);
FMatDuptr prOeOpE;
std::unique_ptr<FullMatrix<std::shared_ptr<FullColumn<double>>>> pprOeOpEpE;
std::unique_ptr<FullColumn<std::shared_ptr<FullMatrix<double>>>> pAOepE;
FMatFMatDuptr ppAOepEpE;
FMatDsptr prOeOpE;
std::shared_ptr<FullMatrix<std::shared_ptr<FullColumn<double>>>> pprOeOpEpE;
std::shared_ptr<FullColumn<std::shared_ptr<FullMatrix<double>>>> pAOepE;
FMatFMatDsptr ppAOepEpE;
std::shared_ptr<EndFramec> endFrameqct;
};
}

74
MbDCode/EndFrameqct.cpp
View File

@@ -1,4 +1,5 @@
#include "EndFrameqct.h"
#include "System.h"
using namespace MbD;
@@ -12,34 +13,63 @@ EndFrameqct::EndFrameqct(const char* str) : EndFrameqc(str) {
void EndFrameqct::initialize()
{
EndFrameqc::initialize();
rmem = std::make_unique<FullColumn<double>>(3);
prmempt = std::make_unique<FullColumn<double>>(3);
pprmemptpt = std::make_unique<FullColumn<double>>(3);
aAme = std::make_unique<FullMatrix<double>>(3, 3);
pAmept = std::make_unique<FullMatrix<double>>(3, 3);
ppAmeptpt = std::make_unique<FullMatrix<double>>(3, 3);
pprOeOpEpt = std::make_unique<FullMatrix<double>>(3, 4);
pprOeOptpt = std::make_unique<FullColumn<double>>(3);
ppAOepEpt = std::make_unique<FullColumn<FullMatrix<double>>>(4);
ppAOeptpt = std::make_unique<FullMatrix<double>>(3, 3);
rmem = std::make_shared<FullColumn<double>>(3);
prmempt = std::make_shared<FullColumn<double>>(3);
pprmemptpt = std::make_shared<FullColumn<double>>(3);
aAme = std::make_shared<FullMatrix<double>>(3, 3);
pAmept = std::make_shared<FullMatrix<double>>(3, 3);
ppAmeptpt = std::make_shared<FullMatrix<double>>(3, 3);
pprOeOpEpt = std::make_shared<FullMatrix<double>>(3, 4);
pprOeOptpt = std::make_shared<FullColumn<double>>(3);
ppAOepEpt = std::make_shared<FullColumn<std::shared_ptr<FullMatrix<double>>>>(4);
ppAOeptpt = std::make_shared<FullMatrix<double>>(3, 3);
}
void EndFrameqct::initializeLocally()
{
//rmemBlks == nil
// ifTrue :
//[rmem zeroSelf.
// prmempt zeroSelf.
// pprmemptpt zeroSelf] .
// phiThePsiBlks == nil
// ifTrue :
//[aAme identity.
// pAmept zeroSelf.
// ppAmeptpt zeroSelf]
if (!rmemBlks) {
rmem->zeroSelf();
prmempt->zeroSelf();
pprmemptpt->zeroSelf();
}
if (!phiThePsiBlks) {
aAme->identity();
pAmept->zeroSelf();
ppAmeptpt->zeroSelf();
}
}
void EndFrameqct::initializeGlobally()
{
if (!rmemBlks) {
initprmemptBlks();
initpprmemptptBlks();
}
if (!phiThePsiBlks) {
initpPhiThePsiptBlks();
initppPhiThePsiptptBlks();
}
}
void MbD::EndFrameqct::initprmemptBlks()
{
auto time = System::getInstance().time;
prmemptBlks = std::make_shared< FullColumn<std::shared_ptr<Symbolic>>>(3);
for (int i = 0; i < 3; i++) {
auto disp = rmemBlks->at(i);
auto vel = (disp->differentiateWRT(time))->simplified();
prmemptBlks->at(i) = vel;
}
}
void MbD::EndFrameqct::initpprmemptptBlks()
{
}
void MbD::EndFrameqct::initpPhiThePsiptBlks()
{
}
void MbD::EndFrameqct::initppPhiThePsiptptBlks()
{
}

21
MbDCode/EndFrameqct.h
View File

@@ -1,8 +1,10 @@
#pragma once
#include "EndFrameqc.h"
#include "Variable.h"
namespace MbD {
class Symbolic;
class EndFrameqct : public EndFrameqc
{
//time rmemBlks prmemptBlks pprmemptptBlks phiThePsiBlks pPhiThePsiptBlks ppPhiThePsiptptBlks
@@ -13,14 +15,19 @@ namespace MbD {
void initialize();
void initializeLocally() override;
void initializeGlobally() override;
void initprmemptBlks();
void initpprmemptptBlks();
void initpPhiThePsiptBlks();
void initppPhiThePsiptptBlks();
double time;
std::shared_ptr<FullColumn<std::shared_ptr<Variable>>> rmemBlks;
std::shared_ptr<FullColumn<std::shared_ptr<Variable>>> phiThePsiBlks;
FColDuptr rmem, prmempt, pprmemptpt, pprOeOptpt;
FMatDuptr aAme, pAmept, ppAmeptpt, ppAOeptpt;
FMatDuptr pprOeOpEpt;
std::unique_ptr<FullColumn<FullMatrix<double>>> ppAOepEpt;
std::shared_ptr<FullColumn<std::shared_ptr<Symbolic>>> rmemBlks, prmemptBlks, pprmemptptBlks;
std::shared_ptr<FullColumn<std::shared_ptr<Symbolic>>> phiThePsiBlks, pPhiThePsiptBlks, ppPhiThePsiptptBlks;
FColDsptr rmem, prmempt, pprmemptpt, pprOeOptpt;
FMatDsptr aAme, pAmept, ppAmeptpt, ppAOeptpt;
FMatDsptr pprOeOpEpt;
std::shared_ptr<FullColumn<std::shared_ptr<FullMatrix<double>>>> ppAOepEpt;
};
using EndFrmqctptr = std::shared_ptr<EndFrameqct>;
}

8
MbDCode/EulerParameters.cpp
View File

@@ -6,7 +6,7 @@
using namespace MbD;
std::unique_ptr<FullMatrix<std::shared_ptr<FullColumn<double>>>> EulerParameters::ppApEpEtimesColumn(FColDsptr col)
std::shared_ptr<FullMatrix<std::shared_ptr<FullColumn<double>>>> EulerParameters::ppApEpEtimesColumn(FColDsptr col)
{
double a2c1 = 2 * col->at(0);
double a2c2 = 2 * col->at(1);
@@ -24,7 +24,7 @@ std::unique_ptr<FullMatrix<std::shared_ptr<FullColumn<double>>>> EulerParameters
auto col33 = std::make_shared<FullColumn<double>>(ListD{ m2c1, m2c2, a2c3 });
auto col34 = std::make_shared<FullColumn<double>>(ListD{ m2c2, a2c1, 0 });
auto col44 = std::make_shared<FullColumn<double>>(ListD{ a2c1, a2c2, a2c3 });
auto answer = std::make_unique<FullMatrix<std::shared_ptr<FullColumn<double>>>>(4, 4);
auto answer = std::make_shared<FullMatrix<std::shared_ptr<FullColumn<double>>>>(4, 4);
auto row1 = answer->at(0);
row1->at(0) = col11;
row1->at(1) = col12;
@@ -48,7 +48,7 @@ std::unique_ptr<FullMatrix<std::shared_ptr<FullColumn<double>>>> EulerParameters
return answer;
}
std::unique_ptr<FullMatrix<std::shared_ptr<FullMatrix<double>>>> EulerParameters::ppApEpEtimesMatrix(FMatDsptr mat)
std::shared_ptr<FullMatrix<std::shared_ptr<FullMatrix<double>>>> EulerParameters::ppApEpEtimesMatrix(FMatDsptr mat)
{
FRowDsptr a2m1 = mat->at(0)->times(2.0);
FRowDsptr a2m2 = mat->at(1)->times(2.0);
@@ -68,7 +68,7 @@ std::unique_ptr<FullMatrix<std::shared_ptr<FullMatrix<double>>>> EulerParameters
auto mat33 = std::make_shared<FullMatrix<double>>(ListFRD{ m2m1, m2m2, a2m3 });
auto mat34 = std::make_shared<FullMatrix<double>>(ListFRD{ m2m2, a2m1, zero });
auto mat44 = std::make_shared<FullMatrix<double>>(ListFRD{ a2m1, a2m2, a2m3 });
auto answer = std::make_unique<FullMatrix<std::shared_ptr<FullMatrix<double>>>>(4, 4);
auto answer = std::make_shared<FullMatrix<std::shared_ptr<FullMatrix<double>>>>(4, 4);
auto row1 = answer->at(0);
row1->at(0) = mat11;
row1->at(1) = mat12;

4
MbDCode/EulerParameters.h
View File

@@ -8,8 +8,8 @@ namespace MbD {
{
//aA aB aC pApE
public:
static std::unique_ptr<FullMatrix<std::shared_ptr<FullColumn<double>>>> ppApEpEtimesColumn(FColDsptr col);
static std::unique_ptr<FullMatrix<std::shared_ptr<FullMatrix<double>>>> ppApEpEtimesMatrix(FMatDsptr col);
static std::shared_ptr<FullMatrix<std::shared_ptr<FullColumn<double>>>> ppApEpEtimesColumn(FColDsptr col);
static std::shared_ptr<FullMatrix<std::shared_ptr<FullMatrix<double>>>> ppApEpEtimesMatrix(FMatDsptr col);
};
}

1
MbDCode/ExpressionX.cpp Normal file
View File

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

10
MbDCode/ExpressionX.h Normal file
View File

@@ -0,0 +1,10 @@
#pragma once
#include "FunctionX.h"
namespace MbD {
class ExpressionX : public FunctionX
{
};
}

7
MbDCode/FullColumn.h
View File

@@ -25,10 +25,7 @@ namespace MbD {
return ss.str();
}
};
typedef std::shared_ptr<FullColumn<double>> FColDsptr;
typedef std::unique_ptr<FullColumn<double>> FColDuptr;
//typedef std::shared_ptr<FullColumn<std::shared_ptr<FullMatrix<double>>>> FColFMatDsptr;
//typedef std::unique_ptr<FullColumn<std::shared_ptr<FullMatrix<double>>>> FColFMatDuptr;
using FColDsptr = std::shared_ptr<FullColumn<double>>;
//using FColFMatDsptr = std::shared_ptr<FullColumn<std::shared_ptr<FullMatrix<double>>>>;
}

22
MbDCode/FullMatrix.h
View File

@@ -31,16 +31,18 @@ namespace MbD {
this->push_back(row);
}
}
void identity();
};
typedef std::initializer_list<std::initializer_list<double>> ListListD;
typedef std::initializer_list<FRowDsptr> ListFRD;
typedef std::shared_ptr<FullMatrix<double>> FMatDsptr;
typedef std::unique_ptr<FullMatrix<double>> FMatDuptr;
//typedef std::shared_ptr<FullMatrix<std::shared_ptr<FullColumn<double>>>> FMatFColDsptr;
//typedef std::unique_ptr<FullMatrix<std::shared_ptr<FullColumn<double>>>> FMatFColDuptr;
typedef std::shared_ptr<FullMatrix<std::shared_ptr<FullMatrix<double>>>> FMatFMatDsptr;
typedef std::unique_ptr<FullMatrix<std::shared_ptr<FullMatrix<double>>>> FMatFMatDuptr;
template <>
inline void FullMatrix<double>::identity() {
this->zeroSelf();
for (int i = 0; i < this->size(); i++) {
this->at(i)->at(i) = 1.0;
}
}
using FMatDsptr = std::shared_ptr<FullMatrix<double>>;
using FMatDsptr = std::shared_ptr<FullMatrix<double>>;
//using FMatFColDsptr = std::shared_ptr<FullMatrix<std::shared_ptr<FullColumn<double>>>>;
using FMatFMatDsptr = std::shared_ptr<FullMatrix<std::shared_ptr<FullMatrix<double>>>>;
}

3
MbDCode/FullRow.h
View File

@@ -13,7 +13,6 @@ namespace MbD {
std::shared_ptr<FullRow<T>> negated();
};
typedef std::shared_ptr<FullRow<double>> FRowDsptr;
template<typename T>
inline std::shared_ptr<FullRow<T>> FullRow<T>::times(double a)
@@ -30,5 +29,7 @@ namespace MbD {
{
return this->times(-1.0);
}
using ListFRD = std::initializer_list<std::shared_ptr<FullRow<double>>>;
using FRowDsptr = std::shared_ptr<FullRow<double>>;
}

1
MbDCode/Function.cpp Normal file
View File

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

10
MbDCode/Function.h Normal file
View File

@@ -0,0 +1,10 @@
#pragma once
#include "Symbolic.h"
namespace MbD {
class Function : public Symbolic
{
};
}

27
MbDCode/FunctionWithManyArgs.cpp Normal file
View File

@@ -0,0 +1,27 @@
#include "FunctionWithManyArgs.h"
#include "Symbolic.h"
using namespace MbD;
MbD::FunctionWithManyArgs::FunctionWithManyArgs(std::shared_ptr<Symbolic> term)
{
terms = std::make_shared<std::vector<std::shared_ptr<Symbolic>>>();
terms->push_back(term);
}
MbD::FunctionWithManyArgs::FunctionWithManyArgs(std::shared_ptr<Symbolic> term, std::shared_ptr<Symbolic> term1) : FunctionWithManyArgs(term)
{
terms->push_back(term1);
}
MbD::FunctionWithManyArgs::FunctionWithManyArgs(std::shared_ptr<Symbolic> term, std::shared_ptr<Symbolic> term1, std::shared_ptr<Symbolic> term2) : FunctionWithManyArgs(term, term1)
{
terms->push_back(term2);
}
MbD::FunctionWithManyArgs::FunctionWithManyArgs(std::shared_ptr<std::vector<std::shared_ptr<Symbolic>>> _terms) {
terms = std::make_shared<std::vector<std::shared_ptr<Symbolic>>>();
for (int i = 0; i < _terms->size(); i++)
terms->push_back(_terms->at(i));
}

18
MbDCode/FunctionWithManyArgs.h Normal file
View File

@@ -0,0 +1,18 @@
#pragma once
#include "Function.h"
namespace MbD {
class FunctionWithManyArgs : public Function
{
//terms
public:
FunctionWithManyArgs(std::shared_ptr<Symbolic> term);
FunctionWithManyArgs(std::shared_ptr<Symbolic> term, std::shared_ptr<Symbolic> term1);
FunctionWithManyArgs(std::shared_ptr<Symbolic> term, std::shared_ptr<Symbolic> term1, std::shared_ptr<Symbolic> term2);
FunctionWithManyArgs(std::shared_ptr<std::vector<std::shared_ptr<Symbolic>>> _terms);
std::shared_ptr<std::vector<std::shared_ptr<Symbolic>>> terms;
};
}

1
MbDCode/FunctionX.cpp Normal file
View File

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

10
MbDCode/FunctionX.h Normal file
View File

@@ -0,0 +1,10 @@
#pragma once
#include "Function.h"
namespace MbD {
class FunctionX : public Function
{
};
}

4
MbDCode/Joint.cpp
View File

@@ -14,10 +14,10 @@ Joint::Joint(const char* str) : Item(str) {
void Joint::initialize()
{
constraints = std::make_unique<std::vector<std::shared_ptr<Constraint>>>();
constraints = std::make_shared<std::vector<std::shared_ptr<Constraint>>>();
}
void Joint::connectsItoJ(std::shared_ptr<EndFramec> frmi, std::shared_ptr<EndFramec> frmj)
void Joint::connectsItoJ(EndFrmcptr frmi, EndFrmcptr frmj)
{
frmI = frmi;
frmJ = frmj;

12
MbDCode/Joint.h
View File

@@ -2,13 +2,15 @@
#include <memory>
#include <vector>
//#include "typedef.h"
#include "Item.h"
#include "EndFramec.h"
#include "Constraint.h"
namespace MbD {
class EndFramec;
//class EndFramec;
class Constraint;
//using EndFrmcptr = std::shared_ptr<EndFramec>;
class Joint : public Item
{
@@ -17,13 +19,13 @@ namespace MbD {
Joint();
Joint(const char* str);
void initialize();
virtual void connectsItoJ(std::shared_ptr<EndFramec> frmI, std::shared_ptr<EndFramec> frmJ);
virtual void connectsItoJ(EndFrmcptr frmI, EndFrmcptr frmJ);
void initializeLocally() override;
void initializeGlobally() override;
std::shared_ptr<EndFramec> frmI;
std::shared_ptr<EndFramec> frmJ;
std::unique_ptr<std::vector<std::shared_ptr<Constraint>>> constraints;
EndFrmcptr frmI;
EndFrmcptr frmJ;
std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> constraints;
};
}

2
MbDCode/KinematicIeJe.cpp
View File

@@ -6,6 +6,6 @@ MbD::KinematicIeJe::KinematicIeJe()
{
}
MbD::KinematicIeJe::KinematicIeJe(std::shared_ptr<EndFramec> frmi, std::shared_ptr<EndFramec> frmj) : frmI(frmi), frmJ(frmj)
MbD::KinematicIeJe::KinematicIeJe(EndFrmcptr frmi, EndFrmcptr frmj) : frmI(frmi), frmJ(frmj)
{
}

5
MbDCode/KinematicIeJe.h
View File

@@ -1,5 +1,6 @@
#pragma once
//#include "typedef.h"
#include "Item.h"
#include "EndFramec.h"
@@ -9,9 +10,9 @@ namespace MbD {
//frmI frmJ
public:
KinematicIeJe();
KinematicIeJe(std::shared_ptr<EndFramec> frmi, std::shared_ptr<EndFramec> frmj);
KinematicIeJe(EndFrmcptr frmi, EndFrmcptr frmj);
std::shared_ptr<EndFramec> frmI, frmJ;
EndFrmcptr frmI, frmJ;
};
}

10
MbDCode/MarkerFrame.cpp
View File

@@ -19,12 +19,9 @@ MarkerFrame::MarkerFrame(const char* str) : CartesianFrame(str) {
void MarkerFrame::initialize()
{
//prOmOpE: = StMFullMatrix new : 3 by : 4.
//pAOmpE : = StMFullColumn new : 4.
//endFrames : = OrderedCollection new
prOmOpE = std::make_shared<FullMatrix<double>>(3, 4);
pAOmpE = std::make_unique<FullColumn<FullMatrix<double>>>(4);
endFrames = std::make_unique<std::vector<std::shared_ptr<EndFramec>>>();
pAOmpE = std::make_shared<FullColumn<std::shared_ptr<FullMatrix<double>>>>(4);
endFrames = std::make_shared<std::vector<EndFrmcptr>>();
auto endFrm = std::make_shared<EndFrameqc>("EndFrame1");
this->addEndFrame(endFrm);
}
@@ -47,7 +44,7 @@ void MarkerFrame::setaApm(FMatDsptr x)
{
aApm->copy(x);
}
void MarkerFrame::addEndFrame(std::shared_ptr<EndFramec> endFrm)
void MarkerFrame::addEndFrame(EndFrmcptr endFrm)
{
endFrm->setMarkerFrame(this);
endFrames->push_back(endFrm);
@@ -62,4 +59,5 @@ void MarkerFrame::initializeLocally()
void MarkerFrame::initializeGlobally()
{
std::for_each(endFrames->begin(), endFrames->end(), [](const auto& endFrame) { endFrame->initializeGlobally(); });
}

11
MbDCode/MarkerFrame.h
View File

@@ -10,6 +10,7 @@
namespace MbD {
class PartFrame;
class EndFramec;
using EndFrmcptr = std::shared_ptr<EndFramec>;
class MarkerFrame : public CartesianFrame
{
@@ -22,7 +23,7 @@ namespace MbD {
PartFrame* getPartFrame();
void setrpmp(FColDsptr x);
void setaApm(FMatDsptr x);
void addEndFrame(std::shared_ptr<EndFramec> x);
void addEndFrame(EndFrmcptr x);
void initializeLocally() override;
void initializeGlobally() override;
@@ -32,10 +33,10 @@ namespace MbD {
FColDsptr rOmO = std::make_shared<FullColumn<double>>(3);
FMatDsptr aAOm = std::make_shared<FullMatrix<double>>(3, 3);
FMatDsptr prOmOpE;
std::unique_ptr<FullColumn<FullMatrix<double>>> pAOmpE;
std::unique_ptr<FullMatrix<std::shared_ptr<FullColumn<double>>>> pprOmOpEpE;
FMatFMatDuptr ppAOmpEpE;
std::unique_ptr<std::vector<std::shared_ptr<EndFramec>>> endFrames;
std::shared_ptr<FullColumn<std::shared_ptr<FullMatrix<double>>>> pAOmpE;
std::shared_ptr<FullMatrix<std::shared_ptr<FullColumn<double>>>> pprOmOpEpE;
FMatFMatDsptr ppAOmpEpE;
std::shared_ptr<std::vector<EndFrmcptr>> endFrames;
};
}

7
MbDCode/MbDCode.cpp
View File

@@ -6,6 +6,7 @@
#include <iostream>
#include "System.h"
#include "Constant.h"
#include "FullColumn.h"
#include "FullMatrix.h"
#include "Part.h"
@@ -14,6 +15,8 @@
#include "RevoluteJoint.h"
#include "ZRotation.h"
#include "EndFrameqc.h"
#include "EndFrameqct.h"
#include "Product.h"
#include "MbDCode.h"
using namespace MbD;
@@ -21,7 +24,6 @@ using namespace MbD;
int main()
{
std::cout << "Hello World!\n";
//System& TheSystem = System::getInstance();
System& TheSystem = System::getInstance("TheSystem");
std::cout << "TheSystem.getName() " << TheSystem.getName() << std::endl;
auto systemSolver = TheSystem.systemSolver;
@@ -195,6 +197,9 @@ int main()
auto rotMotion1 = std::make_shared<ZRotation>("RotMotion1");
rotMotion1->connectsItoJ(assembly1->partFrame->endFrame("Marker2"), crankPart1->partFrame->endFrame("Marker1"));
auto omega = std::make_shared<Constant>(6.2831853071796);
auto timeScale = std::make_shared<Constant>(0.04);
rotMotion1->phiBlk = std::make_shared<Product>(omega, timeScale, TheSystem.time);
TheSystem.jointsMotions->push_back(rotMotion1);
//
TheSystem.runKINEMATICS();

13
MbDCode/MbDCode.vcxproj
View File

@@ -147,9 +147,13 @@
<ClCompile Include="EulerArray.cpp" />
<ClCompile Include="EulerConstraint.cpp" />
<ClCompile Include="EulerParameters.cpp" />
<ClCompile Include="ExpressionX.cpp" />
<ClCompile Include="FullColumn.cpp" />
<ClCompile Include="FullMatrix.cpp" />
<ClCompile Include="FullRow.cpp" />
<ClCompile Include="Function.cpp" />
<ClCompile Include="FunctionWithManyArgs.cpp" />
<ClCompile Include="FunctionX.cpp" />
<ClCompile Include="IndependentVariable.cpp" />
<ClCompile Include="Item.cpp" />
<ClCompile Include="Joint.cpp" />
@@ -160,6 +164,7 @@
<ClCompile Include="Part.cpp" />
<ClCompile Include="PartFrame.cpp" />
<ClCompile Include="PrescribedMotion.cpp" />
<ClCompile Include="Product.cpp" />
<ClCompile Include="RevoluteJoint.cpp" />
<ClCompile Include="RowTypeMatrix.cpp" />
<ClCompile Include="Solver.cpp" />
@@ -167,6 +172,7 @@
<ClCompile Include="SparseMatrix.cpp" />
<ClCompile Include="SparseRow.cpp" />
<ClCompile Include="SparseVector.cpp" />
<ClCompile Include="Sum.cpp" />
<ClCompile Include="Symbolic.cpp" />
<ClCompile Include="System.cpp" />
<ClCompile Include="SystemSolver.cpp" />
@@ -197,9 +203,13 @@
<ClInclude Include="EulerArray.h" />
<ClInclude Include="EulerConstraint.h" />
<ClInclude Include="EulerParameters.h" />
<ClInclude Include="ExpressionX.h" />
<ClInclude Include="FullColumn.h" />
<ClInclude Include="FullMatrix.h" />
<ClInclude Include="FullRow.h" />
<ClInclude Include="Function.h" />
<ClInclude Include="FunctionWithManyArgs.h" />
<ClInclude Include="FunctionX.h" />
<ClInclude Include="IndependentVariable.h" />
<ClInclude Include="Item.h" />
<ClInclude Include="Joint.h" />
@@ -210,6 +220,7 @@
<ClInclude Include="Part.h" />
<ClInclude Include="PartFrame.h" />
<ClInclude Include="PrescribedMotion.h" />
<ClInclude Include="Product.h" />
<ClInclude Include="RevoluteJoint.h" />
<ClInclude Include="RowTypeMatrix.h" />
<ClInclude Include="Solver.h" />
@@ -217,10 +228,12 @@
<ClInclude Include="SparseMatrix.h" />
<ClInclude Include="SparseRow.h" />
<ClInclude Include="SparseVector.h" />
<ClInclude Include="Sum.h" />
<ClInclude Include="Symbolic.h" />
<ClInclude Include="System.h" />
<ClInclude Include="SystemSolver.h" />
<ClInclude Include="Time.h" />
<ClInclude Include="typedef.h" />
<ClInclude Include="UserFunction.h" />
<ClInclude Include="Variable.h" />
<ClInclude Include="Vector.h" />

39
MbDCode/MbDCode.vcxproj.filters
View File

@@ -159,6 +159,24 @@
<ClCompile Include="DirectionCosineIeqctJeqc.cpp">
<Filter>Source Files</Filter>
</ClCompile>
<ClCompile Include="Function.cpp">
<Filter>Source Files</Filter>
</ClCompile>
<ClCompile Include="FunctionWithManyArgs.cpp">
<Filter>Source Files</Filter>
</ClCompile>
<ClCompile Include="Product.cpp">
<Filter>Source Files</Filter>
</ClCompile>
<ClCompile Include="Sum.cpp">
<Filter>Source Files</Filter>
</ClCompile>
<ClCompile Include="FunctionX.cpp">
<Filter>Source Files</Filter>
</ClCompile>
<ClCompile Include="ExpressionX.cpp">
<Filter>Source Files</Filter>
</ClCompile>
</ItemGroup>
<ItemGroup>
<ClInclude Include="Array.h">
@@ -305,5 +323,26 @@
<ClInclude Include="DirectionCosineIeqctJeqc.h">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="typedef.h">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="Function.h">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="FunctionWithManyArgs.h">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="Product.h">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="Sum.h">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="FunctionX.h">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="ExpressionX.h">
<Filter>Header Files</Filter>
</ClInclude>
</ItemGroup>
</Project>

10
MbDCode/NewtonRaphson.h
View File

@@ -1,8 +1,18 @@
#pragma once
#include <memory>
#include "Solver.h"
#include "FullColumn.h"
namespace MbD {
class NewtonRaphson : public Solver
{
//system xold x dx dxNorm dxNorms dxTol y yNorm yNormOld yNorms yNormTol pypx iterNo iterMax nDivergence nBackTracking twoAlp lam
public:
std::shared_ptr<FullColumn<double>> xold, x, dx, dxTol, y;
double dxNorm, yNorm, yNormOld, yNormTol, twoAlp, lam;
};
}

1
MbDCode/Part.cpp
View File

@@ -54,4 +54,5 @@ void Part::initializeLocally()
void Part::initializeGlobally()
{
partFrame->initializeGlobally();
}

6
MbDCode/Part.h
View File

@@ -30,14 +30,14 @@ namespace MbD {
int ipX;
int ipE;
double m;
std::unique_ptr<DiagonalMatrix<double>> aJ;
std::shared_ptr<DiagonalMatrix<double>> aJ;
std::shared_ptr<PartFrame> partFrame;
FColDsptr pX;
FColDsptr pXdot;
FColDsptr pE;
FColDsptr pEdot;
std::unique_ptr<DiagonalMatrix<double>> mX;
std::unique_ptr<DiagonalMatrix<double>> mE;
std::shared_ptr<DiagonalMatrix<double>> mX;
std::shared_ptr<DiagonalMatrix<double>> mE;
FMatDsptr mEdot;
FColDsptr pTpE;
FMatDsptr ppTpEpE;

9
MbDCode/PartFrame.cpp
View File

@@ -20,8 +20,8 @@ void PartFrame::initialize()
{
aGeu = std::make_shared<EulerConstraint>("EulerCon");
aGeu->setOwner(this);
aGabs = std::make_unique<std::vector<std::shared_ptr<AbsConstraint>>>();
markerFrames = std::make_unique<std::vector<std::shared_ptr<MarkerFrame>>>();
aGabs = std::make_shared<std::vector<std::shared_ptr<AbsConstraint>>>();
markerFrames = std::make_shared<std::vector<std::shared_ptr<MarkerFrame>>>();
}
void PartFrame::setqX(FColDsptr x) {
qX->copy(x);
@@ -48,7 +48,7 @@ void PartFrame::addMarkerFrame(std::shared_ptr<MarkerFrame> markerFrame)
markerFrames->push_back(markerFrame);
}
std::shared_ptr<EndFramec> PartFrame::endFrame(std::string name)
EndFrmcptr PartFrame::endFrame(std::string name)
{
auto match = std::find_if(markerFrames->begin(), markerFrames->end(), [&](auto mkr) {return mkr->getName() == name; });
return (*match)->endFrames->at(0);
@@ -72,4 +72,7 @@ void PartFrame::initializeLocally()
void PartFrame::initializeGlobally()
{
std::for_each(markerFrames->begin(), markerFrames->end(), [](const auto& markerFrame) { markerFrame->initializeGlobally(); });
aGeu->initializeGlobally();
std::for_each(aGabs->begin(), aGabs->end(), [](const auto& aGab) { aGab->initializeGlobally(); });
}

7
MbDCode/PartFrame.h
View File

@@ -14,6 +14,7 @@ namespace MbD {
class Part;
class MarkerFrame;
class EndFramec;
using EndFrmcptr = std::shared_ptr<EndFramec>;
class PartFrame : public CartesianFrame
{
@@ -33,7 +34,7 @@ namespace MbD {
void setPart(Part* x);
Part* getPart();
void addMarkerFrame(std::shared_ptr<MarkerFrame> x);
std::shared_ptr<EndFramec> endFrame(std::string name);
EndFrmcptr endFrame(std::string name);
Part* part;
int iqX, iqE; //Position index of frame variables qX and qE in system list of variables
@@ -44,8 +45,8 @@ namespace MbD {
FColDsptr qXddot = std::make_shared<FullColumn<double>>(3);
FColDsptr qEddot = std::make_shared<FullColumn<double>>(4);
std::shared_ptr<EulerConstraint> aGeu;
std::unique_ptr<std::vector<std::shared_ptr<AbsConstraint>>> aGabs;
std::unique_ptr<std::vector<std::shared_ptr<MarkerFrame>>> markerFrames;
std::shared_ptr<std::vector<std::shared_ptr<AbsConstraint>>> aGabs;
std::shared_ptr<std::vector<std::shared_ptr<MarkerFrame>>> markerFrames;
};
}

22
MbDCode/PrescribedMotion.cpp
View File

@@ -27,28 +27,8 @@ void PrescribedMotion::initialize()
psiBlk = std::make_shared<Constant>(0.0);
}
void PrescribedMotion::connectsItoJ(std::shared_ptr<EndFramec> frmi, std::shared_ptr<EndFramec> frmj)
void PrescribedMotion::connectsItoJ(EndFrmcptr frmi, EndFrmcptr frmj)
{
Joint::connectsItoJ(frmi, frmj);
frmI->EndFrameqctFrom(frmI);
}
//void PrescribedMotion::connectsItoJ(std::shared_ptr<EndFramec> frmi, std::shared_ptr<EndFramec> frmj)
//{
// Joint::connectsItoJ(frmi, frmj);
// frmI->EndFrameqctFrom(frmI);
// decltype(frmi) dddd;
// std::cout << "typeid(dddd).name() " << typeid(dddd).name() << std::endl;
// std::cout << "typeid(frmI).name() " << typeid(frmI).name() << std::endl;
// if (typeid(frmI).name() != "EndFrameqct") {
// std::shared_ptr<EndFramec> newFrmI;
// newFrmI = std::make_shared<EndFrameqct>(frmI->getName().c_str());
// decltype(newFrmI) ffff;
// std::cout << "typeid(ffff).name() " << typeid(ffff).name() << std::endl;
// auto gggg = std::make_shared<EndFrameqct>(frmI->getName().c_str());
// std::cout << "typeid(gggg).name() " << typeid(gggg).name() << std::endl;
// std::cout << "typeid(newFrmI).name() " << typeid(newFrmI).name() << std::endl;
// std::swap(frmI, newFrmI);
// assert(typeid(frmI).name() != "EndFrameqct");
// }
//}

17
MbDCode/PrescribedMotion.h
View File

@@ -2,6 +2,9 @@
#include "Joint.h"
namespace MbD {
class Symbolic;
class PrescribedMotion : public Joint
{
//xBlk yBlk zBlk phiBlk theBlk psiBlk
@@ -9,14 +12,14 @@ namespace MbD {
PrescribedMotion();
PrescribedMotion(const char* str);
void initialize();
void connectsItoJ(std::shared_ptr<EndFramec> frmI, std::shared_ptr<EndFramec> frmJ) override;
void connectsItoJ(EndFrmcptr frmI, EndFrmcptr frmJ) override;
std::shared_ptr<Variable> xBlk;
std::shared_ptr<Variable> yBlk;
std::shared_ptr<Variable> zBlk;
std::shared_ptr<Variable> phiBlk;
std::shared_ptr<Variable> theBlk;
std::shared_ptr<Variable> psiBlk;
std::shared_ptr<Symbolic> xBlk;
std::shared_ptr<Symbolic> yBlk;
std::shared_ptr<Symbolic> zBlk;
std::shared_ptr<Symbolic> phiBlk;
std::shared_ptr<Symbolic> theBlk;
std::shared_ptr<Symbolic> psiBlk;
};
}

10
MbDCode/Product.cpp Normal file
View File

@@ -0,0 +1,10 @@
#include "Product.h"
using namespace MbD;
double MbD::Product::getValue()
{
double answer = 1.0;
for (int i = 0; i < terms->size(); i++) answer *= terms->at(i)->getValue();
return answer;
}

16
MbDCode/Product.h Normal file
View File

@@ -0,0 +1,16 @@
#pragma once
#include "FunctionWithManyArgs.h"
namespace MbD {
class Product : public FunctionWithManyArgs
{
public:
Product(std::shared_ptr<Symbolic> term) : FunctionWithManyArgs(term) {}
Product(std::shared_ptr<Symbolic> term, std::shared_ptr<Symbolic> term1) : FunctionWithManyArgs(term, term1) {}
Product(std::shared_ptr<Symbolic> term, std::shared_ptr<Symbolic> term1, std::shared_ptr<Symbolic> term2) : FunctionWithManyArgs(term, term1, term2) {}
Product(std::shared_ptr<std::vector<std::shared_ptr<Symbolic>>> _terms) : FunctionWithManyArgs(_terms) {}
double getValue() override;
};
}

11
MbDCode/RowTypeMatrix.h
View File

@@ -1,6 +1,10 @@
#pragma once
#include "Array.h"
#include "FullRow.h"
namespace MbD {
template <typename T>
class RowTypeMatrix : public Array<T>
{
@@ -8,6 +12,7 @@ namespace MbD {
RowTypeMatrix() {}
RowTypeMatrix(std::initializer_list<T> list) : Array<T>{ list } {}
void copy(std::shared_ptr<RowTypeMatrix<T>> x);
void zeroSelf();
};
template<typename T>
@@ -17,5 +22,11 @@ namespace MbD {
this->at(i)->copy(x->at(i));
}
}
template <>
inline void RowTypeMatrix< std::shared_ptr<FullRow<double>>>::zeroSelf() {
for (int i = 0; i < this->size(); i++) {
this->at(i)->zeroSelf();
}
}
}

7
MbDCode/SparseMatrix.h
View File

@@ -15,8 +15,5 @@ namespace MbD {
}
}
};
typedef std::shared_ptr<SparseMatrix<double>> SpMatDptr;
typedef std::initializer_list<std::initializer_list<std::initializer_list<double>>> ListListPairD;
}
using SpMatDptr = std::shared_ptr<SparseMatrix<double>>;
}

2
MbDCode/SparseRow.h
View File

@@ -13,6 +13,6 @@ namespace MbD {
SparseRow(std::initializer_list<std::pair<const int, T>> list) : SparseVector<T>{ list } {}
SparseRow(std::initializer_list<std::initializer_list<T>> list) : SparseVector<T>{ list } {}
};
typedef std::shared_ptr<SparseRow<double>> SpRowDptr;
using SpRowDptr = std::shared_ptr<SparseRow<double>>;
}

10
MbDCode/Sum.cpp Normal file
View File

@@ -0,0 +1,10 @@
#include "Sum.h"
using namespace MbD;
double MbD::Sum::getValue()
{
double answer = 0.0;
for (int i = 0; i < terms->size(); i++) answer += terms->at(i)->getValue();
return answer;
}

16
MbDCode/Sum.h Normal file
View File

@@ -0,0 +1,16 @@
#pragma once
#include "FunctionWithManyArgs.h"
namespace MbD {
class Sum : public FunctionWithManyArgs
{
public:
Sum(std::shared_ptr<Symbolic> term) : FunctionWithManyArgs(term) {}
Sum(std::shared_ptr<Symbolic> term, std::shared_ptr<Symbolic> term1) : FunctionWithManyArgs(term, term1) {}
Sum(std::shared_ptr<Symbolic> term, std::shared_ptr<Symbolic> term1, std::shared_ptr<Symbolic> term2) : FunctionWithManyArgs(term, term1, term2) {}
Sum(std::shared_ptr<std::vector<std::shared_ptr<Symbolic>>> _terms) : FunctionWithManyArgs(_terms) {}
double getValue() override;
};
}

30
MbDCode/Symbolic.cpp
View File

@@ -1,4 +1,9 @@
#include <memory>
#include <unordered_set>
#include <assert.h>
#include "Symbolic.h"
#include "Constant.h"
using namespace MbD;
@@ -10,3 +15,28 @@ Symbolic::Symbolic()
void Symbolic::initialize()
{
}
std::shared_ptr<Symbolic> MbD::Symbolic::differentiateWRT(std::shared_ptr<Symbolic> var)
{
return std::shared_ptr<Symbolic>();
}
std::shared_ptr<Symbolic> MbD::Symbolic::simplified()
{
//Debug
auto set = nullptr;
//Debug auto set = std::make_shared<std::unordered_set<Symbolic>>();
auto answer = expandUntil(set);
auto set1 = nullptr; //std::make_shared<std::unordered_set<Symbolic>>();
return answer->simplifyUntil(set1);
}
std::shared_ptr<Symbolic> MbD::Symbolic::expandUntil(std::shared_ptr<std::unordered_set<Symbolic>> set)
{
return std::make_shared<Constant>(0.0);
}
std::shared_ptr<Symbolic> MbD::Symbolic::simplifyUntil(std::shared_ptr<std::unordered_set<Symbolic>> set)
{
return std::make_shared<Constant>(0.0);
}

10
MbDCode/Symbolic.h
View File

@@ -1,10 +1,20 @@
#pragma once
#include <unordered_set>
#include <memory>
namespace MbD {
class Symbolic
{
public:
Symbolic();
void initialize();
virtual std::shared_ptr<Symbolic> differentiateWRT(std::shared_ptr<Symbolic> var);
virtual std::shared_ptr<Symbolic> simplified();
virtual std::shared_ptr<Symbolic> expandUntil(std::shared_ptr<std::unordered_set<Symbolic>> set);
virtual std::shared_ptr<Symbolic> simplifyUntil(std::shared_ptr<std::unordered_set<Symbolic>> set);
virtual double getValue() = 0;
};
}

19
MbDCode/System.cpp
View File

@@ -5,17 +5,17 @@
using namespace MbD;
System::System() {
time = std::make_unique<Time>();
parts = std::make_unique<std::vector<std::shared_ptr<Part>>>();
jointsMotions = std::make_unique<std::vector<std::shared_ptr<Joint>>>();
systemSolver = std::make_unique<SystemSolver>(this);
time = std::make_shared<Time>();
parts = std::make_shared<std::vector<std::shared_ptr<Part>>>();
jointsMotions = std::make_shared<std::vector<std::shared_ptr<Joint>>>();
systemSolver = std::make_shared<SystemSolver>(this);
}
System::System(const char* str) : Item(str) {
time = std::make_unique<Time>();
parts = std::make_unique<std::vector<std::shared_ptr<Part>>>();
jointsMotions = std::make_unique<std::vector<std::shared_ptr<Joint>>>();
systemSolver = std::make_unique<SystemSolver>(this);
time = std::make_shared<Time>();
parts = std::make_shared<std::vector<std::shared_ptr<Part>>>();
jointsMotions = std::make_shared<std::vector<std::shared_ptr<Joint>>>();
systemSolver = std::make_shared<SystemSolver>(this);
}
void System::addPart(std::shared_ptr<Part> part)
@@ -61,4 +61,7 @@ void System::initializeLocally()
void System::initializeGlobally()
{
std::for_each(parts->begin(), parts->end(), [](const auto& part) { part->initializeGlobally(); });
std::for_each(jointsMotions->begin(), jointsMotions->end(), [](const auto& joint) { joint->initializeGlobally(); });
systemSolver->initializeGlobally();
}

6
MbDCode/System.h
View File

@@ -37,8 +37,8 @@ namespace MbD {
};
std::unique_ptr<std::vector<std::shared_ptr<Part>>> parts;
std::unique_ptr<std::vector<std::shared_ptr<Joint>>> jointsMotions;
std::shared_ptr<std::vector<std::shared_ptr<Part>>> parts;
std::shared_ptr<std::vector<std::shared_ptr<Joint>>> jointsMotions;
bool hasChanged = false;
std::shared_ptr<SystemSolver> systemSolver;
void addPart(std::shared_ptr<Part> part);
@@ -46,7 +46,7 @@ namespace MbD {
void initializeLocally() override;
void initializeGlobally() override;
std::unique_ptr<Time> time;
std::shared_ptr<Time> time;
private:
System();
System(const char* str);

2
MbDCode/SystemSolver.cpp
View File

@@ -4,7 +4,7 @@ using namespace MbD;
void SystemSolver::initializeLocally()
{
setsOfRedundantConstraints = std::make_unique<std::vector<std::vector<std::shared_ptr<Constraint>>>>();
setsOfRedundantConstraints = std::make_shared<std::vector<std::vector<std::shared_ptr<Constraint>>>>();
direction = (tstart < tend) ? 1.0 : -1.0;
toutFirst = tstart + (direction * hout);
}

2
MbDCode/SystemSolver.h
View File

@@ -23,7 +23,7 @@ namespace MbD {
std::shared_ptr<NewtonRaphson> icTypeSolver;
System* system;
std::unique_ptr<std::vector<std::vector<std::shared_ptr<Constraint>>>> setsOfRedundantConstraints;
std::shared_ptr<std::vector<std::vector<std::shared_ptr<Constraint>>>> setsOfRedundantConstraints;
double tstart = 0;
double tend = 25;

5
MbDCode/Variable.cpp
View File

@@ -29,3 +29,8 @@ const std::string& Variable::getName() const
{
return name;
}
double MbD::Variable::getValue()
{
return value;
}

1
MbDCode/Variable.h
View File

@@ -13,6 +13,7 @@ namespace MbD {
void initialize();
void setName(std::string& str);
const std::string& getName() const;
double getValue() override;
std::string name;
double value;

2
MbDCode/Vector.h
View File

@@ -12,5 +12,3 @@ namespace MbD {
Vector(std::initializer_list<T> list) : Array<T>{ list } {}
};
}
typedef std::initializer_list<double> ListD;

9
MbDCode/ZRotation.cpp
View File

@@ -1,3 +1,4 @@
#include "System.h"
#include "ZRotation.h"
#include "FullColumn.h"
#include "DirectionCosineConstraintIJ.h"
@@ -39,10 +40,10 @@ void MbD::ZRotation::initializeGlobally()
void MbD::ZRotation::initMotions()
{
auto xyzBlks = std::initializer_list<std::shared_ptr<Variable>>{ xBlk, yBlk, zBlk };
std::static_pointer_cast<EndFrameqct>(frmI)->rmemBlks = std::make_shared<FullColumn<std::shared_ptr<Variable>>>(xyzBlks);
auto xyzRotBlks = std::initializer_list<std::shared_ptr<Variable>>{ xBlk, yBlk, zBlk };
std::static_pointer_cast<EndFrameqct>(frmI)->phiThePsiBlks = std::make_shared<FullColumn<std::shared_ptr<Variable>>>(xyzRotBlks);
auto xyzBlks = std::initializer_list<std::shared_ptr<Symbolic>>{ xBlk, yBlk, zBlk };
std::static_pointer_cast<EndFrameqc>(frmI)->setrmemBlks(std::make_shared<FullColumn<std::shared_ptr<Symbolic>>>(xyzBlks));
auto xyzRotBlks = std::initializer_list<std::shared_ptr<Symbolic>>{ phiBlk, theBlk, psiBlk };
std::static_pointer_cast<EndFrameqc>(frmI)->setphiThePsiBlks(std::make_shared<FullColumn<std::shared_ptr<Symbolic>>>(xyzRotBlks));
}
void MbD::ZRotation::addConstraint(std::shared_ptr<Constraint> con)

30
MbDCode/typedef.h Normal file
View File

@@ -0,0 +1,30 @@
#pragma once
#include <memory>
namespace MbD {
//class EndFramec;
//class SparseRow;
//class SparseMatrix;
//typedef std::initializer_list<double> ListD;
//typedef std::initializer_list<std::initializer_list<double>> ListListD;
//typedef std::initializer_list<std::initializer_list<std::initializer_list<double>>> ListListPairD;
//typedef std::initializer_list<std::shared_ptr<FullRow<double>>> ListFRD;
//typedef std::shared_ptr<FullColumn<double>> FColDsptr;
//typedef std::shared_ptr<FullColumn<std::shared_ptr<FullMatrix<double>>>> FColFMatDsptr;
//typedef std::shared_ptr<FullRow<double>> FRowDsptr;
//typedef std::shared_ptr<FullMatrix<double>> FMatDsptr;
//typedef std::shared_ptr<FullMatrix<double>> FMatDsptr;
//typedef std::shared_ptr<FullMatrix<std::shared_ptr<FullMatrix<double>>>> FMatFMatDsptr;
//typedef std::shared_ptr<FullMatrix<std::shared_ptr<FullMatrix<double>>>> FMatFMatDsptr;
//typedef std::shared_ptr<FullMatrix<std::shared_ptr<FullColumn<double>>>> FMatFColDsptr;
//typedef std::shared_ptr<SparseRow<double>> SpRowDptr;
//typedef std::shared_ptr<SparseMatrix<double>> SpMatDptr;
//typedef std::shared_ptr<EndFramec> EndFrmcptr;
//typedef std::shared_ptr<EndFrameqct> EndFrmqctptr;
}