315 lines
10 KiB
C++
315 lines
10 KiB
C++
/***************************************************************************
|
|
* Copyright (c) 2023 Ondsel, Inc. *
|
|
* *
|
|
* This file is part of OndselSolver. *
|
|
* *
|
|
* See LICENSE file for details about copyright. *
|
|
***************************************************************************/
|
|
|
|
#include "EulerParameters.h"
|
|
#include "FullColumn.h"
|
|
#include "FullRow.h"
|
|
#include "FullMatrix.h"
|
|
|
|
namespace MbD {
|
|
template<>
|
|
FMatFColDsptr EulerParameters<double>::ppApEpEtimesColumn(FColDsptr col)
|
|
{
|
|
double a2c0 = 2 * col->at(0);
|
|
double a2c1 = 2 * col->at(1);
|
|
double a2c2 = 2 * col->at(2);
|
|
double m2c0 = 0 - a2c0;
|
|
double m2c1 = 0 - a2c1;
|
|
double m2c2 = 0 - a2c2;
|
|
auto col00 = std::make_shared<FullColumn<double>>(ListD{ a2c0, m2c1, m2c2 });
|
|
auto col01 = std::make_shared<FullColumn<double>>(ListD{ a2c1, a2c0, 0 });
|
|
auto col02 = std::make_shared<FullColumn<double>>(ListD{ a2c2, 0, a2c0 });
|
|
auto col03 = std::make_shared<FullColumn<double>>(ListD{ 0, m2c2, a2c1 });
|
|
auto col11 = std::make_shared<FullColumn<double>>(ListD{ m2c0, a2c1, m2c2 });
|
|
auto col12 = std::make_shared<FullColumn<double>>(ListD{ 0, a2c2, a2c1 });
|
|
auto col13 = std::make_shared<FullColumn<double>>(ListD{ a2c2, 0, m2c0 });
|
|
auto col22 = std::make_shared<FullColumn<double>>(ListD{ m2c0, m2c1, a2c2 });
|
|
auto col23 = std::make_shared<FullColumn<double>>(ListD{ m2c1, a2c0, 0 });
|
|
auto col33 = std::make_shared<FullColumn<double>>(ListD{ a2c0, a2c1, a2c2 });
|
|
auto answer = std::make_shared<FullMatrixFullColumnDouble>(4, 4);
|
|
auto& row0 = answer->at(0);
|
|
row0->at(0) = col00;
|
|
row0->at(1) = col01;
|
|
row0->at(2) = col02;
|
|
row0->at(3) = col03;
|
|
auto& row1 = answer->at(1);
|
|
row1->at(0) = col01;
|
|
row1->at(1) = col11;
|
|
row1->at(2) = col12;
|
|
row1->at(3) = col13;
|
|
auto& row2 = answer->at(2);
|
|
row2->at(0) = col02;
|
|
row2->at(1) = col12;
|
|
row2->at(2) = col22;
|
|
row2->at(3) = col23;
|
|
auto& row3 = answer->at(3);
|
|
row3->at(0) = col03;
|
|
row3->at(1) = col13;
|
|
row3->at(2) = col23;
|
|
row3->at(3) = col33;
|
|
return answer;
|
|
}
|
|
|
|
template<>
|
|
FMatDsptr EulerParameters<double>::pCpEtimesColumn(FColDsptr col)
|
|
{
|
|
//"col size = 4."
|
|
auto c0 = col->at(0);
|
|
auto c1 = col->at(1);
|
|
auto c2 = col->at(2);
|
|
auto mc0 = -c0;
|
|
auto mc1 = -c1;
|
|
auto mc2 = -c2;
|
|
auto mc3 = -col->at(3);
|
|
auto answer = std::make_shared<FullMatrixDouble>(3, 4);
|
|
auto& row0 = answer->at(0);
|
|
auto& row1 = answer->at(1);
|
|
auto& row2 = answer->at(2);
|
|
row0->atiput(0, mc3);
|
|
row0->atiput(1, mc2);
|
|
row0->atiput(2, c1);
|
|
row0->atiput(3, c0);
|
|
row1->atiput(0, c2);
|
|
row1->atiput(1, mc3);
|
|
row1->atiput(2, mc0);
|
|
row1->atiput(3, c1);
|
|
row2->atiput(0, mc1);
|
|
row2->atiput(1, c0);
|
|
row2->atiput(2, mc3);
|
|
row2->atiput(3, c2);
|
|
return answer;
|
|
}
|
|
|
|
template<typename T>
|
|
FMatDsptr EulerParameters<T>::pCTpEtimesColumn(FColDsptr col)
|
|
{
|
|
//"col size = 3."
|
|
auto c0 = col->at(0);
|
|
auto c1 = col->at(1);
|
|
auto c2 = col->at(2);
|
|
auto mc0 = -c0;
|
|
auto mc1 = -c1;
|
|
auto mc2 = -c2;
|
|
auto answer = std::make_shared<FullMatrixDouble>(4, 4);
|
|
auto& row0 = answer->at(0);
|
|
auto& row1 = answer->at(1);
|
|
auto& row2 = answer->at(2);
|
|
auto& row3 = answer->at(3);
|
|
row0->atiput(0, 0.0);
|
|
row0->atiput(1, c2);
|
|
row0->atiput(2, mc1);
|
|
row0->atiput(3, c0);
|
|
row1->atiput(0, mc2);
|
|
row1->atiput(1, 0.0);
|
|
row1->atiput(2, c0);
|
|
row1->atiput(3, c1);
|
|
row2->atiput(0, c1);
|
|
row2->atiput(1, mc0);
|
|
row2->atiput(2, 0.0);
|
|
row2->atiput(3, c2);
|
|
row3->atiput(0, mc0);
|
|
row3->atiput(1, mc1);
|
|
row3->atiput(2, mc2);
|
|
row3->atiput(3, 0.0);
|
|
return answer;
|
|
}
|
|
|
|
template<>
|
|
FMatFMatDsptr EulerParameters<double>::ppApEpEtimesMatrix(FMatDsptr mat)
|
|
{
|
|
FRowDsptr a2m0 = mat->at(0)->times(2.0);
|
|
FRowDsptr a2m1 = mat->at(1)->times(2.0);
|
|
FRowDsptr a2m2 = mat->at(2)->times(2.0);
|
|
FRowDsptr m2m0 = a2m0->negated();
|
|
FRowDsptr m2m1 = a2m1->negated();
|
|
FRowDsptr m2m2 = a2m2->negated();
|
|
FRowDsptr zero = std::make_shared<FullRow<double>>(3, 0.0);
|
|
auto mat00 = std::make_shared<FullMatrixDouble>(ListFRD{ a2m0, m2m1, m2m2 });
|
|
auto mat01 = std::make_shared<FullMatrixDouble>(ListFRD{ a2m1, a2m0, zero });
|
|
auto mat02 = std::make_shared<FullMatrixDouble>(ListFRD{ a2m2, zero, a2m0 });
|
|
auto mat03 = std::make_shared<FullMatrixDouble>(ListFRD{ zero, m2m2, a2m1 });
|
|
auto mat11 = std::make_shared<FullMatrixDouble>(ListFRD{ m2m0, a2m1, m2m2 });
|
|
auto mat12 = std::make_shared<FullMatrixDouble>(ListFRD{ zero, a2m2, a2m1 });
|
|
auto mat13 = std::make_shared<FullMatrixDouble>(ListFRD{ a2m2, zero, m2m0 });
|
|
auto mat22 = std::make_shared<FullMatrixDouble>(ListFRD{ m2m0, m2m1, a2m2 });
|
|
auto mat23 = std::make_shared<FullMatrixDouble>(ListFRD{ m2m1, a2m0, zero });
|
|
auto mat33 = std::make_shared<FullMatrixDouble>(ListFRD{ a2m0, a2m1, a2m2 });
|
|
auto answer = std::make_shared<FullMatrixFullMatrixDouble>(4, 4);
|
|
auto& row0 = answer->at(0);
|
|
row0->at(0) = mat00;
|
|
row0->at(1) = mat01;
|
|
row0->at(2) = mat02;
|
|
row0->at(3) = mat03;
|
|
auto& row1 = answer->at(1);
|
|
row1->at(0) = mat01;
|
|
row1->at(1) = mat11;
|
|
row1->at(2) = mat12;
|
|
row1->at(3) = mat13;
|
|
auto& row2 = answer->at(2);
|
|
row2->at(0) = mat02;
|
|
row2->at(1) = mat12;
|
|
row2->at(2) = mat22;
|
|
row2->at(3) = mat23;
|
|
auto& row3 = answer->at(3);
|
|
row3->at(0) = mat03;
|
|
row3->at(1) = mat13;
|
|
row3->at(2) = mat23;
|
|
row3->at(3) = mat33;
|
|
return answer;
|
|
}
|
|
|
|
template<typename T> // this is ALWAYS double; see note below.
|
|
void EulerParameters<T>::initialize()
|
|
{
|
|
aA = FullMatrixDouble::identitysptr(3);
|
|
aB = std::make_shared<FullMatrixDouble>(3, 4);
|
|
aC = std::make_shared<FullMatrixDouble>(3, 4);
|
|
pApE = std::make_shared<FullColumn<FMatDsptr>>(4);
|
|
for (int i = 0; i < 4; i++)
|
|
{
|
|
pApE->at(i) = std::make_shared<FullMatrixDouble>(3, 3);
|
|
}
|
|
}
|
|
|
|
// the following can't be valid as FullMatrix instatiatiates <double>, yet
|
|
// this class needs to see the member functions of FullMatrix
|
|
//template<>
|
|
//void EulerParameters<double>::initialize()
|
|
//{
|
|
//}
|
|
|
|
template<typename T>
|
|
void EulerParameters<T>::calc()
|
|
{
|
|
this->calcABC();
|
|
this->calcpApE();
|
|
}
|
|
template<>
|
|
void EulerParameters<double>::calcABC()
|
|
{
|
|
double aE0 = this->at(0);
|
|
double aE1 = this->at(1);
|
|
double aE2 = this->at(2);
|
|
double aE3 = this->at(3);
|
|
double mE0 = -aE0;
|
|
double mE1 = -aE1;
|
|
double mE2 = -aE2;
|
|
FRowDsptr aBi;
|
|
aBi = aB->at(0);
|
|
aBi->at(0) = aE3;
|
|
aBi->at(1) = mE2;
|
|
aBi->at(2) = aE1;
|
|
aBi->at(3) = mE0;
|
|
aBi = aB->at(1);
|
|
aBi->at(0) = aE2;
|
|
aBi->at(1) = aE3;
|
|
aBi->at(2) = mE0;
|
|
aBi->at(3) = mE1;
|
|
aBi = aB->at(2);
|
|
aBi->at(0) = mE1;
|
|
aBi->at(1) = aE0;
|
|
aBi->at(2) = aE3;
|
|
aBi->at(3) = mE2;
|
|
FRowDsptr aCi;
|
|
aCi = aC->at(0);
|
|
aCi->at(0) = aE3;
|
|
aCi->at(1) = aE2;
|
|
aCi->at(2) = mE1;
|
|
aCi->at(3) = mE0;
|
|
aCi = aC->at(1);
|
|
aCi->at(0) = mE2;
|
|
aCi->at(1) = aE3;
|
|
aCi->at(2) = aE0;
|
|
aCi->at(3) = mE1;
|
|
aCi = aC->at(2);
|
|
aCi->at(0) = aE1;
|
|
aCi->at(1) = mE0;
|
|
aCi->at(2) = aE3;
|
|
aCi->at(3) = mE2;
|
|
|
|
aA = aB->timesTransposeFullMatrix(aC);
|
|
}
|
|
template<>
|
|
void EulerParameters<double>::calcpApE()
|
|
{
|
|
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 = pApE->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 = pApE->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 = pApE->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 = pApE->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>
|
|
void EulerParameters<T>::conditionSelf()
|
|
{
|
|
EulerArray<T>::conditionSelf();
|
|
this->normalizeSelf();
|
|
}
|
|
|
|
template class EulerParameters<double>;
|
|
}
|