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

runPosIC, VelIC, AccIC numerically correct

Browse Source
This commit is contained in:
Aik-Siong Koh
2023-06-24 23:08:29 -06:00
parent 371b13a9e0
commit c30ee64b89
110 changed files with 2171 additions and 129 deletions
Download Patch File Download Diff File Expand all files Collapse all files

131
MbDCode/EulerParametersDot.h
View File

@@ -4,7 +4,6 @@
#include "FullColumn.h"
#include "FullMatrix.h"
#include "EulerParameters.h"
#include "CREATE.h"
namespace MbD {
@@ -17,20 +16,23 @@ namespace MbD {
EulerParametersDot(int count, const T& value) : EulerArray<T>(count, value) {}
EulerParametersDot(std::initializer_list<T> list) : EulerArray<T>{ list } {}
static std::shared_ptr<EulerParametersDot<T>> FromqEOpAndOmegaOpO(std::shared_ptr<EulerParameters<T>> qe, FColDsptr omeOpO);
void initialize() override;
void calcAdotBdotCdot();
void calcpAdotpE();
void calcpAdotpE();
FMatDsptr aB();
std::shared_ptr<EulerParameters<T>> qE;
FMatDsptr aAdot, aBdot, aCdot;
FColFMatDsptr pAdotpE;
void calc();
void calc() override;
};
template<typename T>
inline std::shared_ptr<EulerParametersDot<T>> EulerParametersDot<T>::FromqEOpAndOmegaOpO(std::shared_ptr<EulerParameters<T>> qEOp, FColDsptr omeOpO)
{
auto answer = CREATE<EulerParametersDot<T>>::With(4);
auto answer = std::make_shared<EulerParametersDot<T>>(4);
answer->initialize(); //Cannot use CREATE.h in subclasses of std::vector. Why?
qEOp->calcABC();
auto aB = qEOp->aB;
answer->equalFullColumn(aB->transposeTimesFullColumn(omeOpO->times(0.5)));
@@ -39,16 +41,133 @@ namespace MbD {
return answer;
}
template<typename T>
inline void EulerParametersDot<T>::initialize()
{
aAdot = std::make_shared<FullMatrix<double>>(3, 3);
aBdot = std::make_shared<FullMatrix<double>>(3, 4);
aCdot = std::make_shared<FullMatrix<double>>(3, 4);
pAdotpE = std::make_shared<FullColumn<FMatDsptr>>(4);
for (int i = 0; i < 4; i++)
{
pAdotpE->at(i) = std::make_shared<FullMatrix<double>>(3, 3);
}
}
template<typename T>
inline void EulerParametersDot<T>::calcAdotBdotCdot()
{
assert(false);
//"aAdot, aBdot and aCdot are all calculated together and only here."
auto aE0dot = this->at(0);
auto aE1dot = this->at(1);
auto aE2dot = this->at(2);
auto aE3dot = this->at(3);
auto mE0dot = -aE0dot;
auto mE1dot = -aE1dot;
auto mE2dot = -aE2dot;
aBdot->at(0)->at(0) = aE3dot;
aBdot->at(0)->at(1) = mE2dot;
aBdot->at(0)->at(2) = aE1dot;
aBdot->at(0)->at(3) = mE0dot;
aBdot->at(1)->at(0) = aE2dot;
aBdot->at(1)->at(1) = aE3dot;
aBdot->at(1)->at(2) = mE0dot;
aBdot->at(1)->at(3) = mE1dot;
aBdot->at(2)->at(0) = mE1dot;
aBdot->at(2)->at(1) = aE0dot;
aBdot->at(2)->at(2) = aE3dot;
aBdot->at(2)->at(3) = mE2dot;
aCdot->at(0)->at(0) = aE3dot;
aCdot->at(0)->at(1) = aE2dot;
aCdot->at(0)->at(2) = mE1dot;
aCdot->at(0)->at(3) = mE0dot;
aCdot->at(1)->at(0) = mE2dot;
aCdot->at(1)->at(1) = aE3dot;
aCdot->at(1)->at(2) = aE0dot;
aCdot->at(1)->at(3) = mE1dot;
aCdot->at(2)->at(0) = aE1dot;
aCdot->at(2)->at(1) = mE0dot;
aCdot->at(2)->at(2) = aE3dot;
aCdot->at(2)->at(3) = mE2dot;
aAdot = this->aB()->timesTransposeFullMatrix(aCdot)->times(2.0);
}
template<typename T>
inline void EulerParametersDot<T>::calcpAdotpE()
{
assert(false);
//"Mimic calcpApE"
//"All aE's are actually aEdot's."
double a2E0 = 2.0 * (this->at(0));
double a2E1 = 2.0 * (this->at(1));
double a2E2 = 2.0 * (this->at(2));
double a2E3 = 2.0 * (this->at(3));
double m2E0 = -a2E0;
double m2E1 = -a2E1;
double m2E2 = -a2E2;
double m2E3 = -a2E3;
FMatDsptr pApEk;
pApEk = pAdotpE->at(0);
FRowDsptr pAipEk;
pAipEk = pApEk->at(0);
pAipEk->at(0) = a2E0;
pAipEk->at(1) = a2E1;
pAipEk->at(2) = a2E2;
pAipEk = pApEk->at(1);
pAipEk->at(0) = a2E1;
pAipEk->at(1) = m2E0;
pAipEk->at(2) = m2E3;
pAipEk = pApEk->at(2);
pAipEk->at(0) = a2E2;
pAipEk->at(1) = a2E3;
pAipEk->at(2) = m2E0;
//
pApEk = pAdotpE->at(1);
pAipEk = pApEk->at(0);
pAipEk->at(0) = m2E1;
pAipEk->at(1) = a2E0;
pAipEk->at(2) = a2E3;
pAipEk = pApEk->at(1);
pAipEk->at(0) = a2E0;
pAipEk->at(1) = a2E1;
pAipEk->at(2) = a2E2;
pAipEk = pApEk->at(2);
pAipEk->at(0) = m2E3;
pAipEk->at(1) = a2E2;
pAipEk->at(2) = m2E1;
//
pApEk = pAdotpE->at(2);
pAipEk = pApEk->at(0);
pAipEk->at(0) = m2E2;
pAipEk->at(1) = m2E3;
pAipEk->at(2) = a2E0;
pAipEk = pApEk->at(1);
pAipEk->at(0) = a2E3;
pAipEk->at(1) = m2E2;
pAipEk->at(2) = a2E1;
pAipEk = pApEk->at(2);
pAipEk->at(0) = a2E0;
pAipEk->at(1) = a2E1;
pAipEk->at(2) = a2E2;
//
pApEk = pAdotpE->at(3);
pAipEk = pApEk->at(0);
pAipEk->at(0) = a2E3;
pAipEk->at(1) = m2E2;
pAipEk->at(2) = a2E1;
pAipEk = pApEk->at(1);
pAipEk->at(0) = a2E2;
pAipEk->at(1) = a2E3;
pAipEk->at(2) = m2E0;
pAipEk = pApEk->at(2);
pAipEk->at(0) = m2E1;
pAipEk->at(1) = a2E0;
pAipEk->at(2) = a2E3;
}
template<typename T>
inline FMatDsptr EulerParametersDot<T>::aB()
{
return qE->aB;
}
template<typename T>