87ed8700e2
* first fix to start branch PR * explicit conversion from sizet to int * Array.h and DiagonalMatrix.h * many sizet to int conversions * removed some unused variables and added expl overrides * removed many unused parameters * more typing * even more * last of the easy changes
402 lines
10 KiB
C++
402 lines
10 KiB
C++
#include "MomentOfInertiaSolver.h"
|
|
#include "EulerParameters.h"
|
|
#include <iostream>
|
|
#include <algorithm>
|
|
|
|
using namespace MbD;
|
|
|
|
void MbD::MomentOfInertiaSolver::example1()
|
|
{
|
|
auto aJpp = std::make_shared<FullMatrix<double>>(ListListD{
|
|
{ 1, 0, 0 },
|
|
{ 0, 2, 0 },
|
|
{ 0, 0, 3 }
|
|
});
|
|
|
|
auto rpPp = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 1 });
|
|
auto rotAxis = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 1 });
|
|
auto aApP = std::make_shared<EulerParameters<double>>(rotAxis, OS_M_PI*10/180)->aA;
|
|
auto solver = std::make_shared<MomentOfInertiaSolver>();
|
|
solver->setm(4.0);
|
|
solver->setJPP(aJpp);
|
|
solver->setrPoP(rpPp);
|
|
solver->setAPo(aApP);
|
|
auto rPcmP = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
|
|
solver->setrPcmP(rPcmP);
|
|
solver->calc();
|
|
auto aJPP = solver->getJoo();
|
|
std::cout << *solver->getJoo() << std::endl;
|
|
std::cout << *solver->getJpp() << std::endl;
|
|
std::cout << *solver->getAPp() << std::endl;
|
|
solver->setm(4.0);
|
|
solver->setJPP(aJPP);
|
|
auto rPoP = aApP->transposeTimesFullColumn(rpPp->negated());
|
|
solver->setrPoP(rPoP);
|
|
auto aAPo = aApP->transpose();
|
|
solver->setAPo(aAPo);
|
|
solver->setrPcmP(rPoP);
|
|
solver->calc();
|
|
std::cout << *solver->getJoo() << std::endl;
|
|
std::cout << *solver->getJpp() << std::endl;
|
|
std::cout << *solver->getAPp() << std::endl;
|
|
}
|
|
|
|
void MbD::MomentOfInertiaSolver::doFullPivoting(int p)
|
|
{
|
|
double max = 0.0;
|
|
auto pivotRow = p;
|
|
auto pivotCol = p;
|
|
for (int i = p; i < 3; i++)
|
|
{
|
|
auto rowi = aJcmPcopy->at(i);
|
|
for (int j = p; j < 3; j++)
|
|
{
|
|
auto aij = rowi->at(j);
|
|
if (aij != 0.0) {
|
|
auto mag = std::abs(aij);
|
|
if (mag > max) {
|
|
max = mag;
|
|
pivotRow = i;
|
|
pivotCol = j;
|
|
}
|
|
}
|
|
}
|
|
|
|
}
|
|
if (p != pivotRow) aJcmPcopy->swapElems(p, pivotRow);
|
|
if (p != pivotCol) {
|
|
for (auto& row : *aJcmPcopy) {
|
|
row->swapElems(p, pivotCol);
|
|
}
|
|
colOrder->swapElems(p, pivotCol);
|
|
}
|
|
}
|
|
|
|
void MbD::MomentOfInertiaSolver::forwardEliminateWithPivot(int p)
|
|
{
|
|
auto rowp = aJcmPcopy->at(p);
|
|
auto app = rowp->at(p);
|
|
for (int i = p + 1; i < 3; i++)
|
|
{
|
|
auto rowi = aJcmPcopy->at(i);
|
|
auto aip = rowi->at(p);
|
|
if (aip != 0) {
|
|
rowi->atiput(p, 0.0);
|
|
auto factor = aip / app;
|
|
for (int j = p + 1; j < 3; j++)
|
|
{
|
|
rowi->atiminusNumber(j, factor * rowp->at(j));
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void MbD::MomentOfInertiaSolver::backSubstituteIntoDU()
|
|
{
|
|
}
|
|
|
|
void MbD::MomentOfInertiaSolver::postSolve()
|
|
{
|
|
}
|
|
|
|
FColDsptr MbD::MomentOfInertiaSolver::basicSolvewithsaveOriginal(FMatDsptr, FColDsptr, bool)
|
|
{
|
|
return FColDsptr();
|
|
}
|
|
|
|
FColDsptr MbD::MomentOfInertiaSolver::basicSolvewithsaveOriginal(SpMatDsptr, FColDsptr, bool)
|
|
{
|
|
return FColDsptr();
|
|
}
|
|
|
|
void MbD::MomentOfInertiaSolver::preSolvewithsaveOriginal(FMatDsptr, FColDsptr, bool)
|
|
{
|
|
}
|
|
|
|
void MbD::MomentOfInertiaSolver::preSolvewithsaveOriginal(SpMatDsptr, FColDsptr, bool)
|
|
{
|
|
}
|
|
|
|
double MbD::MomentOfInertiaSolver::getmatrixArowimaxMagnitude(int)
|
|
{
|
|
return 0.0;
|
|
}
|
|
|
|
void MbD::MomentOfInertiaSolver::doPivoting(int)
|
|
{
|
|
}
|
|
|
|
void MbD::MomentOfInertiaSolver::setm(double mass)
|
|
{
|
|
m = mass;
|
|
}
|
|
|
|
void MbD::MomentOfInertiaSolver::setJPP(FMatDsptr mat)
|
|
{
|
|
aJPP = mat;
|
|
}
|
|
|
|
void MbD::MomentOfInertiaSolver::setrPoP(FColDsptr col)
|
|
{
|
|
rPoP = col;
|
|
}
|
|
|
|
void MbD::MomentOfInertiaSolver::setAPo(FMatDsptr mat)
|
|
{
|
|
aAPo = mat;
|
|
}
|
|
|
|
void MbD::MomentOfInertiaSolver::setrPcmP(FColDsptr col)
|
|
{
|
|
rPcmP = col;
|
|
}
|
|
|
|
FMatDsptr MbD::MomentOfInertiaSolver::getJoo()
|
|
{
|
|
return aJoo;
|
|
}
|
|
|
|
DiagMatDsptr MbD::MomentOfInertiaSolver::getJpp()
|
|
{
|
|
return aJpp;
|
|
}
|
|
|
|
FMatDsptr MbD::MomentOfInertiaSolver::getAPp()
|
|
{
|
|
return aAPp;
|
|
}
|
|
|
|
void MbD::MomentOfInertiaSolver::calc()
|
|
{
|
|
calcJoo();
|
|
calcJpp();
|
|
calcAPp();
|
|
}
|
|
|
|
void MbD::MomentOfInertiaSolver::calcJoo()
|
|
{
|
|
//"aJoo = aAPoT*[aJPP + mass*(rPoPTilde*rPoPTilde + rPoPTilde*rocmPTilde + rocmPTilde*rPoPTilde)]*aAPo"
|
|
|
|
if (!rPoP) {
|
|
rPoP = rPcmP;
|
|
aAPo = FullMatrix<double>::identitysptr(3);
|
|
}
|
|
auto rocmPtilde = FullMatrix<double>::tildeMatrix(rPcmP->minusFullColumn(rPoP));
|
|
auto rPoPtilde = FullMatrix<double>::tildeMatrix(rPoP);
|
|
auto term1 = aJPP;
|
|
auto term21 = rPoPtilde->timesFullMatrix(rPoPtilde);
|
|
auto term22 = rPoPtilde->timesFullMatrix(rocmPtilde);
|
|
auto term23 = term22->transpose();
|
|
auto term2 = term21->plusFullMatrix(term22)->plusFullMatrix(term23)->times(m);
|
|
aJoo = aAPo->transposeTimesFullMatrix(term1->plusFullMatrix(term2))->timesFullMatrix(aAPo);
|
|
aJoo->symLowerWithUpper();
|
|
aJoo->conditionSelfWithTol(aJoo->maxMagnitude() * 1.0e-6);
|
|
}
|
|
|
|
void MbD::MomentOfInertiaSolver::calcJpp()
|
|
{
|
|
//"aJcmP = aJPP + mass*(rPcmPTilde*rPcmPTilde)"
|
|
|
|
auto rPcmPtilde = FullMatrix<double>::tildeMatrix(rPcmP);
|
|
aJcmP = aJPP->plusFullMatrix(rPcmPtilde->timesFullMatrix(rPcmPtilde)->times(m));
|
|
aJcmP->symLowerWithUpper();
|
|
aJcmP->conditionSelfWithTol(aJcmP->maxMagnitude() * 1.0e-6);
|
|
if (aJcmP->isDiagonal()) {
|
|
calcJppFromDiagJcmP();
|
|
}
|
|
else {
|
|
calcJppFromFullJcmP();
|
|
}
|
|
}
|
|
|
|
void MbD::MomentOfInertiaSolver::calcAPp()
|
|
{
|
|
FColDsptr eigenvector1, eigenvector2, eigenvector0;
|
|
auto lam0 = aJpp->at(0);
|
|
auto lam1 = aJpp->at(1);
|
|
auto lam2 = aJpp->at(2);
|
|
if (lam0 == lam1) {
|
|
if (lam1 == lam2) {
|
|
aAPp = FullMatrix<double>::identitysptr(3);
|
|
}
|
|
else {
|
|
eigenvector1 = eigenvectorFor(lam1);
|
|
eigenvector2 = eigenvectorFor(lam2);
|
|
eigenvector1->normalizeSelf();
|
|
eigenvector2->normalizeSelf();
|
|
if (eigenvector1->at(1) < 0.0) eigenvector1->negateSelf();
|
|
if (eigenvector2->at(2) < 0.0) eigenvector2->negateSelf();
|
|
eigenvector0 = eigenvector1->cross(eigenvector2);
|
|
}
|
|
}
|
|
else {
|
|
eigenvector0 = eigenvectorFor(lam0);
|
|
eigenvector1 = eigenvectorFor(lam1);
|
|
eigenvector0->normalizeSelf();
|
|
eigenvector1->normalizeSelf();
|
|
if (eigenvector0->at(0) < 0.0) eigenvector0->negateSelf();
|
|
if (eigenvector1->at(1) < 0.0) eigenvector1->negateSelf();
|
|
eigenvector2 = eigenvector0->cross(eigenvector1);
|
|
}
|
|
aAPp = std::make_shared<FullMatrix<double>>(3, 3);
|
|
aAPp->atijputFullColumn(0, 0, eigenvector0);
|
|
aAPp->atijputFullColumn(0, 1, eigenvector1);
|
|
aAPp->atijputFullColumn(0, 2, eigenvector2);
|
|
}
|
|
|
|
FColDsptr MbD::MomentOfInertiaSolver::eigenvectorFor(double lam)
|
|
{
|
|
//"[aJcmP] - lam[I]."
|
|
|
|
double e0, e1, e2;
|
|
aJcmPcopy = aJcmP->copy();
|
|
colOrder = std::make_shared<FullRow<int>>(3);
|
|
auto eigenvector = std::make_shared<FullColumn<double>>(3);
|
|
for (int i = 0; i < 3; i++)
|
|
{
|
|
colOrder->atiput(i, i);
|
|
aJcmPcopy->atijminusNumber(i, i, lam);
|
|
}
|
|
for (int p = 0; p < 3; p++)
|
|
{
|
|
doFullPivoting(p);
|
|
forwardEliminateWithPivot(p);
|
|
}
|
|
|
|
auto row0 = aJcmPcopy->at(0);
|
|
auto row1 = aJcmPcopy->at(1);
|
|
auto row2 = aJcmPcopy->at(2);
|
|
auto norm0 = row0->length();
|
|
//auto aaaa = row2->length();
|
|
if ((row2->length() / norm0) > 1.0e-5) throw std::runtime_error("3rd row should be very small.");
|
|
if ((row1->length() / norm0) > 1.0e-5) {
|
|
e2 = 1.0;
|
|
e1 = e2 * -row1->at(2) / row1->at(1);
|
|
e0 = -(e1 * row0->at(1) + e2 * row0->at(2)) / row0->at(0);
|
|
}
|
|
else {
|
|
//"Repeated roots."
|
|
e2 = 1.0;
|
|
e1 = 0.0;
|
|
e0 = -(e1 * row0->at(1) + e2 * row0->at(2)) / row0->at(0);
|
|
}
|
|
|
|
|
|
auto dum = std::make_shared<FullColumn<double>>(ListD{ e0, e1, e2 });
|
|
for (int i = 0; i < 3; i++)
|
|
{
|
|
eigenvector->atiput(colOrder->at(i), dum->at(i));
|
|
}
|
|
return eigenvector;
|
|
}
|
|
|
|
void MbD::MomentOfInertiaSolver::calcJppFromDiagJcmP()
|
|
{
|
|
//"Eigenvalues are orders from smallest to largest."
|
|
|
|
double average;
|
|
auto sortedJ = std::make_shared<DiagonalMatrix<double>>();
|
|
sortedJ->push_back(aJcmP->at(0)->at(0));
|
|
sortedJ->push_back(aJcmP->at(1)->at(1));
|
|
sortedJ->push_back(aJcmP->at(2)->at(2));
|
|
std::sort(sortedJ->begin(), sortedJ->end(), [](double a, double b) { return a < b; });
|
|
auto lam0 = sortedJ->at(0);
|
|
auto lam1 = sortedJ->at(1);
|
|
auto lam2 = sortedJ->at(2);
|
|
if (lam1 == 0 || std::abs((lam0 / lam1) - 1.0) < 1.0e-6) {
|
|
if (lam2 == 0 || std::abs((lam1 / lam2) - 1.0) < 1.0e-6) {
|
|
//"All are equal."
|
|
average = (lam0 + lam1 + lam2) / 3.0;
|
|
lam0 = average;
|
|
lam1 = average;
|
|
lam2 = average;
|
|
}
|
|
else {
|
|
//"lam0 = lam1"
|
|
average = (lam0 + lam1) / 2.0;
|
|
lam0 = average;
|
|
lam1 = average;
|
|
}
|
|
}
|
|
else {
|
|
if (std::abs(lam1 / lam2 - 1.0) < 1.0e-6) {
|
|
//"lam1 = lam2"
|
|
average = (lam1 + lam2) / 2.0;
|
|
lam1 = average;
|
|
lam2 = average;
|
|
}
|
|
}
|
|
aJpp = std::make_shared<DiagonalMatrix<double>>(ListD{ lam0, lam1, lam2 });
|
|
}
|
|
|
|
void MbD::MomentOfInertiaSolver::calcJppFromFullJcmP()
|
|
{
|
|
//"Eigenvalues are orders from smallest to largest."
|
|
//"Rounding error can be significant."
|
|
//"e.g. (diag 100.0d 100.0d 1.0d) gives only 8 digit accuracy."
|
|
|
|
double average;
|
|
auto a00 = aJcmP->at(0)->at(0);
|
|
auto a01 = aJcmP->at(0)->at(1);
|
|
auto a02 = aJcmP->at(0)->at(2);
|
|
auto a11 = aJcmP->at(1)->at(1);
|
|
auto a12 = aJcmP->at(1)->at(2);
|
|
auto a22 = aJcmP->at(2)->at(2);
|
|
auto a = -(a00 + a11 + a22);
|
|
auto b = a00 * a11 + (a00 * a22) + (a11 * a22) - (a01 * a01) - (a02 * a02) - (a12 * a12);
|
|
auto c = a00 * a12 * a12 + (a11 * a02 * a02) + (a22 * a01 * a01) - (a00 * a11 * a22) - (2.0 * a01 * a02 * a12);
|
|
auto aDiv3 = a / 3.0;
|
|
auto p = (b / 3.0) - (aDiv3 * aDiv3);
|
|
auto q = (c + (2.0 * aDiv3 * aDiv3 * aDiv3) - (aDiv3 * b)) / 2.0;
|
|
auto phiDiv3 = modifiedArcCos(-q / std::sqrt(-p * p * p)) / 3.0;
|
|
auto twoSqrtMinusp = 2.0 * std::sqrt(-p);
|
|
auto piDiv3 = OS_M_PI / 3.0;
|
|
auto sortedJ = std::make_shared<DiagonalMatrix<double>>();
|
|
sortedJ->push_back(twoSqrtMinusp * std::cos(phiDiv3));
|
|
sortedJ->push_back(twoSqrtMinusp * -std::cos(phiDiv3 + piDiv3));
|
|
sortedJ->push_back(twoSqrtMinusp * -std::cos(phiDiv3 - piDiv3));
|
|
std::sort(sortedJ->begin(), sortedJ->end(), [](double a, double b) { return a < b; });
|
|
auto lam0 = sortedJ->at(0) - aDiv3;
|
|
auto lam1 = sortedJ->at(1) - aDiv3;
|
|
auto lam2 = sortedJ->at(2) - aDiv3;
|
|
if (lam1 == 0 || std::abs((lam0 / lam1) - 1.0) < 1.0e-6) {
|
|
if (lam2 == 0 || std::abs((lam1 / lam2) - 1.0) < 1.0e-6) {
|
|
//"All are equal."
|
|
average = (lam0 + lam1 + lam2) / 3.0;
|
|
lam0 = average;
|
|
lam1 = average;
|
|
lam2 = average;
|
|
}
|
|
else {
|
|
//"lam0 = lam1"
|
|
average = (lam0 + lam1) / 2.0;
|
|
lam0 = average;
|
|
lam1 = average;
|
|
}
|
|
}
|
|
else {
|
|
if (std::abs((lam1 / lam2) - 1.0) < 1.0e-6) {
|
|
//"lam1 = lam2"
|
|
average = (lam1 + lam2) / 2.0;
|
|
lam1 = average;
|
|
lam2 = average;
|
|
}
|
|
}
|
|
aJpp = std::make_shared<DiagonalMatrix<double>>(ListD{ lam0, lam1, lam2 });
|
|
}
|
|
|
|
double MbD::MomentOfInertiaSolver::modifiedArcCos(double val)
|
|
{
|
|
if (val > 1.0) {
|
|
return 0.0;
|
|
}
|
|
else {
|
|
if (val < -1.0) {
|
|
return OS_M_PI;
|
|
}
|
|
else {
|
|
return std::acos(val);
|
|
}
|
|
}
|
|
}
|