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

DiagonalMatrix is now double only also.

Browse Source
This commit is contained in:
John Dupuy
2023-11-06 19:44:07 -06:00
parent 87bd168b50
commit c406ad8b00
23 changed files with 421 additions and 487 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
OndselSolver/ASMTAssembly.cpp
View File

@@ -262,7 +262,7 @@ void MbD::ASMTAssembly::runSinglePendulum()
auto massMarker = std::make_shared<ASMTPrincipalMassMarker>();
massMarker->setMass(0.0);
massMarker->setDensity(0.0);
auto aJ = std::make_shared<DiagonalMatrix<double>>(ListD{ 0, 0, 0 });
auto aJ = std::make_shared<DiagonalMatrix>(ListD{ 0, 0, 0 });
massMarker->setMomentOfInertias(aJ);
pos3D = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
massMarker->setPosition3D(pos3D);
@@ -307,7 +307,7 @@ void MbD::ASMTAssembly::runSinglePendulum()
massMarker = std::make_shared<ASMTPrincipalMassMarker>();
massMarker->setMass(0.2);
massMarker->setDensity(10.0);
aJ = std::make_shared<DiagonalMatrix<double>>(ListD{ 8.3333333333333e-4, 0.016833333333333, 0.017333333333333 });
aJ = std::make_shared<DiagonalMatrix>(ListD{ 8.3333333333333e-4, 0.016833333333333, 0.017333333333333 });
massMarker->setMomentOfInertias(aJ);
pos3D = std::make_shared<FullColumn<double>>(ListD{ 0.5, 0.1, 0.05 });
massMarker->setPosition3D(pos3D);
@@ -995,7 +995,7 @@ void MbD::ASMTAssembly::initprincipalMassMarker()
principalMassMarker = std::make_shared<ASMTPrincipalMassMarker>();
principalMassMarker->mass = 0.0;
principalMassMarker->density = 0.0;
principalMassMarker->momentOfInertias = std::make_shared<DiagonalMatrix<double>>(3, 0);
principalMassMarker->momentOfInertias = std::make_shared<DiagonalMatrix>(3, 0);
//principalMassMarker->position3D = std::make_shared<FullColumn<double>>(3, 0);
//principalMassMarker->rotationMatrix = FullMatrixDouble>::identitysptr(3);
}

4
OndselSolver/ASMTPrincipalMassMarker.cpp
View File

@@ -39,7 +39,7 @@ void MbD::ASMTPrincipalMassMarker::parseASMT(std::vector<std::string>& lines)
lines.erase(lines.begin());
assert(lines[0] == (leadingTabs + "MomentOfInertias"));
lines.erase(lines.begin());
momentOfInertias = std::make_shared<DiagonalMatrix<double>>(3);
momentOfInertias = std::make_shared<DiagonalMatrix>(3);
auto row = readRowOfDoubles(lines[0]);
lines.erase(lines.begin());
for (int i = 0; i < 3; i++)
@@ -70,7 +70,7 @@ void MbD::ASMTPrincipalMassMarker::setMomentOfInertias(DiagMatDsptr mat)
// Overloads to simplify syntax.
void MbD::ASMTPrincipalMassMarker::setMomentOfInertias(double a, double b, double c)
{
momentOfInertias = std::make_shared<DiagonalMatrix<double>>(ListD{ a, b, c });
momentOfInertias = std::make_shared<DiagonalMatrix>(ListD{ a, b, c });
}
void MbD::ASMTPrincipalMassMarker::storeOnLevel(std::ofstream& os, int level)

2
OndselSolver/ASMTPrincipalMassMarker.h
View File

@@ -26,7 +26,7 @@ namespace MbD {
double mass = 0.0;
double density = 0.0;
DiagMatDsptr momentOfInertias = std::make_shared<DiagonalMatrix<double>>(ListD{ 0.,0.,0. });
DiagMatDsptr momentOfInertias = std::make_shared<DiagonalMatrix>(ListD{ 0.,0.,0. });
};
}

2
OndselSolver/AnyPosICNewtonRaphson.cpp
View File

@@ -32,7 +32,7 @@ void AnyPosICNewtonRaphson::initializeGlobally()
void AnyPosICNewtonRaphson::createVectorsAndMatrices()
{
qsuOld = std::make_shared<FullColumn<double>>(nqsu);
qsuWeights = std::make_shared<DiagonalMatrix<double>>(nqsu);
qsuWeights = std::make_shared<DiagonalMatrix>(nqsu);
SystemNewtonRaphson::createVectorsAndMatrices();
}

26
OndselSolver/CADSystem.cpp
View File

@@ -87,7 +87,7 @@ void CADSystem::runOndselSinglePendulum()
auto assembly1 = CREATE<Part>::With("/Assembly1");
std::cout << "assembly1->name " << assembly1->name << std::endl;
assembly1->m = 0.0;
assembly1->aJ = std::make_shared<DiagonalMatrix<double>>(ListD{ 0, 0, 0 });
assembly1->aJ = std::make_shared<DiagonalMatrix>(ListD{ 0, 0, 0 });
qX = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
aAap = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
@@ -130,7 +130,7 @@ void CADSystem::runOndselSinglePendulum()
auto crankPart1 = CREATE<Part>::With("/Assembly1/Part1");
std::cout << "crankPart1->name " << crankPart1->name << std::endl;
crankPart1->m = 1.0;
crankPart1->aJ = std::make_shared<DiagonalMatrix<double>>(ListD{ 1, 1, 1 });
crankPart1->aJ = std::make_shared<DiagonalMatrix>(ListD{ 1, 1, 1 });
qX = std::make_shared<FullColumn<double>>(ListD{ 0.4, 0.0, -0.05 });
aAap = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
@@ -222,7 +222,7 @@ void CADSystem::runOndselDoublePendulum()
auto assembly1 = CREATE<Part>::With("/Assembly1");
std::cout << "assembly1->name " << assembly1->name << std::endl;
assembly1->m = 0.0;
assembly1->aJ = std::make_shared<DiagonalMatrix<double>>(ListD{ 0, 0, 0 });
assembly1->aJ = std::make_shared<DiagonalMatrix>(ListD{ 0, 0, 0 });
qX = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
aAap = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
@@ -265,7 +265,7 @@ void CADSystem::runOndselDoublePendulum()
auto crankPart1 = CREATE<Part>::With("/Assembly1/Part1");
std::cout << "crankPart1->name " << crankPart1->name << std::endl;
crankPart1->m = 1.0;
crankPart1->aJ = std::make_shared<DiagonalMatrix<double>>(ListD{ 1, 1, 1 });
crankPart1->aJ = std::make_shared<DiagonalMatrix>(ListD{ 1, 1, 1 });
qX = std::make_shared<FullColumn<double>>(ListD{ 0.4, 0.0, -0.05 });
aAap = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
@@ -307,7 +307,7 @@ void CADSystem::runOndselDoublePendulum()
auto conrodPart2 = CREATE<Part>::With("/Assembly1/Part2");
std::cout << "conrodPart2->name " << conrodPart2->name << std::endl;
conrodPart2->m = 1.0;
conrodPart2->aJ = std::make_shared<DiagonalMatrix<double>>(ListD{ 1, 1, 1 });
conrodPart2->aJ = std::make_shared<DiagonalMatrix>(ListD{ 1, 1, 1 });
qX = std::make_shared<FullColumn<double>>(ListD{ 0.15, 0.1, 0.05 });
qE = std::make_shared<FullColumn<double>>(ListD{ 0.0, 0.0, 1.0, 0.0 });
auto eulerParameters = CREATE<EulerParameters<double>>::With(ListD{ 0.0, 0.0, 1.0, 0.0 });
@@ -396,7 +396,7 @@ void CADSystem::runOndselPiston()
auto assembly1 = CREATE<Part>::With("/Assembly1");
std::cout << "assembly1->name " << assembly1->name << std::endl;
assembly1->m = 0.0;
assembly1->aJ = std::make_shared<DiagonalMatrix<double>>(ListD{ 0, 0, 0 });
assembly1->aJ = std::make_shared<DiagonalMatrix>(ListD{ 0, 0, 0 });
qX = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
qE = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0, 1 });
assembly1->setqX(qX);
@@ -441,7 +441,7 @@ void CADSystem::runOndselPiston()
auto crankPart1 = CREATE<Part>::With("/Assembly1/Part1");
std::cout << "crankPart1->name " << crankPart1->name << std::endl;
crankPart1->m = 1.0;
crankPart1->aJ = std::make_shared<DiagonalMatrix<double>>(ListD{ 1, 1, 1 });
crankPart1->aJ = std::make_shared<DiagonalMatrix>(ListD{ 1, 1, 1 });
qX = std::make_shared<FullColumn<double>>(ListD{ 0.4, 0.0, -0.05 });
qE = std::make_shared<FullColumn<double>>(ListD{ 0.0, 0.0, 0.0, 1.0 });
crankPart1->setqX(qX);
@@ -483,7 +483,7 @@ void CADSystem::runOndselPiston()
auto conrodPart2 = CREATE<Part>::With("/Assembly1/Part2");
std::cout << "conrodPart2->name " << conrodPart2->name << std::endl;
conrodPart2->m = 1.0;
conrodPart2->aJ = std::make_shared<DiagonalMatrix<double>>(ListD{ 1, 1, 1 });
conrodPart2->aJ = std::make_shared<DiagonalMatrix>(ListD{ 1, 1, 1 });
qX = std::make_shared<FullColumn<double>>(ListD{ 0.15, 0.1, 0.05 });
qE = std::make_shared<FullColumn<double>>(ListD{ 0.0, 0.0, 1.0, 0.0 });
conrodPart2->setqX(qX);
@@ -525,7 +525,7 @@ void CADSystem::runOndselPiston()
auto pistonPart3 = CREATE<Part>::With("/Assembly1/Part3");
std::cout << "pistonPart3->name " << pistonPart3->name << std::endl;
pistonPart3->m = 1.0;
pistonPart3->aJ = std::make_shared<DiagonalMatrix<double>>(ListD{ 1, 1, 1 });
pistonPart3->aJ = std::make_shared<DiagonalMatrix>(ListD{ 1, 1, 1 });
qX = std::make_shared<FullColumn<double>>(ListD{ -0.0, 1.5, 0.0 });
qE = std::make_shared<FullColumn<double>>(ListD{ 0.70710678118655, 0.70710678118655, 0.0, 0.0 });
pistonPart3->setqX(qX);
@@ -633,7 +633,7 @@ void CADSystem::runPiston()
auto assembly1 = CREATE<Part>::With("/Assembly1");
std::cout << "assembly1->name " << assembly1->name << std::endl;
assembly1->m = 0.0;
assembly1->aJ = std::make_shared<DiagonalMatrix<double>>(ListD{ 0, 0, 0 });
assembly1->aJ = std::make_shared<DiagonalMatrix>(ListD{ 0, 0, 0 });
qX = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
qE = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0, 1 });
assembly1->setqX(qX);
@@ -678,7 +678,7 @@ void CADSystem::runPiston()
auto crankPart1 = CREATE<Part>::With("/Assembly1/Part1");
std::cout << "crankPart1->name " << crankPart1->name << std::endl;
crankPart1->m = 0.045210530089461;
crankPart1->aJ = std::make_shared<DiagonalMatrix<double>>(ListD{ 1.7381980042084e-4, 0.003511159968501, 0.0036154518487535 });
crankPart1->aJ = std::make_shared<DiagonalMatrix>(ListD{ 1.7381980042084e-4, 0.003511159968501, 0.0036154518487535 });
qX = std::make_shared<FullColumn<double>>(ListD{ 0.38423368514246, 2.6661567755108e-17, -0.048029210642807 });
qE = std::make_shared<FullColumn<double>>(ListD{ 0.0, 0.0, 0.0, 1.0 });
crankPart1->setqX(qX);
@@ -720,7 +720,7 @@ void CADSystem::runPiston()
auto conrodPart2 = CREATE<Part>::With("/Assembly1/Part2");
std::cout << "conrodPart2->name " << conrodPart2->name << std::endl;
conrodPart2->m = 0.067815795134192;
conrodPart2->aJ = std::make_shared<DiagonalMatrix<double>>(ListD{ 2.6072970063126e-4, 0.011784982468533, 0.011941420288912 });
conrodPart2->aJ = std::make_shared<DiagonalMatrix>(ListD{ 2.6072970063126e-4, 0.011784982468533, 0.011941420288912 });
qX = std::make_shared<FullColumn<double>>(ListD{ 0.38423368514246, 0.49215295678475, 0.048029210642807 });
qE = std::make_shared<FullColumn<double>>(ListD{ 0.0, 0.0, 0.89871703427292, 0.43852900965351 });
conrodPart2->setqX(qX);
@@ -762,7 +762,7 @@ void CADSystem::runPiston()
auto pistonPart3 = CREATE<Part>::With("/Assembly1/Part3");
std::cout << "pistonPart3->name " << pistonPart3->name << std::endl;
pistonPart3->m = 1.730132083368;
pistonPart3->aJ = std::make_shared<DiagonalMatrix<double>>(ListD{ 0.19449049546716, 0.23028116340971, 0.23028116340971 });
pistonPart3->aJ = std::make_shared<DiagonalMatrix>(ListD{ 0.19449049546716, 0.23028116340971, 0.23028116340971 });
qX = std::make_shared<FullColumn<double>>(ListD{ -1.283972762056e-18, 1.4645980199976, -4.7652385308244e-17 });
qE = std::make_shared<FullColumn<double>>(ListD{ 0.70710678118655, 0.70710678118655, 0.0, 0.0 });
pistonPart3->setqX(qX);

2
OndselSolver/CMakeLists.txt
View File

@@ -5,6 +5,8 @@ project(OndselSolverLibrary VERSION 1.0.1 DESCRIPTION "Assembly Constraints and
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED True)
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -gdwarf-4")
include(GNUInstallDirs)
file(GLOB ONDSELSOLVER_SOURCES "*.cpp")

42
OndselSolver/DiagonalMatrix.cpp
View File

@@ -11,45 +11,36 @@
namespace MbD {
template<>
inline DiagMatDsptr DiagonalMatrix<double>::times(double factor)
DiagMatDsptr DiagonalMatrix::times(double factor)
{
auto nrow = (int)this->size();
auto answer = std::make_shared<DiagonalMatrix<double>>(nrow);
auto answer = std::make_shared<DiagonalMatrix>(nrow);
for (int i = 0; i < nrow; i++)
{
answer->at(i) = this->at(i) * factor;
}
return answer;
}
template<typename T>
inline void DiagonalMatrix<T>::atiputDiagonalMatrix(int i, std::shared_ptr<DiagonalMatrix<T>> diagMat)
void DiagonalMatrix::atiputDiagonalMatrix(int i, std::shared_ptr<DiagonalMatrix> diagMat)
{
for (int ii = 0; ii < diagMat->size(); ii++)
{
this->at(i + ii) = diagMat->at(ii);
}
}
template<typename T>
inline DiagMatsptr<T> DiagonalMatrix<T>::times(T factor)
{
assert(false);
}
template<typename T>
inline FColsptr<T> DiagonalMatrix<T>::timesFullColumn(FColsptr<T> fullCol)
FColsptr<double> DiagonalMatrix::timesFullColumn(FColsptr<double> fullCol)
{
//"a*b = a(i,j)b(j) sum j."
auto nrow = (int)this->size();
auto answer = std::make_shared<FullColumn<T>>(nrow);
auto answer = std::make_shared<FullColumn<double>>(nrow);
for (int i = 0; i < nrow; i++)
{
answer->at(i) = this->at(i) * fullCol->at(i);
}
return answer;
}
template<typename T>
inline FMatDsptr DiagonalMatrix<T>::timesFullMatrix(FMatDsptr fullMat)
FMatDsptr DiagonalMatrix::timesFullMatrix(FMatDsptr fullMat)
{
auto nrow = (int)this->size();
auto answer = std::make_shared<FullMatrixDouble>(nrow);
@@ -59,8 +50,7 @@ namespace MbD {
}
return answer;
}
template<>
inline double DiagonalMatrix<double>::sumOfSquares()
double DiagonalMatrix::sumOfSquares()
{
double sum = 0.0;
for (int i = 0; i < this->size(); i++)
@@ -70,21 +60,18 @@ namespace MbD {
}
return sum;
}
template<typename T>
inline int DiagonalMatrix<T>::numberOfElements()
int DiagonalMatrix::numberOfElements()
{
auto n = (int)this->size();
return n * n;
}
template<>
inline void DiagonalMatrix<double>::zeroSelf()
void DiagonalMatrix::zeroSelf()
{
for (int i = 0; i < this->size(); i++) {
this->at(i) = 0.0;
}
}
template<>
inline double DiagonalMatrix<double>::maxMagnitude()
double DiagonalMatrix::maxMagnitude()
{
double max = 0.0;
for (int i = 0; i < this->size(); i++)
@@ -95,14 +82,7 @@ namespace MbD {
}
return max;
}
template<typename T>
inline double DiagonalMatrix<T>::maxMagnitude()
{
assert(false);
return 0.0;
}
template<typename T>
inline std::ostream& DiagonalMatrix<T>::printOn(std::ostream& s) const
std::ostream& DiagonalMatrix::printOn(std::ostream& s) const
{
s << "DiagMat[";
s << this->at(0);

21
OndselSolver/DiagonalMatrix.h
View File

@@ -14,22 +14,19 @@
#include "DiagonalMatrix.ref.h"
#include "Array.h"
#include "FullColumn.h"
//#include "FullRow.h"
// #include "FullMatrix.h"
namespace MbD {
template<typename T>
class DiagonalMatrix : public Array<T>
class DiagonalMatrix : public Array<double>
{
//
public:
DiagonalMatrix() : Array<T>() {}
DiagonalMatrix(int count) : Array<T>(count) {}
DiagonalMatrix(int count, const T& value) : Array<T>(count, value) {}
DiagonalMatrix(std::initializer_list<T> list) : Array<T>{ list } {}
void atiputDiagonalMatrix(int i, std::shared_ptr<DiagonalMatrix<T>> diagMat);
DiagMatsptr<T> times(T factor);
FColsptr<T> timesFullColumn(FColsptr<T> fullCol);
DiagonalMatrix() : Array<double>() {}
explicit DiagonalMatrix(int count) : Array<double>(count) {}
DiagonalMatrix(int count, const double& value) : Array<double>(count, value) {}
DiagonalMatrix(std::initializer_list<double> list) : Array<double>{ list } {}
void atiputDiagonalMatrix(int i, std::shared_ptr<DiagonalMatrix> diagMat);
DiagMatDsptr times(double factor);
FColsptr<double> timesFullColumn(FColsptr<double> fullCol);
FMatDsptr timesFullMatrix(FMatDsptr fullMat);
int nrow() {
return (int)this->size();
@@ -44,7 +41,5 @@ namespace MbD {
std::ostream& printOn(std::ostream& s) const override;
};
template class DiagonalMatrix<double>;
}

6
OndselSolver/DiagonalMatrix.ref.h
View File

@@ -3,9 +3,7 @@
#include <memory>
namespace MbD {
template<typename T>
class DiagonalMatrix;
template<typename T>
using DiagMatsptr = std::shared_ptr<DiagonalMatrix<T>>;
using DiagMatDsptr = std::shared_ptr<DiagonalMatrix<double>>;
using DiagMatsptr = std::shared_ptr<DiagonalMatrix>;
using DiagMatDsptr = std::shared_ptr<DiagonalMatrix>;
}

645
OndselSolver/EndFrameqct.cpp
View File

@@ -9,7 +9,6 @@
#include "EndFrameqct.h"
#include "MarkerFrame.h"
#include "System.h"
#include "Symbolic.h"
#include "Time.h"
#include "EulerParameters.h"
#include "CREATE.h"
@@ -17,347 +16,309 @@
#include "EulerAngleszxzDot.h"
#include "EulerAngleszxzDDot.h"
using namespace MbD;
namespace MbD {
template class EulerParameters<double>;
EndFrameqct::EndFrameqct() {
EndFrameqct::EndFrameqct() {
}
EndFrameqct::EndFrameqct(const char *str) : EndFrameqc(str) {
}
void EndFrameqct::initialize() {
EndFrameqc::initialize();
rmem = std::make_shared<FullColumn<double>>(3);
prmempt = std::make_shared<FullColumn<double>>(3);
pprmemptpt = std::make_shared<FullColumn<double>>(3);
aAme = FullMatrixDouble::identitysptr(3);
pAmept = std::make_shared<FullMatrixDouble>(3, 3);
ppAmeptpt = std::make_shared<FullMatrixDouble>(3, 3);
pprOeOpEpt = std::make_shared<FullMatrixDouble>(3, 4);
pprOeOptpt = std::make_shared<FullColumn<double>>(3);
ppAOepEpt = std::make_shared<FullColumn<FMatDsptr>>(4);
ppAOeptpt = std::make_shared<FullMatrixDouble>(3, 3);
}
void EndFrameqct::initializeLocally() {
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 EndFrameqct::initprmemptBlks() {
auto &mbdTime = this->root()->time;
prmemptBlks = std::make_shared<FullColumn<Symsptr>>(3);
for (int i = 0; i < 3; i++) {
auto &disp = rmemBlks->at(i);
auto var = disp->differentiateWRT(mbdTime);
auto vel = var->simplified(var);
prmemptBlks->at(i) = vel;
}
}
void EndFrameqct::initpprmemptptBlks() {
auto &mbdTime = this->root()->time;
pprmemptptBlks = std::make_shared<FullColumn<Symsptr>>(3);
for (int i = 0; i < 3; i++) {
auto &vel = prmemptBlks->at(i);
auto var = vel->differentiateWRT(mbdTime);
auto acc = var->simplified(var);
pprmemptptBlks->at(i) = acc;
}
}
void EndFrameqct::initpPhiThePsiptBlks() {
auto &mbdTime = this->root()->time;
pPhiThePsiptBlks = std::make_shared<FullColumn<Symsptr>>(3);
for (int i = 0; i < 3; i++) {
auto &angle = phiThePsiBlks->at(i);
auto var = angle->differentiateWRT(mbdTime);
//std::cout << "var " << *var << std::endl;
auto vel = var->simplified(var);
//std::cout << "vel " << *vel << std::endl;
pPhiThePsiptBlks->at(i) = vel;
}
}
void EndFrameqct::initppPhiThePsiptptBlks() {
auto &mbdTime = this->root()->time;
ppPhiThePsiptptBlks = std::make_shared<FullColumn<Symsptr>>(3);
for (int i = 0; i < 3; i++) {
auto &angleVel = pPhiThePsiptBlks->at(i);
auto var = angleVel->differentiateWRT(mbdTime);
auto angleAcc = var->simplified(var);
ppPhiThePsiptptBlks->at(i) = angleAcc;
}
}
void EndFrameqct::postInput() {
this->evalrmem();
this->evalAme();
Item::postInput();
}
void EndFrameqct::calcPostDynCorrectorIteration() {
auto &rOmO = markerFrame->rOmO;
auto &aAOm = markerFrame->aAOm;
rOeO = rOmO->plusFullColumn(aAOm->timesFullColumn(rmem));
auto &prOmOpE = markerFrame->prOmOpE;
auto &pAOmpE = markerFrame->pAOmpE;
for (int i = 0; i < 3; i++) {
auto &prOmOpEi = prOmOpE->at(i);
auto &prOeOpEi = prOeOpE->at(i);
for (int j = 0; j < 4; j++) {
auto prOeOpEij = prOmOpEi->at(j) + pAOmpE->at(j)->at(i)->timesFullColumn(rmem);
prOeOpEi->at(j) = prOeOpEij;
}
}
auto rpep = markerFrame->rpmp->plusFullColumn(markerFrame->aApm->timesFullColumn(rmem));
pprOeOpEpE = EulerParameters<double>::ppApEpEtimesColumn(rpep);
aAOe = aAOm->timesFullMatrix(aAme);
for (int i = 0; i < 4; i++) {
pAOepE->at(i) = pAOmpE->at(i)->timesFullMatrix(aAme);
}
auto aApe = markerFrame->aApm->timesFullMatrix(aAme);
ppAOepEpE = EulerParameters<double>::ppApEpEtimesMatrix(aApe);
}
void EndFrameqct::prePosIC() {
time = this->root()->mbdTimeValue();
this->evalrmem();
this->evalAme();
EndFrameqc::prePosIC();
}
void EndFrameqct::evalrmem() {
if (rmemBlks) {
for (int i = 0; i < 3; i++) {
auto &expression = rmemBlks->at(i);
double value = expression->getValue();
rmem->at(i) = value;
}
}
}
void EndFrameqct::evalAme() {
if (phiThePsiBlks) {
auto phiThePsi = CREATE<EulerAngleszxz<double>>::With();
for (int i = 0; i < 3; i++) {
auto &expression = phiThePsiBlks->at(i);
auto value = expression->getValue();
phiThePsi->at(i) = value;
}
phiThePsi->calc();
aAme = phiThePsi->aA;
}
}
void EndFrameqct::preVelIC() {
time = this->root()->mbdTimeValue();
this->evalrmem();
this->evalAme();
Item::preVelIC();
this->evalprmempt();
this->evalpAmept();
auto &aAOm = markerFrame->aAOm;
prOeOpt = aAOm->timesFullColumn(prmempt);
pAOept = aAOm->timesFullMatrix(pAmept);
}
void EndFrameqct::postVelIC() {
auto &pAOmpE = markerFrame->pAOmpE;
for (int i = 0; i < 3; i++) {
auto &pprOeOpEpti = pprOeOpEpt->at(i);
for (int j = 0; j < 4; j++) {
auto pprOeOpEptij = pAOmpE->at(j)->at(i)->dot(prmempt);
pprOeOpEpti->atiput(j, pprOeOpEptij);
}
}
for (int i = 0; i < 4; i++) {
ppAOepEpt->atiput(i, pAOmpE->at(i)->timesFullMatrix(pAmept));
}
}
FColDsptr EndFrameqct::pAjOept(int j) {
return pAOept->column(j);
}
FMatDsptr EndFrameqct::ppAjOepETpt(int jj) {
auto answer = std::make_shared<FullMatrixDouble>(4, 3);
for (int i = 0; i < 4; i++) {
auto &answeri = answer->at(i);
auto &ppAOepEipt = ppAOepEpt->at(i);
for (int j = 0; j < 3; j++) {
auto &answerij = ppAOepEipt->at(j)->at(jj);
answeri->atiput(j, answerij);
}
}
return answer;
}
FColDsptr EndFrameqct::ppAjOeptpt(int j) {
return ppAOeptpt->column(j);
}
double EndFrameqct::priOeOpt(int i) {
return prOeOpt->at(i);
}
FRowDsptr EndFrameqct::ppriOeOpEpt(int i) {
return pprOeOpEpt->at(i);
}
double EndFrameqct::ppriOeOptpt(int i) {
return pprOeOptpt->at(i);
}
void EndFrameqct::evalprmempt() {
if (rmemBlks) {
for (int i = 0; i < 3; i++) {
auto &derivative = prmemptBlks->at(i);
auto value = derivative->getValue();
prmempt->at(i) = value;
}
}
}
void EndFrameqct::evalpAmept() {
if (phiThePsiBlks) {
auto phiThePsi = CREATE<EulerAngleszxz<double>>::With();
auto phiThePsiDot = CREATE<EulerAngleszxzDot<double>>::With();
phiThePsiDot->phiThePsi = phiThePsi;
for (int i = 0; i < 3; i++) {
auto &expression = phiThePsiBlks->at(i);
auto &derivative = pPhiThePsiptBlks->at(i);
auto value = expression->getValue();
auto valueDot = derivative->getValue();
phiThePsi->at(i) = value;
phiThePsiDot->at(i) = valueDot;
}
phiThePsi->calc();
phiThePsiDot->calc();
pAmept = phiThePsiDot->aAdot;
}
}
void EndFrameqct::evalpprmemptpt() {
if (rmemBlks) {
for (int i = 0; i < 3; i++) {
auto &secondDerivative = pprmemptptBlks->at(i);
auto value = secondDerivative->getValue();
pprmemptpt->atiput(i, value);
}
}
}
void EndFrameqct::evalppAmeptpt() {
if (phiThePsiBlks) {
auto phiThePsi = CREATE<EulerAngleszxz<double>>::With();
auto phiThePsiDot = CREATE<EulerAngleszxzDot<double>>::With();
phiThePsiDot->phiThePsi = phiThePsi;
auto phiThePsiDDot = CREATE<EulerAngleszxzDDot<double>>::With();
phiThePsiDDot->phiThePsiDot = phiThePsiDot;
for (int i = 0; i < 3; i++) {
auto &expression = phiThePsiBlks->at(i);
auto &derivative = pPhiThePsiptBlks->at(i);
auto &secondDerivative = ppPhiThePsiptptBlks->at(i);
auto value = expression->getValue();
auto valueDot = derivative->getValue();
auto valueDDot = secondDerivative->getValue();
phiThePsi->atiput(i, value);
phiThePsiDot->atiput(i, valueDot);
phiThePsiDDot->atiput(i, valueDDot);
}
phiThePsi->calc();
phiThePsiDot->calc();
phiThePsiDDot->calc();
ppAmeptpt = phiThePsiDDot->aAddot;
}
}
FColDsptr EndFrameqct::rmeO() {
return markerFrame->aAOm->timesFullColumn(rmem);
}
FColDsptr EndFrameqct::rpep() {
auto &rpmp = markerFrame->rpmp;
auto &aApm = markerFrame->aApm;
auto rpep = rpmp->plusFullColumn(aApm->timesFullColumn(rmem));
return rpep;
}
void EndFrameqct::preAccIC() {
time = this->root()->mbdTimeValue();
this->evalrmem();
this->evalAme();
Item::preVelIC();
this->evalprmempt();
this->evalpAmept();
auto &aAOm = markerFrame->aAOm;
prOeOpt = aAOm->timesFullColumn(prmempt);
pAOept = aAOm->timesFullMatrix(pAmept);
Item::preAccIC();
this->evalpprmemptpt();
this->evalppAmeptpt();
aAOm = markerFrame->aAOm;
pprOeOptpt = aAOm->timesFullColumn(pprmemptpt);
ppAOeptpt = aAOm->timesFullMatrix(ppAmeptpt);
}
}
EndFrameqct::EndFrameqct(const char* str) : EndFrameqc(str) {
}
void EndFrameqct::initialize()
{
EndFrameqc::initialize();
rmem = std::make_shared<FullColumn<double>>(3);
prmempt = std::make_shared<FullColumn<double>>(3);
pprmemptpt = std::make_shared<FullColumn<double>>(3);
aAme = FullMatrixDouble::identitysptr(3);
pAmept = std::make_shared<FullMatrixDouble>(3, 3);
ppAmeptpt = std::make_shared<FullMatrixDouble>(3, 3);
pprOeOpEpt = std::make_shared<FullMatrixDouble>(3, 4);
pprOeOptpt = std::make_shared<FullColumn<double>>(3);
ppAOepEpt = std::make_shared<FullColumn<FMatDsptr>>(4);
ppAOeptpt = std::make_shared<FullMatrixDouble>(3, 3);
}
void EndFrameqct::initializeLocally()
{
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 EndFrameqct::initprmemptBlks()
{
auto& mbdTime = this->root()->time;
prmemptBlks = std::make_shared< FullColumn<Symsptr>>(3);
for (int i = 0; i < 3; i++) {
auto& disp = rmemBlks->at(i);
auto var = disp->differentiateWRT(mbdTime);
auto vel = var->simplified(var);
prmemptBlks->at(i) = vel;
}
}
void EndFrameqct::initpprmemptptBlks()
{
auto& mbdTime = this->root()->time;
pprmemptptBlks = std::make_shared< FullColumn<Symsptr>>(3);
for (int i = 0; i < 3; i++) {
auto& vel = prmemptBlks->at(i);
auto var = vel->differentiateWRT(mbdTime);
auto acc = var->simplified(var);
pprmemptptBlks->at(i) = acc;
}
}
void EndFrameqct::initpPhiThePsiptBlks()
{
auto& mbdTime = this->root()->time;
pPhiThePsiptBlks = std::make_shared< FullColumn<Symsptr>>(3);
for (int i = 0; i < 3; i++) {
auto& angle = phiThePsiBlks->at(i);
auto var = angle->differentiateWRT(mbdTime);
//std::cout << "var " << *var << std::endl;
auto vel = var->simplified(var);
//std::cout << "vel " << *vel << std::endl;
pPhiThePsiptBlks->at(i) = vel;
}
}
void EndFrameqct::initppPhiThePsiptptBlks()
{
auto& mbdTime = this->root()->time;
ppPhiThePsiptptBlks = std::make_shared< FullColumn<Symsptr>>(3);
for (int i = 0; i < 3; i++) {
auto& angleVel = pPhiThePsiptBlks->at(i);
auto var = angleVel->differentiateWRT(mbdTime);
auto angleAcc = var->simplified(var);
ppPhiThePsiptptBlks->at(i) = angleAcc;
}
}
void EndFrameqct::postInput()
{
this->evalrmem();
this->evalAme();
Item::postInput();
}
void EndFrameqct::calcPostDynCorrectorIteration()
{
auto& rOmO = markerFrame->rOmO;
auto& aAOm = markerFrame->aAOm;
rOeO = rOmO->plusFullColumn(aAOm->timesFullColumn(rmem));
auto& prOmOpE = markerFrame->prOmOpE;
auto& pAOmpE = markerFrame->pAOmpE;
for (int i = 0; i < 3; i++)
{
auto& prOmOpEi = prOmOpE->at(i);
auto& prOeOpEi = prOeOpE->at(i);
for (int j = 0; j < 4; j++)
{
auto prOeOpEij = prOmOpEi->at(j) + pAOmpE->at(j)->at(i)->timesFullColumn(rmem);
prOeOpEi->at(j) = prOeOpEij;
}
}
auto rpep = markerFrame->rpmp->plusFullColumn(markerFrame->aApm->timesFullColumn(rmem));
pprOeOpEpE = EulerParameters<double>::ppApEpEtimesColumn(rpep);
aAOe = aAOm->timesFullMatrix(aAme);
for (int i = 0; i < 4; i++)
{
pAOepE->at(i) = pAOmpE->at(i)->timesFullMatrix(aAme);
}
auto aApe = markerFrame->aApm->timesFullMatrix(aAme);
ppAOepEpE = EulerParameters<double>::ppApEpEtimesMatrix(aApe);
}
void EndFrameqct::prePosIC()
{
time = this->root()->mbdTimeValue();
this->evalrmem();
this->evalAme();
EndFrameqc::prePosIC();
}
void EndFrameqct::evalrmem()
{
if (rmemBlks) {
for (int i = 0; i < 3; i++)
{
auto& expression = rmemBlks->at(i);
double value = expression->getValue();
rmem->at(i) = value;
}
}
}
void EndFrameqct::evalAme()
{
if (phiThePsiBlks) {
auto phiThePsi = CREATE<EulerAngleszxz<double>>::With();
for (int i = 0; i < 3; i++)
{
auto& expression = phiThePsiBlks->at(i);
auto value = expression->getValue();
phiThePsi->at(i) = value;
}
phiThePsi->calc();
aAme = phiThePsi->aA;
}
}
void EndFrameqct::preVelIC()
{
time = this->root()->mbdTimeValue();
this->evalrmem();
this->evalAme();
Item::preVelIC();
this->evalprmempt();
this->evalpAmept();
auto& aAOm = markerFrame->aAOm;
prOeOpt = aAOm->timesFullColumn(prmempt);
pAOept = aAOm->timesFullMatrix(pAmept);
}
void EndFrameqct::postVelIC()
{
auto& pAOmpE = markerFrame->pAOmpE;
for (int i = 0; i < 3; i++)
{
auto& pprOeOpEpti = pprOeOpEpt->at(i);
for (int j = 0; j < 4; j++)
{
auto pprOeOpEptij = pAOmpE->at(j)->at(i)->dot(prmempt);
pprOeOpEpti->atiput(j, pprOeOpEptij);
}
}
for (int i = 0; i < 4; i++)
{
ppAOepEpt->atiput(i, pAOmpE->at(i)->timesFullMatrix(pAmept));
}
}
FColDsptr EndFrameqct::pAjOept(int j)
{
return pAOept->column(j);
}
FMatDsptr EndFrameqct::ppAjOepETpt(int jj)
{
auto answer = std::make_shared<FullMatrixDouble>(4, 3);
for (int i = 0; i < 4; i++)
{
auto& answeri = answer->at(i);
auto& ppAOepEipt = ppAOepEpt->at(i);
for (int j = 0; j < 3; j++)
{
auto& answerij = ppAOepEipt->at(j)->at(jj);
answeri->atiput(j, answerij);
}
}
return answer;
}
FColDsptr EndFrameqct::ppAjOeptpt(int j)
{
return ppAOeptpt->column(j);
}
double EndFrameqct::priOeOpt(int i)
{
return prOeOpt->at(i);
}
FRowDsptr EndFrameqct::ppriOeOpEpt(int i)
{
return pprOeOpEpt->at(i);
}
double EndFrameqct::ppriOeOptpt(int i)
{
return pprOeOptpt->at(i);
}
void EndFrameqct::evalprmempt()
{
if (rmemBlks) {
for (int i = 0; i < 3; i++)
{
auto& derivative = prmemptBlks->at(i);
auto value = derivative->getValue();
prmempt->at(i) = value;
}
}
}
void EndFrameqct::evalpAmept()
{
if (phiThePsiBlks) {
auto phiThePsi = CREATE<EulerAngleszxz<double>>::With();
auto phiThePsiDot = CREATE<EulerAngleszxzDot<double>>::With();
phiThePsiDot->phiThePsi = phiThePsi;
for (int i = 0; i < 3; i++)
{
auto& expression = phiThePsiBlks->at(i);
auto& derivative = pPhiThePsiptBlks->at(i);
auto value = expression->getValue();
auto valueDot = derivative->getValue();
phiThePsi->at(i) = value;
phiThePsiDot->at(i) = valueDot;
}
phiThePsi->calc();
phiThePsiDot->calc();
pAmept = phiThePsiDot->aAdot;
}
}
void EndFrameqct::evalpprmemptpt()
{
if (rmemBlks) {
for (int i = 0; i < 3; i++)
{
auto& secondDerivative = pprmemptptBlks->at(i);
auto value = secondDerivative->getValue();
pprmemptpt->atiput(i, value);
}
}
}
void EndFrameqct::evalppAmeptpt()
{
if (phiThePsiBlks) {
auto phiThePsi = CREATE<EulerAngleszxz<double>>::With();
auto phiThePsiDot = CREATE<EulerAngleszxzDot<double>>::With();
phiThePsiDot->phiThePsi = phiThePsi;
auto phiThePsiDDot = CREATE<EulerAngleszxzDDot<double>>::With();
phiThePsiDDot->phiThePsiDot = phiThePsiDot;
for (int i = 0; i < 3; i++)
{
auto& expression = phiThePsiBlks->at(i);
auto& derivative = pPhiThePsiptBlks->at(i);
auto& secondDerivative = ppPhiThePsiptptBlks->at(i);
auto value = expression->getValue();
auto valueDot = derivative->getValue();
auto valueDDot = secondDerivative->getValue();
phiThePsi->atiput(i, value);
phiThePsiDot->atiput(i, valueDot);
phiThePsiDDot->atiput(i, valueDDot);
}
phiThePsi->calc();
phiThePsiDot->calc();
phiThePsiDDot->calc();
ppAmeptpt = phiThePsiDDot->aAddot;
}
}
FColDsptr EndFrameqct::rmeO()
{
return markerFrame->aAOm->timesFullColumn(rmem);
}
FColDsptr EndFrameqct::rpep()
{
auto& rpmp = markerFrame->rpmp;
auto& aApm = markerFrame->aApm;
auto rpep = rpmp->plusFullColumn(aApm->timesFullColumn(rmem));
return rpep;
}
void EndFrameqct::preAccIC()
{
time = this->root()->mbdTimeValue();
this->evalrmem();
this->evalAme();
Item::preVelIC();
this->evalprmempt();
this->evalpAmept();
auto& aAOm = markerFrame->aAOm;
prOeOpt = aAOm->timesFullColumn(prmempt);
pAOept = aAOm->timesFullMatrix(pAmept);
Item::preAccIC();
this->evalpprmemptpt();
this->evalppAmeptpt();
aAOm = markerFrame->aAOm;
pprOeOptpt = aAOm->timesFullColumn(pprmemptpt);
ppAOeptpt = aAOm->timesFullMatrix(ppAmeptpt);
}

1
OndselSolver/EndFrameqct.h
View File

@@ -58,7 +58,6 @@ namespace MbD {
FMatDsptr aAme, pAmept, ppAmeptpt, pAOept, ppAOeptpt;
FMatDsptr pprOeOpEpt;
FColFMatDsptr ppAOepEpt;
};
}

2
OndselSolver/EndFrameqct2.cpp
View File

@@ -29,7 +29,7 @@ void EndFrameqct2::initpPhiThePsiptBlks()
{
auto& mbdTime = this->root()->time;
auto eulerAngles = std::static_pointer_cast<EulerAngles<Symsptr>>(phiThePsiBlks);
pPhiThePsiptBlks = eulerAngles->differentiateWRT(mbdTime);
pPhiThePsiptBlks = differentiateWRT(*eulerAngles, mbdTime);
}
void EndFrameqct2::initppPhiThePsiptptBlks()

21
OndselSolver/EulerAngles.cpp
View File

@@ -66,16 +66,6 @@ namespace MbD {
assert(false);
}
template<typename T>
inline std::shared_ptr<EulerAnglesDot<T>> EulerAngles<T>::differentiateWRT(T var)
{
auto derivatives = std::make_shared<EulerAnglesDot<T>>();
std::transform(this->begin(), this->end(), derivatives->begin(),
[var](T term) { return term->differentiateWRT(var); }
);
derivatives->aEulerAngles = this;
return derivatives;
}
template<typename T>
inline void EulerAngles<T>::setRotOrder(int i, int j, int k)
{
rotOrder = std::make_shared<FullColumn<int>>(3);
@@ -83,6 +73,15 @@ namespace MbD {
rotOrder->at(1) = j;
rotOrder->at(2) = k;
}
template class EulerAngles<std::shared_ptr<MbD::Symbolic>>;
// type-specific helper functions
std::shared_ptr<EulerAnglesDot<std::shared_ptr<MbD::Symbolic>>> differentiateWRT(EulerAngles<std::shared_ptr<MbD::Symbolic>>& ref, std::shared_ptr<MbD::Symbolic> var)
{
auto derivatives = std::make_shared<EulerAnglesDot<std::shared_ptr<MbD::Symbolic>>>();
std::transform(ref.begin(), ref.end(), derivatives->begin(),
[var](std::shared_ptr<MbD::Symbolic> term) { return term->differentiateWRT(var); }
);
derivatives->aEulerAngles = &ref;
return derivatives;
}
}

7
OndselSolver/EulerAngles.h
View File

@@ -27,15 +27,14 @@ namespace MbD {
EulerAngles(std::initializer_list<T> list) : EulerArray<T>{ list } {}
void initialize() override;
void calc() override;
std::shared_ptr<EulerAnglesDot<T>> differentiateWRT(T var);
void setRotOrder(int i, int j, int k);
std::shared_ptr<FullColumn<int>> rotOrder;
FColFMatDsptr cA;
FMatDsptr aA;
};
// // NOTE: do NOT instantiate EulerAngles<double> as a whole as differentiateWRT breaks
// template <>
// void EulerAngles<double>::setRotOrder(int, int, int);
template class EulerAngles<std::shared_ptr<MbD::Symbolic>>;
template class EulerAngles<double>;
std::shared_ptr<EulerAnglesDot<std::shared_ptr<MbD::Symbolic>>> differentiateWRT(EulerAngles<std::shared_ptr<MbD::Symbolic>>& ref, std::shared_ptr<MbD::Symbolic> var);
}

22
OndselSolver/EulerParameters.cpp
View File

@@ -13,7 +13,7 @@
namespace MbD {
template<>
inline FMatFColDsptr EulerParameters<double>::ppApEpEtimesColumn(FColDsptr col)
FMatFColDsptr EulerParameters<double>::ppApEpEtimesColumn(FColDsptr col)
{
double a2c0 = 2 * col->at(0);
double a2c1 = 2 * col->at(1);
@@ -56,7 +56,7 @@ namespace MbD {
}
template<>
inline FMatDsptr EulerParameters<double>::pCpEtimesColumn(FColDsptr col)
FMatDsptr EulerParameters<double>::pCpEtimesColumn(FColDsptr col)
{
//"col size = 4."
auto c0 = col->at(0);
@@ -86,7 +86,7 @@ namespace MbD {
}
template<typename T>
inline FMatDsptr EulerParameters<T>::pCTpEtimesColumn(FColDsptr col)
FMatDsptr EulerParameters<T>::pCTpEtimesColumn(FColDsptr col)
{
//"col size = 3."
auto c0 = col->at(0);
@@ -120,7 +120,7 @@ namespace MbD {
}
template<>
inline FMatFMatDsptr EulerParameters<double>::ppApEpEtimesMatrix(FMatDsptr mat)
FMatFMatDsptr EulerParameters<double>::ppApEpEtimesMatrix(FMatDsptr mat)
{
FRowDsptr a2m0 = mat->at(0)->times(2.0);
FRowDsptr a2m1 = mat->at(1)->times(2.0);
@@ -164,7 +164,7 @@ namespace MbD {
}
template<typename T> // this is ALWAYS double; see note below.
inline void EulerParameters<T>::initialize()
void EulerParameters<T>::initialize()
{
aA = FullMatrixDouble::identitysptr(3);
aB = std::make_shared<FullMatrixDouble>(3, 4);
@@ -179,18 +179,18 @@ namespace MbD {
// the following can't be valid as FullMatrix instatiatiates <double>, yet
// this class needs to see the member functions of FullMatrix
//template<>
//inline void EulerParameters<double>::initialize()
//void EulerParameters<double>::initialize()
//{
//}
template<typename T>
inline void EulerParameters<T>::calc()
void EulerParameters<T>::calc()
{
this->calcABC();
this->calcpApE();
}
template<>
inline void EulerParameters<double>::calcABC()
void EulerParameters<double>::calcABC()
{
double aE0 = this->at(0);
double aE1 = this->at(1);
@@ -235,7 +235,7 @@ namespace MbD {
aA = aB->timesTransposeFullMatrix(aC);
}
template<>
inline void EulerParameters<double>::calcpApE()
void EulerParameters<double>::calcpApE()
{
double a2E0 = 2.0 * (this->at(0));
double a2E1 = 2.0 * (this->at(1));
@@ -304,9 +304,11 @@ namespace MbD {
pAipEk->at(2) = a2E3;
}
template<typename T>
inline void EulerParameters<T>::conditionSelf()
void EulerParameters<T>::conditionSelf()
{
EulerArray<T>::conditionSelf();
this->normalizeSelf();
}
template class EulerParameters<double>;
}

2
OndselSolver/EulerParameters.h
View File

@@ -59,6 +59,4 @@ namespace MbD {
FMatDsptr aC;
FColFMatDsptr pApE;
};
template class EulerParameters<double>;
}

4
OndselSolver/FullMatrix.cpp
View File

@@ -665,10 +665,10 @@ namespace MbD {
return false;
}
}
std::shared_ptr<DiagonalMatrix<double>> FullMatrixDouble::asDiagonalMatrix()
std::shared_ptr<DiagonalMatrix> FullMatrixDouble::asDiagonalMatrix()
{
int nrow = this->nrow();
auto diagMat = std::make_shared<DiagonalMatrix<double>>(nrow);
auto diagMat = std::make_shared<DiagonalMatrix>(nrow);
for (int i = 0; i < nrow; i++)
{
diagMat->atiput(i, this->at(i)->at(i));

2
OndselSolver/FullMatrix.h
View File

@@ -93,7 +93,7 @@ namespace MbD {
double trace();
bool isDiagonal();
bool isDiagonalToWithin(double ratio);
std::shared_ptr<DiagonalMatrix<double>> asDiagonalMatrix();
std::shared_ptr<DiagonalMatrix> asDiagonalMatrix();
void conditionSelfWithTol(double tol);
std::ostream& printOn(std::ostream& s) const override;
FColsptr<double> timesFullColumn(FColsptr<double> fullCol);

47
OndselSolver/MBDynNode.cpp
View File

@@ -20,29 +20,28 @@ void MbD::MBDynNode::outputLine(int i, std::ostream& os)
auto x = asmtPart->xs->at(i);
auto y = asmtPart->ys->at(i);
auto z = asmtPart->zs->at(i);
// TODO: undo the breaking I just did on purpose
// auto bryantAngles = std::make_shared<EulerAngles<double>>();
// bryantAngles->setRotOrder(1, 2, 3);
// bryantAngles->at(0) = asmtPart->bryxs->at(i);
// bryantAngles->at(1) = asmtPart->bryys->at(i);
// bryantAngles->at(2) = asmtPart->bryzs->at(i);
// bryantAngles->calc();
// auto aA = bryantAngles->aA;
// auto vx = asmtPart->vxs->at(i);
// auto vy = asmtPart->vys->at(i);
// auto vz = asmtPart->vzs->at(i);
// auto omex = asmtPart->omexs->at(i);
// auto omey = asmtPart->omeys->at(i);
// auto omez = asmtPart->omezs->at(i);
// os << id << " ";
// os << x << " " << y << " " << z << " ";
// auto row = aA->at(0);
// os << row->at(0) << " " << row->at(1) << " " << row->at(2) << " ";
// row = aA->at(1);
// os << row->at(0) << " " << row->at(1) << " " << row->at(2) << " ";
// row = aA->at(2);
// os << row->at(0) << " " << row->at(1) << " " << row->at(2) << " ";
// os << vx << " " << vy << " " << vz << " ";
// os << omex << " " << omey << " " << omez << " ";
auto bryantAngles = std::make_shared<EulerAngles<double>>();
bryantAngles->setRotOrder(1, 2, 3);
bryantAngles->at(0) = asmtPart->bryxs->at(i);
bryantAngles->at(1) = asmtPart->bryys->at(i);
bryantAngles->at(2) = asmtPart->bryzs->at(i);
bryantAngles->calc();
auto aA = bryantAngles->aA;
auto vx = asmtPart->vxs->at(i);
auto vy = asmtPart->vys->at(i);
auto vz = asmtPart->vzs->at(i);
auto omex = asmtPart->omexs->at(i);
auto omey = asmtPart->omeys->at(i);
auto omez = asmtPart->omezs->at(i);
os << id << " ";
os << x << " " << y << " " << z << " ";
auto row = aA->at(0);
os << row->at(0) << " " << row->at(1) << " " << row->at(2) << " ";
row = aA->at(1);
os << row->at(0) << " " << row->at(1) << " " << row->at(2) << " ";
row = aA->at(2);
os << row->at(0) << " " << row->at(1) << " " << row->at(2) << " ";
os << vx << " " << vy << " " << vz << " ";
os << omex << " " << omey << " " << omez << " ";
os << std::endl;
}

8
OndselSolver/MomentOfInertiaSolver.cpp
View File

@@ -295,7 +295,7 @@ void MbD::MomentOfInertiaSolver::calcJppFromDiagJcmP()
//"Eigenvalues are orders from smallest to largest."
double average;
auto sortedJ = std::make_shared<DiagonalMatrix<double>>();
auto sortedJ = std::make_shared<DiagonalMatrix>();
sortedJ->push_back(aJcmP->at(0)->at(0));
sortedJ->push_back(aJcmP->at(1)->at(1));
sortedJ->push_back(aJcmP->at(2)->at(2));
@@ -326,7 +326,7 @@ void MbD::MomentOfInertiaSolver::calcJppFromDiagJcmP()
lam2 = average;
}
}
aJpp = std::make_shared<DiagonalMatrix<double>>(ListD{ lam0, lam1, lam2 });
aJpp = std::make_shared<DiagonalMatrix>(ListD{ lam0, lam1, lam2 });
}
void MbD::MomentOfInertiaSolver::calcJppFromFullJcmP()
@@ -351,7 +351,7 @@ void MbD::MomentOfInertiaSolver::calcJppFromFullJcmP()
auto phiDiv3 = modifiedArcCos(-q / std::sqrt(-p * p * p)) / 3.0;
auto twoSqrtMinusp = 2.0 * std::sqrt(-p);
auto piDiv3 = M_PI / 3.0;
auto sortedJ = std::make_shared<DiagonalMatrix<double>>();
auto sortedJ = std::make_shared<DiagonalMatrix>();
sortedJ->push_back(twoSqrtMinusp * std::cos(phiDiv3));
sortedJ->push_back(twoSqrtMinusp * -std::cos(phiDiv3 + piDiv3));
sortedJ->push_back(twoSqrtMinusp * -std::cos(phiDiv3 - piDiv3));
@@ -382,7 +382,7 @@ void MbD::MomentOfInertiaSolver::calcJppFromFullJcmP()
lam2 = average;
}
}
aJpp = std::make_shared<DiagonalMatrix<double>>(ListD{ lam0, lam1, lam2 });
aJpp = std::make_shared<DiagonalMatrix>(ListD{ lam0, lam1, lam2 });
}
double MbD::MomentOfInertiaSolver::modifiedArcCos(double val)

26
OndselSolver/Part.cpp
View File

@@ -10,9 +10,11 @@
#include "PartFrame.h"
#include "System.h"
#include "CREATE.h"
#include "DiagonalMatrix.h"
#include "EulerParameters.h"
#include "PosVelAccData.h"
#include "FullColumn.h"
#include "FullMatrix.h"
#include "DiagonalMatrix.h"
using namespace MbD;
@@ -41,10 +43,10 @@ void Part::initializeLocally()
{
partFrame->initializeLocally();
if (m > 0) {
mX = std::make_shared<DiagonalMatrix<double>>(3, m);
mX = std::make_shared<DiagonalMatrix>(3, m);
}
else {
mX = std::make_shared<DiagonalMatrix<double>>(3, 0.0);
mX = std::make_shared<DiagonalMatrix>(3, 0.0);
}
}
@@ -269,8 +271,8 @@ void Part::fillqsuWeights(DiagMatDsptr diagMat)
auto aJiMax = this->root()->maximumMomentOfInertia();
double minw = 1.0e3;
double maxw = 1.0e6;
auto wqX = std::make_shared<DiagonalMatrix<double>>(3);
auto wqE = std::make_shared<DiagonalMatrix<double>>(4);
auto wqX = std::make_shared<DiagonalMatrix>(3);
auto wqE = std::make_shared<DiagonalMatrix>(4);
if (mMax == 0) { mMax = 1.0; }
for (int i = 0; i < 3; i++)
{
@@ -317,8 +319,8 @@ void Part::fillqsudotWeights(DiagMatDsptr diagMat)
if (maxInertia == 0) maxInertia = 1.0;
double minw = 1.0e-12 * maxInertia;
double maxw = maxInertia;
auto wqXdot = std::make_shared<DiagonalMatrix<double>>(3);
auto wqEdot = std::make_shared<DiagonalMatrix<double>>(4);
auto wqXdot = std::make_shared<DiagonalMatrix>(3);
auto wqEdot = std::make_shared<DiagonalMatrix>(4);
for (int i = 0; i < 3; i++)
{
wqXdot->at(i) = (maxw * m / maxInertia) + minw;
@@ -449,11 +451,11 @@ void Part::calcp()
pE = mE->timesFullColumn(partFrame->qEdot);
}
void Part::calcpdot()
{
pXdot = mX->timesFullColumn(partFrame->qXddot);
pEdot = mEdot->timesFullColumn(partFrame->qEdot)->plusFullColumn(mE->timesFullColumn(partFrame->qEddot));
}
//void Part::calcpdot()
//{
// pXdot = mX->timesFullColumn(partFrame->qXddot);
// pEdot = mEdot->timesFullColumn(partFrame->qEdot)->plusFullColumn(mE->timesFullColumn(partFrame->qEddot));
//}
void Part::calcmEdot()
{

8
OndselSolver/Part.h
View File

@@ -11,14 +11,14 @@
#include <memory>
#include "Item.h"
#include "FullColumn.h"
#include "FullMatrix.h"
#include "FullColumn.ref.h"
#include "FullMatrix.ref.h"
#include "DiagonalMatrix.ref.h"
#include "EulerParametersDot.h"
namespace MbD {
class System;
class PartFrame;
template<typename T> class DiagonalMatrix;
class Part : public Item
{
@@ -100,7 +100,7 @@ namespace MbD {
void fillVelICJacob(SpMatDsptr mat) override;
void preAccIC() override;
void calcp();
void calcpdot();
// void calcpdot();
void calcmEdot();
void calcpTpE();
void calcppTpEpE();

2
OndselSolver/VelICSolver.cpp
View File

@@ -59,7 +59,7 @@ void VelICSolver::runBasic()
this->assignEquationNumbers();
system->partsJointsMotionsDo([](std::shared_ptr<Item> item) { item->useEquationNumbers(); });
auto qsudotOld = std::make_shared<FullColumn<double>>(nqsu);
auto qsudotWeights = std::make_shared<DiagonalMatrix<double>>(nqsu);
auto qsudotWeights = std::make_shared<DiagonalMatrix>(nqsu);
errorVector = std::make_shared<FullColumn<double>>(n);
jacobian = std::make_shared<SparseMatrix<double>>(n, n);
system->partsJointsMotionsDo([&](std::shared_ptr<Item> item) { item->fillqsudot(qsudotOld); });