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

Merge pull request #26 from Ondsel-Development/john-test

Browse Source 
Refactoring for clang/gcc/linux/mac/windows
This commit is contained in:
aiksiongkoh
2023-11-09 09:18:42 -07:00
committed by GitHub
parent 654c07a68e e2c4510e92
commit 0891112511
97 changed files with 2174 additions and 2061 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
CMakeLists.txt
View File

@@ -7,7 +7,22 @@ set(CMAKE_CXX_STANDARD_REQUIRED True)
include(GNUInstallDirs)
add_library(OndselSolver STATIC)
if( UNIX )
set( ONDSELSOLVER_BUILD_SHARED ON )
ELSEIF ( APPLE )
set( ONDSELSOLVER_BUILD_SHARED ON )
ELSE()
set( ONDSELSOLVER_BUILD_SHARED OFF )
ENDIF ()
if ( ONDSELSOLVER_BUILD_SHARED )
message( STATUS "[OndselSolver] Building shared library" )
add_library(OndselSolver SHARED)
else()
message( STATUS "[OndselSolver] Building static library" )
add_library(OndselSolver STATIC)
endif()
set(ONDSELSOLVER_SRC
OndselSolver/Array.cpp
@@ -601,4 +616,4 @@ install(TARGETS OndselSolver
LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/OndselSolver)
install(FILES ${CMAKE_BINARY_DIR}/OndselSolver.pc
DESTINATION ${CMAKE_INSTALL_DATAROOTDIR}/pkgconfig)
DESTINATION ${CMAKE_INSTALL_DATAROOTDIR}/pkgconfig)

20
OndselSolver/ASMTAssembly.cpp
View File

@@ -248,7 +248,7 @@ void MbD::ASMTAssembly::runSinglePendulum()
assembly->setName(str);
auto pos3D = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
assembly->setPosition3D(pos3D);
auto rotMat = std::make_shared<FullMatrix<double>>(ListListD{
auto rotMat = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
{ 0, 1, 0 },
{ 0, 0, 1 }
@@ -262,11 +262,11 @@ 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);
rotMat = std::make_shared<FullMatrix<double>>(ListListD{
rotMat = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
{ 0, 1, 0 },
{ 0, 0, 1 }
@@ -279,7 +279,7 @@ void MbD::ASMTAssembly::runSinglePendulum()
mkr->setName(str);
pos3D = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
mkr->setPosition3D(pos3D);
rotMat = std::make_shared<FullMatrix<double>>(ListListD{
rotMat = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
{ 0, 1, 0 },
{ 0, 0, 1 }
@@ -292,7 +292,7 @@ void MbD::ASMTAssembly::runSinglePendulum()
part->setName(str);
pos3D = std::make_shared<FullColumn<double>>(ListD{ -0.1, -0.1, -0.1 });
part->setPosition3D(pos3D);
rotMat = std::make_shared<FullMatrix<double>>(ListListD{
rotMat = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
{ 0, 1, 0 },
{ 0, 0, 1 }
@@ -307,11 +307,11 @@ 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);
rotMat = std::make_shared<FullMatrix<double>>(ListListD{
rotMat = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
{ 0, 1, 0 },
{ 0, 0, 1 }
@@ -324,7 +324,7 @@ void MbD::ASMTAssembly::runSinglePendulum()
mkr->setName(str);
pos3D = std::make_shared<FullColumn<double>>(ListD{ 0.1, 0.1, 0.1 });
mkr->setPosition3D(pos3D);
rotMat = std::make_shared<FullMatrix<double>>(ListListD{
rotMat = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
{ 0, 1, 0 },
{ 0, 0, 1 }
@@ -995,9 +995,9 @@ 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 = FullMatrix<double>>::identitysptr(3);
//principalMassMarker->rotationMatrix = FullMatrixDouble>::identitysptr(3);
}
std::shared_ptr<ASMTSpatialContainer> MbD::ASMTAssembly::spatialContainerAt(std::shared_ptr<ASMTAssembly> self, std::string& longname)

1
OndselSolver/ASMTConstraintSet.cpp
View File

@@ -10,6 +10,7 @@
#include "ASMTAssembly.h"
#include "ASMTMarker.h"
#include "Joint.h"
#include "FullMatrix.h"
using namespace MbD;

105
OndselSolver/ASMTMarker.cpp
View File

@@ -7,6 +7,7 @@
***************************************************************************/
#include "ASMTMarker.h"
#include "FullMatrix.h"
#include "ASMTRefItem.h"
#include "ASMTPart.h"
#include "Part.h"
@@ -14,57 +15,57 @@
#include "MarkerFrame.h"
#include "ASMTPrincipalMassMarker.h"
using namespace MbD;
namespace MbD {
void ASMTMarker::parseASMT(std::vector<std::string>& lines)
{
readName(lines);
readPosition3D(lines);
readRotationMatrix(lines);
}
void MbD::ASMTMarker::parseASMT(std::vector<std::string>& lines)
{
readName(lines);
readPosition3D(lines);
readRotationMatrix(lines);
}
FColDsptr MbD::ASMTMarker::rpmp()
{
//p is cm
auto refItem = static_cast<ASMTRefItem*>(owner);
auto& rPrefP = refItem->position3D;
auto& aAPref = refItem->rotationMatrix;
auto& rrefmref = position3D;
auto rPmP = rPrefP->plusFullColumn(aAPref->timesFullColumn(rrefmref));
auto& principalMassMarker = static_cast<ASMTPart*>(refItem->owner)->principalMassMarker;
auto& rPcmP = principalMassMarker->position3D;
auto& aAPcm = principalMassMarker->rotationMatrix;
auto rpmp = aAPcm->transposeTimesFullColumn(rPmP->minusFullColumn(rPcmP));
return rpmp;
}
FMatDsptr MbD::ASMTMarker::aApm()
{
//p is cm
auto refItem = static_cast<ASMTRefItem*>(owner);
auto& aAPref = refItem->rotationMatrix;
auto& aArefm = rotationMatrix;
auto& principalMassMarker = static_cast<ASMTPart*>(refItem->owner)->principalMassMarker;
auto& aAPcm = principalMassMarker->rotationMatrix;
auto aApm = aAPcm->transposeTimesFullMatrix(aAPref->timesFullMatrix(aArefm));
return aApm;
}
void MbD::ASMTMarker::createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits)
{
auto mkr = CREATE<MarkerFrame>::With(name.c_str());
auto prt = std::static_pointer_cast<Part>(partOrAssembly()->mbdObject);
prt->partFrame->addMarkerFrame(mkr);
mkr->rpmp = rpmp()->times(1.0 / mbdUnits->length);
mkr->aApm = aApm();
mbdObject = mkr->endFrames->at(0);
}
void MbD::ASMTMarker::storeOnLevel(std::ofstream& os, int level)
{
storeOnLevelString(os, level, "Marker");
storeOnLevelString(os, level + 1, "Name");
storeOnLevelString(os, level + 2, name);
ASMTSpatialItem::storeOnLevel(os, level);
FColDsptr ASMTMarker::rpmp()
{
//p is cm
auto refItem = static_cast<ASMTRefItem*>(owner);
auto& rPrefP = refItem->position3D;
auto& aAPref = refItem->rotationMatrix;
auto& rrefmref = position3D;
auto rPmP = rPrefP->plusFullColumn(aAPref->timesFullColumn(rrefmref));
auto& principalMassMarker = static_cast<ASMTPart*>(refItem->owner)->principalMassMarker;
auto& rPcmP = principalMassMarker->position3D;
auto& aAPcm = principalMassMarker->rotationMatrix;
auto rpmp = aAPcm->transposeTimesFullColumn(rPmP->minusFullColumn(rPcmP));
return rpmp;
}
FMatDsptr ASMTMarker::aApm()
{
//p is cm
auto refItem = static_cast<ASMTRefItem*>(owner);
auto& aAPref = refItem->rotationMatrix;
auto& aArefm = rotationMatrix;
auto& principalMassMarker = static_cast<ASMTPart*>(refItem->owner)->principalMassMarker;
auto& aAPcm = principalMassMarker->rotationMatrix;
auto aApm = aAPcm->transposeTimesFullMatrix(aAPref->timesFullMatrix(aArefm));
return aApm;
}
void ASMTMarker::createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits)
{
auto mkr = CREATE<MarkerFrame>::With(name.c_str());
auto prt = std::static_pointer_cast<Part>(partOrAssembly()->mbdObject);
prt->partFrame->addMarkerFrame(mkr);
mkr->rpmp = rpmp()->times(1.0 / mbdUnits->length);
mkr->aApm = aApm();
mbdObject = mkr->endFrames->at(0);
}
void ASMTMarker::storeOnLevel(std::ofstream& os, int level)
{
storeOnLevelString(os, level, "Marker");
storeOnLevelString(os, level + 1, "Name");
storeOnLevelString(os, level + 2, name);
ASMTSpatialItem::storeOnLevel(os, level);
}
}

1
OndselSolver/ASMTMarker.h
View File

@@ -23,7 +23,6 @@ namespace MbD {
FMatDsptr aApm();
void createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits) override;
void storeOnLevel(std::ofstream& os, int level) override;
};
}

58
OndselSolver/ASMTMotion.cpp
View File

@@ -9,36 +9,36 @@
#include "ASMTMotion.h"
using namespace MbD;
namespace MbD {
void ASMTMotion::readMotionSeries(std::vector<std::string>& lines)
{
std::string str = lines[0];
std::string substr = "MotionSeries";
auto pos = str.find(substr);
assert(pos != std::string::npos);
str.erase(0, pos + substr.length());
auto seriesName = readString(str);
assert(fullName("") == seriesName);
lines.erase(lines.begin());
readFXonIs(lines);
readFYonIs(lines);
readFZonIs(lines);
readTXonIs(lines);
readTYonIs(lines);
readTZonIs(lines);
}
void MbD::ASMTMotion::readMotionSeries(std::vector<std::string>& lines)
{
std::string str = lines[0];
std::string substr = "MotionSeries";
auto pos = str.find(substr);
assert(pos != std::string::npos);
str.erase(0, pos + substr.length());
auto seriesName = readString(str);
assert(fullName("") == seriesName);
lines.erase(lines.begin());
readFXonIs(lines);
readFYonIs(lines);
readFZonIs(lines);
readTXonIs(lines);
readTYonIs(lines);
readTZonIs(lines);
}
void ASMTMotion::initMarkers()
{
}
void MbD::ASMTMotion::initMarkers()
{
}
void ASMTMotion::storeOnLevel(std::ofstream& os, int level)
{
assert(false);
}
void MbD::ASMTMotion::storeOnLevel(std::ofstream& os, int level)
{
assert(false);
}
void MbD::ASMTMotion::storeOnTimeSeries(std::ofstream& os)
{
assert(false);
void ASMTMotion::storeOnTimeSeries(std::ofstream& os)
{
assert(false);
}
}

6
OndselSolver/ASMTPrincipalMassMarker.cpp
View File

@@ -26,7 +26,7 @@ void MbD::ASMTPrincipalMassMarker::parseASMT(std::vector<std::string>& lines)
lines.erase(lines.begin());
assert(lines[0] == (leadingTabs + "RotationMatrix"));
lines.erase(lines.begin());
rotationMatrix = std::make_shared<FullMatrix<double>>(3);
rotationMatrix = std::make_shared<FullMatrixDouble>(3);
for (int i = 0; i < 3; i++)
{
auto row = readRowOfDoubles(lines[0]);
@@ -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. });
};
}

4
OndselSolver/ASMTSpatialItem.cpp
View File

@@ -47,7 +47,7 @@ void MbD::ASMTSpatialItem::readRotationMatrix(std::vector<std::string>& lines)
{
assert(lines[0].find("RotationMatrix") != std::string::npos);
lines.erase(lines.begin());
rotationMatrix = std::make_shared<FullMatrix<double>>(3, 0);
rotationMatrix = std::make_shared<FullMatrixDouble>(3, 0);
for (int i = 0; i < 3; i++)
{
auto& row = rotationMatrix->at(i);
@@ -86,7 +86,7 @@ void MbD::ASMTSpatialItem::setRotationMatrix(double v11, double v12, double v13,
double v21, double v22, double v23,
double v31, double v32, double v33)
{
rotationMatrix = std::make_shared<FullMatrix<double>>(ListListD{
rotationMatrix = std::make_shared<FullMatrixDouble>(ListListD{
{ v11, v12, v13 },
{ v21, v22, v23 },
{ v31, v32, v33 }

2
OndselSolver/ASMTSpatialItem.h
View File

@@ -33,7 +33,7 @@ namespace MbD {
void storeOnLevelRotationMatrix(std::ofstream& os, int level);
FColDsptr position3D = std::make_shared<FullColumn<double>>(3);
FMatDsptr rotationMatrix = std::make_shared<FullMatrix<double>>(ListListD{
FMatDsptr rotationMatrix = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
{ 0, 1, 0 },
{ 0, 0, 1 }

2
OndselSolver/AngleZIeqcJec.cpp
View File

@@ -65,7 +65,7 @@ void MbD::AngleZIeqcJec::initialize()
{
AngleZIecJec::initialize();
pthezpEI = std::make_shared<FullRow<double>>(4);
ppthezpEIpEI = std::make_shared<FullMatrix<double>>(4, 4);
ppthezpEIpEI = std::make_shared<FullMatrixDouble>(4, 4);
}
FMatDsptr MbD::AngleZIeqcJec::ppvaluepEIpEI()

4
OndselSolver/AngleZIeqcJeqc.cpp
View File

@@ -85,8 +85,8 @@ void MbD::AngleZIeqcJeqc::initialize()
{
AngleZIeqcJec::initialize();
pthezpEJ = std::make_shared<FullRow<double>>(4);
ppthezpEIpEJ = std::make_shared<FullMatrix<double>>(4, 4);
ppthezpEJpEJ = std::make_shared<FullMatrix<double>>(4, 4);
ppthezpEIpEJ = std::make_shared<FullMatrixDouble>(4, 4);
ppthezpEJpEJ = std::make_shared<FullMatrixDouble>(4, 4);
}
FMatDsptr MbD::AngleZIeqcJeqc::ppvaluepEIpEJ()

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();
}

86
OndselSolver/CADSystem.cpp
View File

@@ -87,9 +87,9 @@ 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<FullMatrix<double>>(ListListD{
aAap = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
{ 0, 1, 0 },
{ 0, 0, 1 }
@@ -106,7 +106,7 @@ void CADSystem::runOndselSinglePendulum()
auto marker2 = CREATE<MarkerFrame>::With("/Assembly1/Marker2");
rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.0, 0.0, 0.0 });
marker2->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
aApm = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
{ 0, 1, 0 },
{ 0, 0, 1 }
@@ -117,7 +117,7 @@ void CADSystem::runOndselSinglePendulum()
auto marker1 = CREATE<MarkerFrame>::With("/Assembly1/Marker1");
rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.0, 3.0, 0.0 });
marker1->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
aApm = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
{ 0, 0, 1 },
{ 0, -1, 0 }
@@ -130,9 +130,9 @@ 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<FullMatrix<double>>(ListListD{
aAap = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
{ 0, 1, 0 },
{ 0, 0, 1 }
@@ -149,7 +149,7 @@ void CADSystem::runOndselSinglePendulum()
auto marker1 = CREATE<MarkerFrame>::With("/Assembly1/Part1/Marker1");
rpmp = std::make_shared<FullColumn<double>>(ListD{ -0.4, 0.0, 0.05 });
marker1->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
aApm = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
{ 0, 1, 0 },
{ 0, 0, 1 }
@@ -160,7 +160,7 @@ void CADSystem::runOndselSinglePendulum()
auto marker2 = CREATE<MarkerFrame>::With("/Assembly1/Part1/Marker2");
rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.4, 0.0, 0.05 });
marker2->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
aApm = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
{ 0, 1, 0 },
{ 0, 0, 1 }
@@ -222,9 +222,9 @@ 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<FullMatrix<double>>(ListListD{
aAap = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
{ 0, 1, 0 },
{ 0, 0, 1 }
@@ -241,7 +241,7 @@ void CADSystem::runOndselDoublePendulum()
auto marker2 = CREATE<MarkerFrame>::With("/Assembly1/Marker2");
rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.0, 0.0, 0.0 });
marker2->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
aApm = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
{ 0, 1, 0 },
{ 0, 0, 1 }
@@ -252,7 +252,7 @@ void CADSystem::runOndselDoublePendulum()
auto marker1 = CREATE<MarkerFrame>::With("/Assembly1/Marker1");
rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.0, 3.0, 0.0 });
marker1->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
aApm = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
{ 0, 0, 1 },
{ 0, -1, 0 }
@@ -265,9 +265,9 @@ 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<FullMatrix<double>>(ListListD{
aAap = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
{ 0, 1, 0 },
{ 0, 0, 1 }
@@ -284,7 +284,7 @@ void CADSystem::runOndselDoublePendulum()
auto marker1 = CREATE<MarkerFrame>::With("/Assembly1/Part1/Marker1");
rpmp = std::make_shared<FullColumn<double>>(ListD{ -0.4, 0.0, 0.05 });
marker1->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
aApm = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
{ 0, 1, 0 },
{ 0, 0, 1 }
@@ -295,7 +295,7 @@ void CADSystem::runOndselDoublePendulum()
auto marker2 = CREATE<MarkerFrame>::With("/Assembly1/Part1/Marker2");
rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.4, 0.0, 0.05 });
marker2->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
aApm = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
{ 0, 1, 0 },
{ 0, 0, 1 }
@@ -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 });
@@ -325,7 +325,7 @@ void CADSystem::runOndselDoublePendulum()
auto marker1 = CREATE<MarkerFrame>::With("/Assembly1/Part2/Marker1");
rpmp = std::make_shared<FullColumn<double>>(ListD{ -0.65, 0.0, -0.05 });
marker1->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
aApm = std::make_shared<FullMatrixDouble>(ListListD{
{1.0, 0.0, 0.0},
{0.0, 1.0, 0.0},
{0.0, 0.0, 1.0}
@@ -336,7 +336,7 @@ void CADSystem::runOndselDoublePendulum()
auto marker2 = CREATE<MarkerFrame>::With("/Assembly1/Part2/Marker2");
rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.65, 0.0, -0.05 });
marker2->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
aApm = std::make_shared<FullMatrixDouble>(ListListD{
{1.0, 0.0, 0.0},
{0.0, 1.0, 0.0},
{0.0, 0.0, 1.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);
@@ -417,7 +417,7 @@ void CADSystem::runOndselPiston()
auto marker2 = CREATE<MarkerFrame>::With("/Assembly1/Marker2");
rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.0, 0.0, 0.0 });
marker2->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
aApm = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
{ 0, 1, 0 },
{ 0, 0, 1 }
@@ -428,7 +428,7 @@ void CADSystem::runOndselPiston()
auto marker1 = CREATE<MarkerFrame>::With("/Assembly1/Marker1");
rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.0, 3.0, 0.0 });
marker1->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
aApm = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
{ 0, 0, 1 },
{ 0, -1, 0 }
@@ -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);
@@ -460,7 +460,7 @@ void CADSystem::runOndselPiston()
auto marker1 = CREATE<MarkerFrame>::With("/Assembly1/Part1/Marker1");
rpmp = std::make_shared<FullColumn<double>>(ListD{ -0.4, 0.0, 0.05 });
marker1->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
aApm = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
{ 0, 1, 0 },
{ 0, 0, 1 }
@@ -471,7 +471,7 @@ void CADSystem::runOndselPiston()
auto marker2 = CREATE<MarkerFrame>::With("/Assembly1/Part1/Marker2");
rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.4, 0.0, 0.05 });
marker2->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
aApm = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
{ 0, 1, 0 },
{ 0, 0, 1 }
@@ -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);
@@ -502,7 +502,7 @@ void CADSystem::runOndselPiston()
auto marker1 = CREATE<MarkerFrame>::With("/Assembly1/Part2/Marker1");
rpmp = std::make_shared<FullColumn<double>>(ListD{ -0.65, 0.0, -0.05 });
marker1->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
aApm = std::make_shared<FullMatrixDouble>(ListListD{
{1.0, 0.0, 0.0},
{0.0, 1.0, 0.0},
{0.0, 0.0, 1.0}
@@ -513,7 +513,7 @@ void CADSystem::runOndselPiston()
auto marker2 = CREATE<MarkerFrame>::With("/Assembly1/Part2/Marker2");
rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.65, 0.0, -0.05 });
marker2->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
aApm = std::make_shared<FullMatrixDouble>(ListListD{
{1.0, 0.0, 0.0},
{0.0, 1.0, 0.0},
{0.0, 0.0, 1.0}
@@ -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);
@@ -544,7 +544,7 @@ void CADSystem::runOndselPiston()
auto marker1 = CREATE<MarkerFrame>::With("/Assembly1/Part3/Marker1");
rpmp = std::make_shared<FullColumn<double>>(ListD{ -0.5, 0.0, 0.0 });
marker1->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
aApm = std::make_shared<FullMatrixDouble>(ListListD{
{0.0, 1.0, 0.0},
{1.0, 0.0, 0.0},
{0.0, 0.0, -1.0}
@@ -555,7 +555,7 @@ void CADSystem::runOndselPiston()
auto marker2 = CREATE<MarkerFrame>::With("/Assembly1/Part3/Marker2");
rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.5, 0.0, 0.0 });
marker2->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
aApm = std::make_shared<FullMatrixDouble>(ListListD{
{0.0, 0.0, 1.0},
{1.0, 0.0, 0.0},
{0.0, 1.0, 0.0}
@@ -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);
@@ -654,7 +654,7 @@ void CADSystem::runPiston()
auto marker2 = CREATE<MarkerFrame>::With("/Assembly1/Marker2");
rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.0, 0.0, 0.0 });
marker2->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
aApm = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
{ 0, 1, 0 },
{ 0, 0, 1 }
@@ -665,7 +665,7 @@ void CADSystem::runPiston()
auto marker1 = CREATE<MarkerFrame>::With("/Assembly1/Marker1");
rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.0, 2.8817526385684, 0.0 });
marker1->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
aApm = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
{ 0, 0, 1 },
{ 0, -1, 0 }
@@ -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);
@@ -697,7 +697,7 @@ void CADSystem::runPiston()
auto marker1 = CREATE<MarkerFrame>::With("/Assembly1/Part1/Marker1");
rpmp = std::make_shared<FullColumn<double>>(ListD{ -0.38423368514246, -2.6661567755108e-17, 0.048029210642807 });
marker1->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
aApm = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
{ 0, 1, 0 },
{ 0, 0, 1 }
@@ -708,7 +708,7 @@ void CADSystem::runPiston()
auto marker2 = CREATE<MarkerFrame>::With("/Assembly1/Part1/Marker2");
rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.38423368514246, -2.6661567755108e-17, 0.048029210642807 });
marker2->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
aApm = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
{ 0, 1, 0 },
{ 0, 0, 1 }
@@ -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);
@@ -739,7 +739,7 @@ void CADSystem::runPiston()
auto marker1 = CREATE<MarkerFrame>::With("/Assembly1/Part2/Marker1");
rpmp = std::make_shared<FullColumn<double>>(ListD{ -0.6243797383565, 1.1997705489799e-16, -0.048029210642807 });
marker1->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
aApm = std::make_shared<FullMatrixDouble>(ListListD{
{1.0, 2.7755575615629e-16, 0.0},
{-2.7755575615629e-16, 1.0, 0.0},
{0.0, 0.0, 1.0}
@@ -750,7 +750,7 @@ void CADSystem::runPiston()
auto marker2 = CREATE<MarkerFrame>::With("/Assembly1/Part2/Marker2");
rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.6243797383565, -2.1329254204087e-16, -0.048029210642807 });
marker2->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
aApm = std::make_shared<FullMatrixDouble>(ListListD{
{1.0, 2.4980018054066e-16, 2.2204460492503e-16},
{-2.4980018054066e-16, 1.0, 4.1633363423443e-17},
{-2.2204460492503e-16, -4.1633363423443e-17, 1.0}
@@ -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);
@@ -781,7 +781,7 @@ void CADSystem::runPiston()
auto marker1 = CREATE<MarkerFrame>::With("/Assembly1/Part3/Marker1");
rpmp = std::make_shared<FullColumn<double>>(ListD{ -0.48029210642807, 7.6201599718927e-18, -2.816737703896e-17 });
marker1->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
aApm = std::make_shared<FullMatrixDouble>(ListListD{
{9.2444637330587e-33, 1.0, 2.2204460492503e-16},
{1.0, -9.2444637330587e-33, -1.0785207688569e-32},
{-1.0785207688569e-32, 2.2204460492503e-16, -1.0}
@@ -792,7 +792,7 @@ void CADSystem::runPiston()
auto marker2 = CREATE<MarkerFrame>::With("/Assembly1/Part3/Marker2");
rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.48029210642807, 1.7618247880058e-17, 2.5155758471256e-17 });
marker2->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
aApm = std::make_shared<FullMatrixDouble>(ListListD{
{6.9388939039072e-18, -6.4146353042213e-50, 1.0},
{1.0, -6.9388939039072e-18, 6.9388939039072e-18},
{-6.9388939039072e-18, 1.0, -7.4837411882581e-50}

2
OndselSolver/CREATE.h
View File

@@ -6,7 +6,7 @@
* See LICENSE file for details about copyright. *
***************************************************************************/
//This header file causes wierd problems in Visual Studio when included in subclasses of std::vector or std::map. Why?
//This header file causes weird problems in Visual Studio when included in subclasses of std::vector or std::map. Why?
#pragma once

2
OndselSolver/ConstVelConstraintIqcJc.cpp
View File

@@ -97,7 +97,7 @@ void MbD::ConstVelConstraintIqcJc::initialize()
{
ConstVelConstraintIJ::initialize();
pGpEI = std::make_shared<FullRow<double>>(4);
ppGpEIpEI = std::make_shared<FullMatrix<double>>(4, 4);
ppGpEIpEI = std::make_shared<FullMatrixDouble>(4, 4);
}
void MbD::ConstVelConstraintIqcJc::useEquationNumbers()

4
OndselSolver/ConstVelConstraintIqcJqc.cpp
View File

@@ -118,8 +118,8 @@ void MbD::ConstVelConstraintIqcJqc::initialize()
{
ConstVelConstraintIqcJc::initialize();
pGpEJ = std::make_shared<FullRow<double>>(4);
ppGpEIpEJ = std::make_shared<FullMatrix<double>>(4, 4);
ppGpEJpEJ = std::make_shared<FullMatrix<double>>(4, 4);
ppGpEIpEJ = std::make_shared<FullMatrixDouble>(4, 4);
ppGpEJpEJ = std::make_shared<FullMatrixDouble>(4, 4);
}
void MbD::ConstVelConstraintIqcJqc::useEquationNumbers()

87
OndselSolver/DiagonalMatrix.cpp
View File

@@ -7,5 +7,90 @@
***************************************************************************/
#include "DiagonalMatrix.h"
#include "FullMatrix.h"
using namespace MbD;
namespace MbD {
DiagMatDsptr DiagonalMatrix::times(double factor)
{
auto nrow = (int)this->size();
auto answer = std::make_shared<DiagonalMatrix>(nrow);
for (int i = 0; i < nrow; i++)
{
answer->at(i) = this->at(i) * factor;
}
return answer;
}
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);
}
}
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<double>>(nrow);
for (int i = 0; i < nrow; i++)
{
answer->at(i) = this->at(i) * fullCol->at(i);
}
return answer;
}
FMatDsptr DiagonalMatrix::timesFullMatrix(FMatDsptr fullMat)
{
auto nrow = (int)this->size();
auto answer = std::make_shared<FullMatrixDouble>(nrow);
for (int i = 0; i < nrow; i++)
{
answer->at(i) = fullMat->at(i)->times(this->at(i));
}
return answer;
}
double DiagonalMatrix::sumOfSquares()
{
double sum = 0.0;
for (int i = 0; i < this->size(); i++)
{
double element = this->at(i);
sum += element * element;
}
return sum;
}
int DiagonalMatrix::numberOfElements()
{
auto n = (int)this->size();
return n * n;
}
void DiagonalMatrix::zeroSelf()
{
for (int i = 0; i < this->size(); i++) {
this->at(i) = 0.0;
}
}
double DiagonalMatrix::maxMagnitude()
{
double max = 0.0;
for (int i = 0; i < this->size(); i++)
{
double element = this->at(i);
if (element < 0.0) element = -element;
if (max < element) max = element;
}
return max;
}
std::ostream& DiagonalMatrix::printOn(std::ostream& s) const
{
s << "DiagMat[";
s << this->at(0);
for (int i = 1; i < this->size(); i++)
{
s << ", " << this->at(i);
}
s << "]";
return s;
}
}

126
OndselSolver/DiagonalMatrix.h
View File

@@ -8,25 +8,26 @@
#pragma once
#include "FullColumn.ref.h"
#include "FullRow.ref.h"
#include "FullMatrix.ref.h"
#include "DiagonalMatrix.ref.h"
#include "Array.h"
#include "FullColumn.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);
FMatsptr<T> timesFullMatrix(FMatsptr<T> fullMat);
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();
}
@@ -39,109 +40,6 @@ namespace MbD {
double maxMagnitude() override;
std::ostream& printOn(std::ostream& s) const override;
};
template<>
inline DiagMatDsptr DiagonalMatrix<double>::times(double factor)
{
auto nrow = (int)this->size();
auto answer = std::make_shared<DiagonalMatrix<double>>(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)
{
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)
{
//"a*b = a(i,j)b(j) sum j."
auto nrow = (int)this->size();
auto answer = std::make_shared<FullColumn<T>>(nrow);
for (int i = 0; i < nrow; i++)
{
answer->at(i) = this->at(i) * fullCol->at(i);
}
return answer;
}
template<typename T>
inline FMatsptr<T> DiagonalMatrix<T>::timesFullMatrix(FMatsptr<T> fullMat)
{
auto nrow = (int)this->size();
auto answer = std::make_shared<FullMatrix<T>>(nrow);
for (int i = 0; i < nrow; i++)
{
answer->at(i) = fullMat->at(i)->times(this->at(i));
}
return answer;
}
template<>
inline double DiagonalMatrix<double>::sumOfSquares()
{
double sum = 0.0;
for (int i = 0; i < this->size(); i++)
{
double element = this->at(i);
sum += element * element;
}
return sum;
}
template<typename T>
inline int DiagonalMatrix<T>::numberOfElements()
{
auto n = (int)this->size();
return n * n;
}
template<>
inline void DiagonalMatrix<double>::zeroSelf()
{
for (int i = 0; i < this->size(); i++) {
this->at(i) = 0.0;
}
}
template<>
inline double DiagonalMatrix<double>::maxMagnitude()
{
double max = 0.0;
for (int i = 0; i < this->size(); i++)
{
double element = this->at(i);
if (element < 0.0) element = -element;
if (max < element) max = element;
}
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
{
s << "DiagMat[";
s << this->at(0);
for (int i = 1; i < this->size(); i++)
{
s << ", " << this->at(i);
}
s << "]";
return s;
}
}

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>;
}

2
OndselSolver/DifferenceOperator.cpp
View File

@@ -57,7 +57,7 @@ void DifferenceOperator::setorder(int o)
void DifferenceOperator::instantiateTaylorMatrix()
{
if (taylorMatrix == nullptr || (taylorMatrix->nrow() != (order + 1))) {
taylorMatrix = std::make_shared<FullMatrix<double>>(order + 1, order + 1);
taylorMatrix = std::make_shared<FullMatrixDouble>(order + 1, order + 1);
}
}

2
OndselSolver/DirectionCosineIeqcJec.cpp
View File

@@ -24,7 +24,7 @@ void DirectionCosineIeqcJec::initialize()
{
DirectionCosineIecJec::initialize();
pAijIeJepEI = std::make_shared<FullRow<double>>(4);
ppAijIeJepEIpEI = std::make_shared<FullMatrix<double>>(4, 4);
ppAijIeJepEIpEI = std::make_shared<FullMatrixDouble>(4, 4);
}
void DirectionCosineIeqcJec::initializeGlobally()

4
OndselSolver/DirectionCosineIeqcJeqc.cpp
View File

@@ -24,8 +24,8 @@ void DirectionCosineIeqcJeqc::initialize()
{
DirectionCosineIeqcJec::initialize();
pAijIeJepEJ = std::make_shared<FullRow<double>>(4);
ppAijIeJepEIpEJ = std::make_shared<FullMatrix<double>>(4, 4);
ppAijIeJepEJpEJ = std::make_shared<FullMatrix<double>>(4, 4);
ppAijIeJepEIpEJ = std::make_shared<FullMatrixDouble>(4, 4);
ppAijIeJepEJpEJ = std::make_shared<FullMatrixDouble>(4, 4);
}
void DirectionCosineIeqcJeqc::initializeGlobally()

2
OndselSolver/DispCompIecJecKeqc.cpp
View File

@@ -22,7 +22,7 @@ DispCompIecJecKeqc::DispCompIecJecKeqc(EndFrmsptr frmi, EndFrmsptr frmj, EndFrms
void DispCompIecJecKeqc::initialize()
{
priIeJeKepEK = std::make_shared<FullRow<double>>(4);
ppriIeJeKepEKpEK = std::make_shared<FullMatrix<double>>(4, 4);
ppriIeJeKepEKpEK = std::make_shared<FullMatrixDouble>(4, 4);
}
void DispCompIecJecKeqc::initializeGlobally()

4
OndselSolver/DispCompIeqcJecIe.cpp
View File

@@ -87,8 +87,8 @@ void MbD::DispCompIeqcJecIe::initialize()
DispCompIecJecIe::initialize();
priIeJeIepXI = std::make_shared<FullRow<double>>(3);
priIeJeIepEI = std::make_shared<FullRow<double>>(4);
ppriIeJeIepXIpEI = std::make_shared<FullMatrix<double>>(3, 4);
ppriIeJeIepEIpEI = std::make_shared<FullMatrix<double>>(4, 4);
ppriIeJeIepXIpEI = std::make_shared<FullMatrixDouble>(3, 4);
ppriIeJeIepEIpEI = std::make_shared<FullMatrixDouble>(4, 4);
}
void MbD::DispCompIeqcJecIe::initializeGlobally()

6
OndselSolver/DispCompIeqcJecKeqc.cpp
View File

@@ -24,9 +24,9 @@ void DispCompIeqcJecKeqc::initialize()
DispCompIecJecKeqc::initialize();
priIeJeKepXI = std::make_shared<FullRow<double>>(3);
priIeJeKepEI = std::make_shared<FullRow<double>>(4);
ppriIeJeKepEIpEI = std::make_shared<FullMatrix<double>>(4, 4);
ppriIeJeKepXIpEK = std::make_shared<FullMatrix<double>>(3, 4);
ppriIeJeKepEIpEK = std::make_shared<FullMatrix<double>>(4, 4);
ppriIeJeKepEIpEI = std::make_shared<FullMatrixDouble>(4, 4);
ppriIeJeKepXIpEK = std::make_shared<FullMatrixDouble>(3, 4);
ppriIeJeKepEIpEK = std::make_shared<FullMatrixDouble>(4, 4);
}
void DispCompIeqcJecKeqc::calcPostDynCorrectorIteration()

6
OndselSolver/DispCompIeqcJeqcIe.cpp
View File

@@ -83,9 +83,9 @@ void MbD::DispCompIeqcJeqcIe::initialize()
DispCompIeqcJecIe::initialize();
priIeJeIepXJ = std::make_shared<FullRow<double>>(3);
priIeJeIepEJ = std::make_shared<FullRow<double>>(4);
ppriIeJeIepEIpXJ = std::make_shared<FullMatrix<double>>(4, 3);
ppriIeJeIepEIpEJ = std::make_shared<FullMatrix<double>>(4, 4);
ppriIeJeIepEJpEJ = std::make_shared<FullMatrix<double>>(4, 4);
ppriIeJeIepEIpXJ = std::make_shared<FullMatrixDouble>(4, 3);
ppriIeJeIepEIpEJ = std::make_shared<FullMatrixDouble>(4, 4);
ppriIeJeIepEJpEJ = std::make_shared<FullMatrixDouble>(4, 4);
}
FMatDsptr MbD::DispCompIeqcJeqcIe::ppvaluepEIpEJ()

6
OndselSolver/DispCompIeqcJeqcKeqc.cpp
View File

@@ -24,9 +24,9 @@ void DispCompIeqcJeqcKeqc::initialize()
DispCompIeqcJecKeqc::initialize();
priIeJeKepXJ = std::make_shared<FullRow<double>>(3);
priIeJeKepEJ = std::make_shared<FullRow<double>>(4);
ppriIeJeKepEJpEJ = std::make_shared<FullMatrix<double>>(4, 4);
ppriIeJeKepXJpEK = std::make_shared<FullMatrix<double>>(3, 4);
ppriIeJeKepEJpEK = std::make_shared<FullMatrix<double>>(4, 4);
ppriIeJeKepEJpEJ = std::make_shared<FullMatrixDouble>(4, 4);
ppriIeJeKepXJpEK = std::make_shared<FullMatrixDouble>(3, 4);
ppriIeJeKepEJpEK = std::make_shared<FullMatrixDouble>(4, 4);
}
void DispCompIeqcJeqcKeqc::calcPostDynCorrectorIteration()

10
OndselSolver/DistIeqcJec.cpp
View File

@@ -41,7 +41,7 @@ void MbD::DistIeqcJec::calcPrivate()
pprIeJepXIipXI->atiput(j, element / rIeJe);
}
}
pprIeJepXIpEI = std::make_shared<FullMatrix<double>>(3, 4);
pprIeJepXIpEI = std::make_shared<FullMatrixDouble>(3, 4);
for (int i = 0; i < 3; i++)
{
auto& pprIeJepXIipEI = pprIeJepXIpEI->at(i);
@@ -53,7 +53,7 @@ void MbD::DistIeqcJec::calcPrivate()
pprIeJepXIipEI->atiput(j, element / rIeJe);
}
}
pprIeJepEIpEI = std::make_shared<FullMatrix<double>>(4, 4);
pprIeJepEIpEI = std::make_shared<FullMatrixDouble>(4, 4);
for (int i = 0; i < 4; i++)
{
auto& pprIeJepEIipEI = pprIeJepEIpEI->at(i);
@@ -74,9 +74,9 @@ void MbD::DistIeqcJec::initialize()
DistIecJec::initialize();
prIeJepXI = std::make_shared<FullRow<double>>(3);
prIeJepEI = std::make_shared<FullRow<double>>(4);
pprIeJepXIpXI = std::make_shared<FullMatrix<double>>(3, 3);
pprIeJepXIpEI = std::make_shared<FullMatrix<double>>(3, 4);
pprIeJepEIpEI = std::make_shared<FullMatrix<double>>(4, 4);
pprIeJepXIpXI = std::make_shared<FullMatrixDouble>(3, 3);
pprIeJepXIpEI = std::make_shared<FullMatrixDouble>(3, 4);
pprIeJepEIpEI = std::make_shared<FullMatrixDouble>(4, 4);
}
FMatDsptr MbD::DistIeqcJec::ppvaluepEIpEI()

14
OndselSolver/DistIeqcJeqc.cpp
View File

@@ -122,13 +122,13 @@ void MbD::DistIeqcJeqc::initialize()
DistIeqcJec::initialize();
prIeJepXJ = std::make_shared<FullRow<double>>(3);
prIeJepEJ = std::make_shared<FullRow<double>>(4);
pprIeJepXIpXJ = std::make_shared<FullMatrix<double>>(3, 3);
pprIeJepEIpXJ = std::make_shared<FullMatrix<double>>(4, 3);
pprIeJepXJpXJ = std::make_shared<FullMatrix<double>>(3, 3);
pprIeJepXIpEJ = std::make_shared<FullMatrix<double>>(3, 4);
pprIeJepEIpEJ = std::make_shared<FullMatrix<double>>(4, 4);
pprIeJepXJpEJ = std::make_shared<FullMatrix<double>>(3, 4);
pprIeJepEJpEJ = std::make_shared<FullMatrix<double>>(4, 4);
pprIeJepXIpXJ = std::make_shared<FullMatrixDouble>(3, 3);
pprIeJepEIpXJ = std::make_shared<FullMatrixDouble>(4, 3);
pprIeJepXJpXJ = std::make_shared<FullMatrixDouble>(3, 3);
pprIeJepXIpEJ = std::make_shared<FullMatrixDouble>(3, 4);
pprIeJepEIpEJ = std::make_shared<FullMatrixDouble>(4, 4);
pprIeJepXJpEJ = std::make_shared<FullMatrixDouble>(3, 4);
pprIeJepEJpEJ = std::make_shared<FullMatrixDouble>(4, 4);
}
FMatDsptr MbD::DistIeqcJeqc::ppvaluepEIpEJ()

6
OndselSolver/DistxyIeqcJec.cpp
View File

@@ -161,9 +161,9 @@ void MbD::DistxyIeqcJec::initialize()
DistxyIecJec::initialize();
pdistxypXI = std::make_shared<FullRow<double>>(3);
pdistxypEI = std::make_shared<FullRow<double>>(4);
ppdistxypXIpXI = std::make_shared<FullMatrix<double>>(3, 3);
ppdistxypXIpEI = std::make_shared<FullMatrix<double>>(3, 4);
ppdistxypEIpEI = std::make_shared<FullMatrix<double>>(4, 4);
ppdistxypXIpXI = std::make_shared<FullMatrixDouble>(3, 3);
ppdistxypXIpEI = std::make_shared<FullMatrixDouble>(3, 4);
ppdistxypEIpEI = std::make_shared<FullMatrixDouble>(4, 4);
}
FMatDsptr MbD::DistxyIeqcJec::ppvaluepEIpEI()

14
OndselSolver/DistxyIeqcJeqc.cpp
View File

@@ -293,13 +293,13 @@ void MbD::DistxyIeqcJeqc::initialize()
DistxyIeqcJec::initialize();
pdistxypXJ = std::make_shared<FullRow<double>>(3);
pdistxypEJ = std::make_shared<FullRow<double>>(4);
ppdistxypXIpXJ = std::make_shared<FullMatrix<double>>(3, 3);
ppdistxypXIpEJ = std::make_shared<FullMatrix<double>>(3, 4);
ppdistxypEIpXJ = std::make_shared<FullMatrix<double>>(4, 3);
ppdistxypEIpEJ = std::make_shared<FullMatrix<double>>(4, 4);
ppdistxypXJpXJ = std::make_shared<FullMatrix<double>>(3, 3);
ppdistxypXJpEJ = std::make_shared<FullMatrix<double>>(3, 4);
ppdistxypEJpEJ = std::make_shared<FullMatrix<double>>(4, 4);
ppdistxypXIpXJ = std::make_shared<FullMatrixDouble>(3, 3);
ppdistxypXIpEJ = std::make_shared<FullMatrixDouble>(3, 4);
ppdistxypEIpXJ = std::make_shared<FullMatrixDouble>(4, 3);
ppdistxypEIpEJ = std::make_shared<FullMatrixDouble>(4, 4);
ppdistxypXJpXJ = std::make_shared<FullMatrixDouble>(3, 3);
ppdistxypXJpEJ = std::make_shared<FullMatrixDouble>(3, 4);
ppdistxypEJpEJ = std::make_shared<FullMatrixDouble>(4, 4);
}
FMatDsptr MbD::DistxyIeqcJeqc::ppvaluepEIpEJ()

2
OndselSolver/EndFramec.h
View File

@@ -44,7 +44,7 @@ namespace MbD {
MarkerFrame* markerFrame; //Use raw pointer when pointing backwards.
FColDsptr rOeO = std::make_shared<FullColumn<double>>(3);
FMatDsptr aAOe = FullMatrix<double>::identitysptr(3);
FMatDsptr aAOe = FullMatrixDouble::identitysptr(3);
};
//using EndFrmsptr = std::shared_ptr<EndFramec>;
}

12
OndselSolver/EndFrameqc.cpp
View File

@@ -25,10 +25,10 @@ EndFrameqc::EndFrameqc(const char* str) : EndFramec(str) {
void EndFrameqc::initialize()
{
prOeOpE = std::make_shared<FullMatrix<double>>(3, 4);
pprOeOpEpE = std::make_shared<FullMatrix<FColDsptr>>(4, 4);
prOeOpE = std::make_shared<FullMatrixDouble>(3, 4);
pprOeOpEpE = std::make_shared<FullMatrixFullColumnDouble>(4, 4);
pAOepE = std::make_shared<FullColumn<FMatDsptr>>(4);
ppAOepEpE = std::make_shared<FullMatrix<FMatDsptr>>(4, 4);
ppAOepEpE = std::make_shared<FullMatrixFullMatrixDouble>(4, 4);
}
void EndFrameqc::initializeGlobally()
@@ -59,7 +59,7 @@ void MbD::EndFrameqc::initEndFrameqct2()
FMatFColDsptr EndFrameqc::ppAjOepEpE(int jj)
{
auto answer = std::make_shared<FullMatrix<FColDsptr>>(4, 4);
auto answer = std::make_shared<FullMatrixFullColumnDouble>(4, 4);
for (int i = 0; i < 4; i++) {
auto& answeri = answer->at(i);
auto& ppAOepEipE = ppAOepEpE->at(i);
@@ -80,7 +80,7 @@ void EndFrameqc::calcPostDynCorrectorIteration()
FMatDsptr EndFrameqc::pAjOepET(int axis)
{
auto answer = std::make_shared<FullMatrix<double>>(4, 3);
auto answer = std::make_shared<FullMatrixDouble>(4, 3);
for (int i = 0; i < 4; i++) {
auto& answeri = answer->at(i);
auto& pAOepEi = pAOepE->at(i);
@@ -94,7 +94,7 @@ FMatDsptr EndFrameqc::pAjOepET(int axis)
FMatDsptr EndFrameqc::ppriOeOpEpE(int ii)
{
auto answer = std::make_shared<FullMatrix<double>>(4, 4);
auto answer = std::make_shared<FullMatrixDouble>(4, 4);
for (int i = 0; i < 4; i++) {
auto& answeri = answer->at(i);
auto& pprOeOpEipE = pprOeOpEpE->at(i);

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 = FullMatrix<double>::identitysptr(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<FMatDsptr>>(4);
ppAOeptpt = std::make_shared<FullMatrix<double>>(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<FullMatrix<double>>(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()

78
OndselSolver/EulerAngles.cpp
View File

@@ -7,3 +7,81 @@
***************************************************************************/
#include "EulerAngles.h"
namespace MbD {
template<typename T>
inline void EulerAngles<T>::initialize()
{
assert(false);
}
template<>
inline void EulerAngles<Symsptr>::calc()
{
cA = std::make_shared<FullColumn<FMatDsptr>>(3);
for (int i = 0; i < 3; i++)
{
auto axis = rotOrder->at(i);
auto angle = this->at(i)->getValue();
if (axis == 1) {
cA->atiput(i, FullMatrixDouble::rotatex(angle));
}
else if (axis == 2) {
cA->atiput(i, FullMatrixDouble::rotatey(angle));
}
else if (axis == 3) {
cA->atiput(i, FullMatrixDouble::rotatez(angle));
}
else {
throw std::runtime_error("Euler angle rotation order must be any permutation of 1,2,3 without consecutive repeats.");
}
}
aA = cA->at(0)->timesFullMatrix(cA->at(1)->timesFullMatrix(cA->at(2)));
}
template<>
inline void EulerAngles<double>::calc()
{
cA = std::make_shared<FullColumn<FMatDsptr>>(3);
for (int i = 0; i < 3; i++)
{
auto axis = rotOrder->at(i);
auto angle = this->at(i);
if (axis == 1) {
cA->atiput(i, FullMatrixDouble::rotatex(angle));
}
else if (axis == 2) {
cA->atiput(i, FullMatrixDouble::rotatey(angle));
}
else if (axis == 3) {
cA->atiput(i, FullMatrixDouble::rotatez(angle));
}
else {
throw std::runtime_error("Euler angle rotation order must be any permutation of 1,2,3 without consecutive repeats.");
}
}
aA = cA->at(0)->timesFullMatrix(cA->at(1)->timesFullMatrix(cA->at(2)));
}
template<typename T>
inline void EulerAngles<T>::calc()
{
assert(false);
}
template<typename T>
inline void EulerAngles<T>::setRotOrder(int i, int j, int k)
{
rotOrder = std::make_shared<FullColumn<int>>(3);
rotOrder->at(0) = i;
rotOrder->at(1) = j;
rotOrder->at(2) = k;
}
// 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;
}
}

81
OndselSolver/EulerAngles.h
View File

@@ -16,8 +16,6 @@
#include "Symbolic.h"
namespace MbD {
//template<typename T>
//class EulerAnglesDot;
template<typename T>
class EulerAngles : public EulerArray<T>
@@ -29,87 +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;
};
template<typename T>
inline void EulerAngles<T>::initialize()
{
assert(false);
}
template<>
inline void EulerAngles<Symsptr>::calc()
{
cA = std::make_shared<FullColumn<FMatDsptr>>(3);
for (int i = 0; i < 3; i++)
{
auto axis = rotOrder->at(i);
auto angle = this->at(i)->getValue();
if (axis == 1) {
cA->atiput(i, FullMatrix<double>::rotatex(angle));
}
else if (axis == 2) {
cA->atiput(i, FullMatrix<double>::rotatey(angle));
}
else if (axis == 3) {
cA->atiput(i, FullMatrix<double>::rotatez(angle));
}
else {
throw std::runtime_error("Euler angle rotation order must be any permutation of 1,2,3 without consecutive repeats.");
}
}
aA = cA->at(0)->timesFullMatrix(cA->at(1)->timesFullMatrix(cA->at(2)));
}
template<>
inline void EulerAngles<double>::calc()
{
cA = std::make_shared<FullColumn<FMatDsptr>>(3);
for (int i = 0; i < 3; i++)
{
auto axis = rotOrder->at(i);
auto angle = this->at(i);
if (axis == 1) {
cA->atiput(i, FullMatrix<double>::rotatex(angle));
}
else if (axis == 2) {
cA->atiput(i, FullMatrix<double>::rotatey(angle));
}
else if (axis == 3) {
cA->atiput(i, FullMatrix<double>::rotatez(angle));
}
else {
throw std::runtime_error("Euler angle rotation order must be any permutation of 1,2,3 without consecutive repeats.");
}
}
aA = cA->at(0)->timesFullMatrix(cA->at(1)->timesFullMatrix(cA->at(2)));
}
template<typename T>
inline void EulerAngles<T>::calc()
{
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);
rotOrder->at(0) = i;
rotOrder->at(1) = j;
rotOrder->at(2) = k;
}
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);
}

12
OndselSolver/EulerAnglesDDot.h
View File

@@ -47,13 +47,13 @@ namespace MbD {
auto angleDot = aEulerAnglesDot->at(i)->getValue();
auto angleDDot = this->at(i)->getValue();
if (axis == 1) {
cAddot->atiput(i, FullMatrix<double>::rotatexrotDotrotDDot(angle, angleDot, angleDDot));
cAddot->atiput(i, FullMatrixDouble::rotatexrotDotrotDDot(angle, angleDot, angleDDot));
}
else if (axis == 2) {
cAddot->atiput(i, FullMatrix<double>::rotateyrotDotrotDDot(angle, angleDot, angleDDot));
cAddot->atiput(i, FullMatrixDouble::rotateyrotDotrotDDot(angle, angleDot, angleDDot));
}
else if (axis == 3) {
cAddot->atiput(i, FullMatrix<double>::rotatezrotDotrotDDot(angle, angleDot, angleDDot));
cAddot->atiput(i, FullMatrixDouble::rotatezrotDotrotDDot(angle, angleDot, angleDDot));
}
else {
throw std::runtime_error("Euler angle rotation order must be any permutation of 1,2,3 without consecutive repeats.");
@@ -79,8 +79,10 @@ namespace MbD {
auto term7 = phiA->timesFullMatrix(theAdot->timesFullMatrix(psiAdot));
auto term8 = phiA->timesFullMatrix(theA->timesFullMatrix(psiAddot));
aAddot = term->plusFullMatrix(term1)->plusFullMatrix(term2)->plusFullMatrix(term3)->plusFullMatrix(term4)
->plusFullMatrix(term5)->plusFullMatrix(term6)->plusFullMatrix(term7)->plusFullMatrix(term8);
aAddot = term->plusFullMatrix(term1)->plusFullMatrix(term2)
->plusFullMatrix(term3)->plusFullMatrix(term4)
->plusFullMatrix(term5)->plusFullMatrix(term6)
->plusFullMatrix(term7)->plusFullMatrix(term8);
}
template<typename T>
inline void EulerAnglesDDot<T>::aEulerAngles(EulerAngles<T>* eulerAngles)

14
OndselSolver/EulerAnglesDot.h
View File

@@ -56,13 +56,13 @@ namespace MbD {
auto angle = aEulerAngles->at(i)->getValue();
auto angleDot = this->at(i)->getValue();
if (axis == 1) {
cAdot->atiput(i, FullMatrix<double>::rotatexrotDot(angle, angleDot));
cAdot->atiput(i, FullMatrixDouble::rotatexrotDot(angle, angleDot));
}
else if (axis == 2) {
cAdot->atiput(i, FullMatrix<double>::rotateyrotDot(angle, angleDot));
cAdot->atiput(i, FullMatrixDouble::rotateyrotDot(angle, angleDot));
}
else if (axis == 3) {
cAdot->atiput(i, FullMatrix<double>::rotatezrotDot(angle, angleDot));
cAdot->atiput(i, FullMatrixDouble::rotatezrotDot(angle, angleDot));
}
else {
throw std::runtime_error("Euler angle rotation order must be any permutation of 1,2,3 without consecutive repeats.");
@@ -79,11 +79,11 @@ namespace MbD {
auto psiAdot = cAdot->at(2);
aAdot = phiAdot->timesFullMatrix(theA->timesFullMatrix(psiA))
->plusFullMatrix(phiA->timesFullMatrix(theAdot->timesFullMatrix(psiA)))
->plusFullMatrix(phiA->timesFullMatrix(theA->timesFullMatrix(psiAdot)));
->plusFullMatrix(phiA->timesFullMatrix(theAdot->timesFullMatrix(psiA)))
->plusFullMatrix(phiA->timesFullMatrix(theA->timesFullMatrix(psiAdot)));
omeF = (phiA->column(0)->times(phidot)
->plusFullColumn(phiA->timesFullMatrix(theA)->column(1)->times(thedot))
->plusFullColumn(aEulerAngles->aA->column(2)->times(psidot)));
->plusFullColumn(phiA->timesFullMatrix(theA)->column(1)->times(thedot))
->plusFullColumn(aEulerAngles->aA->column(2)->times(psidot)));
omef = aEulerAngles->aA->transposeTimesFullColumn(omeF);
}
}

6
OndselSolver/EulerAngleszxz.h
View File

@@ -29,9 +29,9 @@ namespace MbD {
template<typename T>
inline void EulerAngleszxz<T>::initialize()
{
phiA = FullMatrix<T>::identitysptr(3);
theA = FullMatrix<T>::identitysptr(3);
psiA = FullMatrix<T>::identitysptr(3);
phiA = FullMatrixDouble::identitysptr(3);
theA = FullMatrixDouble::identitysptr(3);
psiA = FullMatrixDouble::identitysptr(3);
}
template<typename T>
inline void EulerAngleszxz<T>::calc()

12
OndselSolver/EulerAngleszxzDDot.h
View File

@@ -8,6 +8,7 @@
#pragma once
#include "FullMatrix.h"
#include "EulerArray.h"
#include "EulerAngleszxzDot.h"
@@ -23,16 +24,17 @@ namespace MbD {
void calc() override;
std::shared_ptr<EulerAngleszxzDot<double>> phiThePsiDot;
FMatDsptr phiAddot, theAddot, psiAddot, aAddot;
FMatDsptr phiAddot, theAddot, psiAddot;
std::shared_ptr<FullMatrixDouble> aAddot;
};
template<typename T>
inline void EulerAngleszxzDDot<T>::initialize()
{
phiAddot = std::make_shared<FullMatrix<double>>(3, 3);
phiAddot = std::make_shared<FullMatrixDouble>(3, 3);
phiAddot->zeroSelf();
theAddot = std::make_shared<FullMatrix<double>>(3, 3);
theAddot = std::make_shared<FullMatrixDouble>(3, 3);
theAddot->zeroSelf();
psiAddot = std::make_shared<FullMatrix<double>>(3, 3);
psiAddot = std::make_shared<FullMatrixDouble>(3, 3);
psiAddot->zeroSelf();
}
template<typename T>
@@ -90,7 +92,7 @@ namespace MbD {
+ *(phiAdot->timesFullMatrix(theA->timesFullMatrix(psiAdot)))
+ *(phiA->timesFullMatrix(theAdot->timesFullMatrix(psiAdot)))
+ *(phiA->timesFullMatrix(theA->timesFullMatrix(psiAddot)));
aAddot = std::make_shared<FullMatrix<double>>(mat);
aAddot = std::make_shared<FullMatrixDouble>(mat);
}
}

6
OndselSolver/EulerAngleszxzDot.h
View File

@@ -28,11 +28,11 @@ namespace MbD {
template<typename T>
inline void EulerAngleszxzDot<T>::initialize()
{
phiAdot = std::make_shared<FullMatrix<double>>(3, 3);
phiAdot = std::make_shared<FullMatrixDouble>(3, 3);
phiAdot->zeroSelf();
theAdot = std::make_shared<FullMatrix<double>>(3, 3);
theAdot = std::make_shared<FullMatrixDouble>(3, 3);
theAdot->zeroSelf();
psiAdot = std::make_shared<FullMatrix<double>>(3, 3);
psiAdot = std::make_shared<FullMatrixDouble>(3, 3);
psiAdot->zeroSelf();
}
template<typename T>

586
OndselSolver/EulerParameters.cpp
View File

@@ -7,294 +7,308 @@
***************************************************************************/
#include "EulerParameters.h"
#include "FullColumn.h"
#include "FullRow.h"
#include "FullMatrix.h"
using namespace MbD;
template<>
inline 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<FullMatrix<FColDsptr>>(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<>
inline 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<FullMatrix<double>>(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>
inline 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<FullMatrix<double>>(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<>
inline 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<FullMatrix<double>>(ListFRD{ a2m0, m2m1, m2m2 });
auto mat01 = std::make_shared<FullMatrix<double>>(ListFRD{ a2m1, a2m0, zero });
auto mat02 = std::make_shared<FullMatrix<double>>(ListFRD{ a2m2, zero, a2m0 });
auto mat03 = std::make_shared<FullMatrix<double>>(ListFRD{ zero, m2m2, a2m1 });
auto mat11 = std::make_shared<FullMatrix<double>>(ListFRD{ m2m0, a2m1, m2m2 });
auto mat12 = std::make_shared<FullMatrix<double>>(ListFRD{ zero, a2m2, a2m1 });
auto mat13 = std::make_shared<FullMatrix<double>>(ListFRD{ a2m2, zero, m2m0 });
auto mat22 = std::make_shared<FullMatrix<double>>(ListFRD{ m2m0, m2m1, a2m2 });
auto mat23 = std::make_shared<FullMatrix<double>>(ListFRD{ m2m1, a2m0, zero });
auto mat33 = std::make_shared<FullMatrix<double>>(ListFRD{ a2m0, a2m1, a2m2 });
auto answer = std::make_shared<FullMatrix<FMatDsptr>>(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<>
inline void EulerParameters<double>::initialize()
{
aA = FullMatrix<double>::identitysptr(3);
aB = std::make_shared<FullMatrix<double>>(3, 4);
aC = std::make_shared<FullMatrix<double>>(3, 4);
pApE = std::make_shared<FullColumn<FMatDsptr>>(4);
for (int i = 0; i < 4; i++)
namespace MbD {
template<>
FMatFColDsptr EulerParameters<double>::ppApEpEtimesColumn(FColDsptr col)
{
pApE->at(i) = std::make_shared<FullMatrix<double>>(3, 3);
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<typename T>
inline void EulerParameters<T>::calc()
{
this->calcABC();
this->calcpApE();
}
template<>
inline 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<>
inline 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>
inline void EulerParameters<T>::conditionSelf()
{
EulerArray<T>::conditionSelf();
this->normalizeSelf();
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>;
}

11
OndselSolver/EulerParameters.h
View File

@@ -8,10 +8,12 @@
#pragma once
#include "FullColumn.ref.h"
#include "FullMatrix.ref.h"
#include "EulerParameters.ref.h"
#include "EulerArray.h"
#include "FullColumn.h"
#include "FullMatrix.h"
// #include "FullColumn.h"
// #include "FullMatrix.h"
namespace MbD {
@@ -40,11 +42,10 @@ namespace MbD {
this->initialize();
this->calc();
}
static std::shared_ptr<FullMatrix<FColsptr<T>>> ppApEpEtimesColumn(FColDsptr col);
static std::shared_ptr<FullMatrixFullColumnDouble> ppApEpEtimesColumn(FColDsptr col);
static FMatDsptr pCpEtimesColumn(FColDsptr col);
static FMatDsptr pCTpEtimesColumn(FColDsptr col);
static std::shared_ptr<FullMatrix<FMatsptr<T>>> ppApEpEtimesMatrix(FMatDsptr mat);
static std::shared_ptr<FullMatrixFullMatrixDouble> ppApEpEtimesMatrix(FMatDsptr mat);
void initialize() override;

10
OndselSolver/EulerParametersDot.h
View File

@@ -55,13 +55,13 @@ namespace MbD {
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);
aAdot = std::make_shared<FullMatrixDouble>(3, 3);
aBdot = std::make_shared<FullMatrixDouble>(3, 4);
aCdot = std::make_shared<FullMatrixDouble>(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);
pAdotpE->at(i) = std::make_shared<FullMatrixDouble>(3, 3);
}
}
@@ -185,7 +185,7 @@ namespace MbD {
inline FColDsptr EulerParametersDot<T>::omeOpO()
{
auto aaa = this->aB();
auto bbb = aaa->timesFullColumn(this);
auto bbb = aaa->timesFullColumn((MbD::FColsptr<double>)this);
auto ccc = bbb->times(2.0);
return ccc;
//return this->aB->timesFullColumn(this)->times(2.0);

162
OndselSolver/FullColumn.cpp
View File

@@ -9,5 +9,165 @@
#include <sstream>
#include "FullColumn.h"
#include "FullRow.h"
#include "FullMatrix.h"
using namespace MbD;
namespace MbD {
template<typename T>
FColsptr<T> FullColumn<T>::plusFullColumn(FColsptr<T> fullCol)
{
int n = (int) this->size();
auto answer = std::make_shared<FullColumn<T>>(n);
for (int i = 0; i < n; i++) {
answer->at(i) = this->at(i) + fullCol->at(i);
}
return answer;
}
template<typename T>
FColsptr<T> FullColumn<T>::minusFullColumn(FColsptr<T> fullCol)
{
int n = (int) this->size();
auto answer = std::make_shared<FullColumn<T>>(n);
for (int i = 0; i < n; i++) {
answer->at(i) = this->at(i) - fullCol->at(i);
}
return answer;
}
template<>
FColDsptr FullColumn<double>::times(double a)
{
int n = (int)this->size();
auto answer = std::make_shared<FullColumn<double>>(n);
for (int i = 0; i < n; i++) {
answer->at(i) = this->at(i) * a;
}
return answer;
}
template<typename T>
FColsptr<T> FullColumn<T>::times(T a)
{
assert(false);
auto answer = std::make_shared<FullColumn<T>>();
return answer;
}
template<typename T>
FColsptr<T> FullColumn<T>::negated()
{
return this->times(-1.0);
}
template<typename T>
void FullColumn<T>::atiputFullColumn(int i, FColsptr<T> fullCol)
{
for (int ii = 0; ii < fullCol->size(); ii++)
{
this->at(i + ii) = fullCol->at(ii);
}
}
template<typename T>
void FullColumn<T>::atiplusFullColumn(int i, FColsptr<T> fullCol)
{
for (int ii = 0; ii < fullCol->size(); ii++)
{
this->at(i + ii) += fullCol->at(ii);
}
}
template<typename T>
void FullColumn<T>::equalSelfPlusFullColumnAt(FColsptr<T> fullCol, int ii)
{
//self is subcolumn of fullCol
for (int i = 0; i < this->size(); i++)
{
this->at(i) += fullCol->at(ii + i);
}
}
template<typename T>
void FullColumn<T>::atiminusFullColumn(int i1, FColsptr<T> fullCol)
{
for (int ii = 0; ii < fullCol->size(); ii++)
{
int i = i1 + ii;
this->at(i) -= fullCol->at(ii);
}
}
template<typename T>
void FullColumn<T>::equalFullColumnAt(FColsptr<T> fullCol, int i)
{
this->equalArrayAt(fullCol, i);
//for (int ii = 0; ii < this->size(); ii++)
//{
// this->at(ii) = fullCol->at(i + ii);
//}
}
template<>
FColDsptr FullColumn<double>::copy()
{
auto n = (int) this->size();
auto answer = std::make_shared<FullColumn<double>>(n);
for (int i = 0; i < n; i++)
{
answer->at(i) = this->at(i);
}
return answer;
}
template<typename T>
FRowsptr<T> FullColumn<T>::transpose()
{
return std::make_shared<FullRow<T>>(*this);
}
template<typename T>
void FullColumn<T>::atiplusFullColumntimes(int i1, FColsptr<T> fullCol, T factor)
{
for (int ii = 0; ii < fullCol->size(); ii++)
{
int i = i1 + ii;
this->at(i) += fullCol->at(ii) * factor;
}
}
template<typename T>
T FullColumn<T>::transposeTimesFullColumn(const FColsptr<T> fullCol)
{
return this->dot(fullCol);
}
template<typename T>
void FullColumn<T>::equalSelfPlusFullColumntimes(FColsptr<T> fullCol, T factor)
{
this->equalSelfPlusFullVectortimes(fullCol, factor);
}
template<typename T>
FColsptr<T> FullColumn<T>::cross(FColsptr<T> fullCol)
{
auto a0 = this->at(0);
auto a1 = this->at(1);
auto a2 = this->at(2);
auto b0 = fullCol->at(0);
auto b1 = fullCol->at(1);
auto b2 = fullCol->at(2);
auto answer = std::make_shared<FullColumn<T>>(3);
answer->atiput(0, a1 * b2 - (a2 * b1));
answer->atiput(1, a2 * b0 - (a0 * b2));
answer->atiput(2, a0 * b1 - (a1 * b0));
return answer;
}
//template<>
//inline std::shared_ptr<FullColumn<Symsptr>> FullColumn<Symsptr>::simplified()
//{
// auto n = this->size();
// auto answer = std::make_shared<FullColumn<Symsptr>>(n);
// for (int i = 0; i < n; i++)
// {
// auto func = this->at(i);
// answer->at(i) = func->simplified(func);
// }
// return answer;
//}
template<typename T>
FColsptr<T> FullColumn<T>::simplified()
{
// assert(false);
return FColsptr<T>();
}
// instantiate on purpose to make visible in library api:
template class FullColumn<double>;
template class FullColumn<int>;
}

190
OndselSolver/FullColumn.h
View File

@@ -14,19 +14,10 @@
#include "FullVector.h"
#include "FullColumn.ref.h"
#include "FullRow.ref.h"
#include "FullColumn.h"
#include "FullRow.h"
#include "FullMatrix.ref.h"
//#include "Symbolic.h"
namespace MbD {
// template<typename T>
// class FullColumn;
// using FColDsptr = std::shared_ptr<FullColumn<double>>;
// template<typename T>
// using FColsptr = std::shared_ptr<FullColumn<T>>;
// template<typename T>
// class FullRow;
// template<typename T>
// using FRowsptr = std::shared_ptr<FullRow<T>>;
class Symbolic;
template<typename T>
@@ -58,168 +49,17 @@ namespace MbD {
std::ostream& printOn(std::ostream& s) const override;
};
template<typename T>
inline FColsptr<T> FullColumn<T>::plusFullColumn(FColsptr<T> fullCol)
{
int n = (int) this->size();
auto answer = std::make_shared<FullColumn<T>>(n);
for (int i = 0; i < n; i++) {
answer->at(i) = this->at(i) + fullCol->at(i);
}
return answer;
}
template<typename T>
inline FColsptr<T> FullColumn<T>::minusFullColumn(FColsptr<T> fullCol)
{
int n = (int) this->size();
auto answer = std::make_shared<FullColumn<T>>(n);
for (int i = 0; i < n; i++) {
answer->at(i) = this->at(i) - fullCol->at(i);
}
return answer;
}
template<>
inline FColDsptr FullColumn<double>::times(double a)
{
int n = (int)this->size();
auto answer = std::make_shared<FullColumn<double>>(n);
for (int i = 0; i < n; i++) {
answer->at(i) = this->at(i) * a;
}
return answer;
}
template<typename T>
inline FColsptr<T> FullColumn<T>::times(T a)
{
assert(false);
}
template<typename T>
inline FColsptr<T> FullColumn<T>::negated()
{
return this->times(-1.0);
}
template<typename T>
inline void FullColumn<T>::atiputFullColumn(int i, FColsptr<T> fullCol)
{
for (int ii = 0; ii < fullCol->size(); ii++)
{
this->at(i + ii) = fullCol->at(ii);
}
}
template<typename T>
inline void FullColumn<T>::atiplusFullColumn(int i, FColsptr<T> fullCol)
{
for (int ii = 0; ii < fullCol->size(); ii++)
{
this->at(i + ii) += fullCol->at(ii);
}
}
template<typename T>
inline void FullColumn<T>::equalSelfPlusFullColumnAt(FColsptr<T> fullCol, int ii)
{
//self is subcolumn of fullCol
for (int i = 0; i < this->size(); i++)
{
this->at(i) += fullCol->at(ii + i);
}
}
template<typename T>
inline void FullColumn<T>::atiminusFullColumn(int i1, FColsptr<T> fullCol)
{
for (int ii = 0; ii < fullCol->size(); ii++)
{
int i = i1 + ii;
this->at(i) -= fullCol->at(ii);
}
}
template<typename T>
inline void FullColumn<T>::equalFullColumnAt(FColsptr<T> fullCol, int i)
{
this->equalArrayAt(fullCol, i);
//for (int ii = 0; ii < this->size(); ii++)
//{
// this->at(ii) = fullCol->at(i + ii);
//}
}
template<>
inline FColDsptr FullColumn<double>::copy()
{
auto n = (int) this->size();
auto answer = std::make_shared<FullColumn<double>>(n);
for (int i = 0; i < n; i++)
{
answer->at(i) = this->at(i);
}
return answer;
}
template<typename T>
inline FRowsptr<T> FullColumn<T>::transpose()
{
return std::make_shared<FullRow<T>>(*this);
}
template<typename T>
inline void FullColumn<T>::atiplusFullColumntimes(int i1, FColsptr<T> fullCol, T factor)
{
for (int ii = 0; ii < fullCol->size(); ii++)
{
int i = i1 + ii;
this->at(i) += fullCol->at(ii) * factor;
}
}
template<typename T>
inline T FullColumn<T>::transposeTimesFullColumn(const FColsptr<T> fullCol)
{
return this->dot(fullCol);
}
template<typename T>
inline void FullColumn<T>::equalSelfPlusFullColumntimes(FColsptr<T> fullCol, T factor)
{
this->equalSelfPlusFullVectortimes(fullCol, factor);
}
template<typename T>
inline FColsptr<T> FullColumn<T>::cross(FColsptr<T> fullCol)
{
auto a0 = this->at(0);
auto a1 = this->at(1);
auto a2 = this->at(2);
auto b0 = fullCol->at(0);
auto b1 = fullCol->at(1);
auto b2 = fullCol->at(2);
auto answer = std::make_shared<FullColumn<T>>(3);
answer->atiput(0, a1 * b2 - (a2 * b1));
answer->atiput(1, a2 * b0 - (a0 * b2));
answer->atiput(2, a0 * b1 - (a1 * b0));
return answer;
}
//template<>
//inline std::shared_ptr<FullColumn<Symsptr>> FullColumn<Symsptr>::simplified()
//{
// auto n = this->size();
// auto answer = std::make_shared<FullColumn<Symsptr>>(n);
// for (int i = 0; i < n; i++)
// {
// auto func = this->at(i);
// answer->at(i) = func->simplified(func);
// }
// return answer;
//}
template<typename T>
inline FColsptr<T> FullColumn<T>::simplified()
{
assert(false);
return FColsptr<T>();
}
template<typename T>
inline std::ostream& FullColumn<T>::printOn(std::ostream& s) const
{
s << "FullCol{";
s << this->at(0);
for (int i = 1; i < this->size(); i++)
{
s << ", " << this->at(i);
}
s << "}";
return s;
}
// the following "printOn" needs to be in the header for unknown reasons linker
template<typename T>
std::ostream& FullColumn<T>::printOn(std::ostream& s) const
{
s << "FullCol{";
s << this->at(0);
for (int i = 1; i < int(this->size()); i++)
{
s << ", " << this->at(i);
}
s << "}";
return s;
}
}

3
OndselSolver/FullColumn.ref.h
View File

@@ -1,5 +1,8 @@
#pragma once
#include <algorithm>
#include <memory>
namespace MbD {
template<typename T>
class FullColumn;

1216
OndselSolver/FullMatrix.cpp
View File

File diff suppressed because it is too large Load Diff

195
OndselSolver/FullMatrix.h
View File

@@ -17,82 +17,133 @@
#include "DiagonalMatrix.ref.h"
#include "EulerParameters.ref.h"
#include "RowTypeMatrix.h"
#include "FullRow.h" // now that refs are resolved, go do the full systems
//#include "FullColumn.h"
//#include "DiagonalMatrix.h"
//#include "EulerParameters.h"
namespace MbD {
//
// FULL MATRIX DOUBLE
//
class FullMatrixDouble : public RowTypeMatrix<FRowsptr<double>> {
public:
FullMatrixDouble() = default;
explicit FullMatrixDouble(int m) : RowTypeMatrix<FRowsptr<double>>(m)
{
}
FullMatrixDouble(int m, int n) {
for (int i = 0; i < m; i++) {
auto row = std::make_shared<FullRow<double>>(n);
this->push_back(row);
}
}
FullMatrixDouble(std::initializer_list<FRowsptr<double>> listOfRows) {
for (auto& row : listOfRows)
{
this->push_back(row);
}
}
FullMatrixDouble(std::initializer_list<std::initializer_list<double>> list2D) {
for (auto& rowList : list2D)
{
auto row = std::make_shared<FullRow<double>>(rowList);
this->push_back(row);
}
}
template<typename T>
class FullMatrix : public RowTypeMatrix<FRowsptr<T>>
{
public:
FullMatrix() {}
FullMatrix(int m) : RowTypeMatrix<FRowsptr<T>>(m)
std::shared_ptr<FullMatrixDouble> times(double a);
std::shared_ptr<FullMatrixDouble> timesTransposeFullMatrix(std::shared_ptr<FullMatrixDouble> fullMat);
void identity();
static std::shared_ptr<MbD::FullMatrixDouble> identitysptr(int n);
double sumOfSquares() override;
std::shared_ptr<FullMatrixDouble> transposeTimesFullMatrix(std::shared_ptr<FullMatrixDouble> fullMat);
std::shared_ptr<FullMatrixDouble> timesFullMatrix(std::shared_ptr<FullMatrixDouble> fullMat);
std::shared_ptr<FullMatrixDouble> transpose();
std::shared_ptr<FullMatrixDouble> negated();
std::shared_ptr<FullMatrixDouble> plusFullMatrix(std::shared_ptr<FullMatrixDouble> fullMat);
static std::shared_ptr<FullMatrixDouble> rotatex(double angle);
static std::shared_ptr<FullMatrixDouble> rotatey(double angle);
static std::shared_ptr<FullMatrixDouble> rotatez(double angle);
static std::shared_ptr<FullMatrixDouble> rotatexrotDot(double angle, double angledot);
static std::shared_ptr<FullMatrixDouble> rotateyrotDot(double angle, double angledot);
static std::shared_ptr<FullMatrixDouble> rotatezrotDot(double angle, double angledot);
static std::shared_ptr<FullMatrixDouble> rotatexrotDotrotDDot(double angle, double angleDot, double angleDDot);
static std::shared_ptr<FullMatrixDouble> rotateyrotDotrotDDot(double angle, double angleDot, double angleDDot);
static std::shared_ptr<FullMatrixDouble> rotatezrotDotrotDDot(double angle, double angleDot, double angleDDot);
static std::shared_ptr<FullMatrixDouble> tildeMatrix(FColDsptr col);
void zeroSelf() override;
FColsptr<double> column(int j);
void atiput(int i, FRowsptr<double> fullRow) override;
void atijput(int i, int j, double value);
std::shared_ptr<FullMatrixDouble> copy();
double maxMagnitude() override;
FullMatrixDouble operator+(const FullMatrixDouble fullMat);
std::shared_ptr<FullMatrixDouble> minusFullMatrix(std::shared_ptr<FullMatrixDouble> fullMat);
FColsptr<double> transposeTimesFullColumn(FColsptr<double> fullCol);
void symLowerWithUpper();
void atijputFullColumn(int i1, int j1, FColsptr<double> fullCol);
void atijplusFullRow(int i, int j, FRowsptr<double> fullRow);
void atijplusNumber(int i, int j, double value);
void atijminusNumber(int i, int j, double value);
void magnifySelf(double factor);
std::shared_ptr<EulerParameters<double>> asEulerParameters();
FColsptr<double> bryantAngles();
double trace();
bool isDiagonal();
bool isDiagonalToWithin(double ratio);
std::shared_ptr<DiagonalMatrix> asDiagonalMatrix();
void conditionSelfWithTol(double tol);
std::ostream& printOn(std::ostream& s) const override;
FColsptr<double> timesFullColumn(FColsptr<double> fullCol);
// FColsptr<double> timesFullColumn(FullColumn<double>* fullCol);
};
//
// FULL MATRIX FULL MATRIX DOUBLE
//
class FullMatrixFullMatrixDouble : public RowTypeMatrix<FRowsptr<FMatDsptr>> {
public:
FullMatrixFullMatrixDouble() = default;
explicit FullMatrixFullMatrixDouble(int m) : RowTypeMatrix<FRowsptr<FMatDsptr>>(m)
{
}
FullMatrix(int m, int n) {
for (int i = 0; i < m; i++) {
auto row = std::make_shared<FullRow<T>>(n);
this->push_back(row);
}
}
FullMatrix(std::initializer_list<FRowsptr<T>> listOfRows) {
for (auto& row : listOfRows)
{
this->push_back(row);
}
}
FullMatrix(std::initializer_list<std::initializer_list<T>> list2D) {
for (auto& rowList : list2D)
{
auto row = std::make_shared<FullRow<T>>(rowList);
this->push_back(row);
}
}
static FMatsptr<T> rotatex(T angle);
static FMatsptr<T> rotatey(T angle);
static FMatsptr<T> rotatez(T angle);
static FMatsptr<T> rotatexrotDot(T angle, T angledot);
static FMatsptr<T> rotateyrotDot(T angle, T angledot);
static FMatsptr<T> rotatezrotDot(T angle, T angledot);
static FMatsptr<T> rotatexrotDotrotDDot(T angle, T angleDot, T angleDDot);
static FMatsptr<T> rotateyrotDotrotDDot(T angle, T angleDot, T angleDDot);
static FMatsptr<T> rotatezrotDotrotDDot(T angle, T angleDot, T angleDDot);
static FMatsptr<T> identitysptr(int n);
static FMatsptr<T> tildeMatrix(FColDsptr col);
void identity();
FColsptr<T> column(int j);
FColsptr<T> timesFullColumn(FColsptr<T> fullCol);
FColsptr<T> timesFullColumn(FullColumn<T>* fullCol);
FMatsptr<T> timesFullMatrix(FMatsptr<T> fullMat);
FMatsptr<T> timesTransposeFullMatrix(FMatsptr<T> fullMat);
FMatsptr<T> times(T a);
FMatsptr<T> transposeTimesFullMatrix(FMatsptr<T> fullMat);
FMatsptr<T> plusFullMatrix(FMatsptr<T> fullMat);
FMatsptr<T> minusFullMatrix(FMatsptr<T> fullMat);
FMatsptr<T> transpose();
FMatsptr<T> negated();
void symLowerWithUpper();
void atiput(int i, FRowsptr<T> fullRow) override;
void atijput(int i, int j, T value);
void atijputFullColumn(int i, int j, FColsptr<T> fullCol);
void atijplusFullRow(int i, int j, FRowsptr<T> fullRow);
void atijplusNumber(int i, int j, T value);
void atijminusNumber(int i, int j, T value);
double sumOfSquares() override;
void zeroSelf() override;
FMatsptr<T> copy();
FullMatrix<T> operator+(const FullMatrix<T> fullMat);
FColsptr<T> transposeTimesFullColumn(const FColsptr<T> fullCol);
void magnifySelf(T factor);
std::shared_ptr<EulerParameters<T>> asEulerParameters();
T trace();
double maxMagnitude() override;
FColsptr<T> bryantAngles();
bool isDiagonal();
bool isDiagonalToWithin(double ratio);
std::shared_ptr<DiagonalMatrix<T>> asDiagonalMatrix();
void conditionSelfWithTol(double tol);
FullMatrixFullMatrixDouble(int m, int n) {
for (int i = 0; i < m; i++) {
auto row = std::make_shared<FullRow<FMatDsptr>>(n);
this->push_back(row);
}
}
std::ostream& printOn(std::ostream& s) const override;
};
double maxMagnitude() override;
void zeroSelf() override;
std::shared_ptr<FullMatrixFullMatrixDouble> times(double a);
// std::shared_ptr<FullMatrixFullMatrixDouble> timesTransposeFullMatrix(std::shared_ptr<FullMatrixFullMatrixDouble> fullMat);
double sumOfSquares() override;
void identity();
static std::shared_ptr<MbD::FullMatrixFullMatrixDouble> identitysptr(int n);
};
//
// FULL MATRIX FULL COLUMN DOUBLE
//
class FullMatrixFullColumnDouble : public RowTypeMatrix<FRowsptr<FColDsptr>> {
public:
FullMatrixFullColumnDouble() = default;
explicit FullMatrixFullColumnDouble(int m) : RowTypeMatrix<FRowsptr<FColDsptr>>(m)
{
}
FullMatrixFullColumnDouble(int m, int n) {
for (int i = 0; i < m; i++) {
auto row = std::make_shared<FullRow<FColDsptr>>(n);
this->push_back(row);
}
}
double maxMagnitude() override;
void zeroSelf() override;
double sumOfSquares() override;
void symLowerWithUpper();
std::shared_ptr<FullMatrixFullColumnDouble> times(double a);
};
}

15
OndselSolver/FullMatrix.ref.h
View File

@@ -1,20 +1,19 @@
#pragma once
#include "FullColumn.ref.h"
#include <algorithm>
namespace MbD {
template<typename T>
class FullMatrix;
class FullMatrixDouble;
class FullMatrixFullMatrixDouble;
class FullMatrixFullColumnDouble;
using FMatDsptr = std::shared_ptr<MbD::FullMatrix<double>>;
using FMatDsptr = std::shared_ptr<FullMatrixDouble>;
template<typename T>
using FMatsptr = std::shared_ptr<FullMatrix<T>>;
using FMatFColDsptr = std::shared_ptr<FullMatrix<FColDsptr>>;
using FMatFMatDsptr = std::shared_ptr<FullMatrix<FMatDsptr>>;
using FMatFMatDsptr = std::shared_ptr<FullMatrixFullMatrixDouble>;
using FColFMatDsptr = std::shared_ptr<FullColumn<FMatDsptr>>;
using FMatFColDsptr = std::shared_ptr<FullMatrixFullColumnDouble>;
}

78
OndselSolver/FullRow.cpp
View File

@@ -9,41 +9,55 @@
#include "FullRow.h"
#include "FullMatrix.h"
using namespace MbD;
template<typename T>
FMatsptr<T> FullRow<T>::transposeTimesFullRow(FRowsptr<T> fullRow)
{
//"a*b = a(i)b(j)"
auto nrow = (int)this->size();
auto answer = std::make_shared<FullMatrix<double>>(nrow);
for (int i = 0; i < nrow; i++)
namespace MbD {
template<>
std::shared_ptr<FullMatrixDouble> FullRow<double>::transposeTimesFullRow(FRowsptr<double> fullRow)
{
answer->atiput(i, fullRow->times(this->at(i)));
//"a*b = a(i)b(j)"
auto nrow = (int)this->size();
auto answer = std::make_shared<FullMatrixDouble>(nrow);
for (int i = 0; i < nrow; i++)
{
answer->atiput(i, fullRow->times(this->at(i)));
}
return answer;
}
return answer;
}
template<typename T>
inline FRowsptr<T> FullRow<T>::timesTransposeFullMatrix(FMatsptr<T> fullMat)
{
//"a*bT = a(1,j)b(k,j)"
int ncol = fullMat->nrow();
auto answer = std::make_shared<FullRow<T>>(ncol);
for (int k = 0; k < ncol; k++) {
answer->at(k) = this->dot(fullMat->at(k));
}
return answer;
}
template<typename T>
inline FRowsptr<T> FullRow<T>::timesFullMatrix(FMatsptr<T> fullMat)
{
FRowsptr<T> answer = fullMat->at(0)->times(this->at(0));
for (int j = 1; j < (int) this->size(); j++)
template<>
FRowsptr<double> FullRow<double>::timesTransposeFullMatrix(std::shared_ptr<FullMatrixDouble> fullMat)
{
answer->equalSelfPlusFullRowTimes(fullMat->at(j), this->at(j));
//"a*bT = a(1,j)b(k,j)"
int ncol = fullMat->nrow();
auto answer = std::make_shared<FullRow<double>>(ncol);
for (int k = 0; k < ncol; k++) {
answer->at(k) = this->dot(fullMat->at(k));
}
return answer;
}
// TODO: can't get the following to work, but CLion says the routine that calls it in FullMatrixFullMatrixDouble is also
// never called.
// template<>
// FRowsptr<std::shared_ptr<FullMatrixDouble>> FullRow<std::shared_ptr<FullMatrixDouble>>::timesTransposeFullMatrixForFMFMDsptr(
// std::shared_ptr<FullMatrixFullMatrixDouble> fullMat)
// {
// //"a*bT = a(1,j)b(k,j)"
// int ncol = fullMat->nrow();
// auto answer = std::make_shared<FullRow<std::shared_ptr<FullMatrixDouble>>>(ncol);
// for (int k = 0; k < ncol; k++) {
// answer->at(k) = this->dot(fullMat->at(k));
// }
// return answer;
// }
template<>
FRowsptr<double> FullRow<double>::timesFullMatrix(std::shared_ptr<FullMatrixDouble> fullMat)
{
FRowsptr<double> answer = fullMat->at(0)->times(this->at(0));
for (int j = 1; j < (int) this->size(); j++)
{
answer->equalSelfPlusFullRowTimes(fullMat->at(j), this->at(j));
}
return answer;
}
return answer;
//return FRowsptr<T>();
}

12
OndselSolver/FullRow.h
View File

@@ -30,17 +30,18 @@ namespace MbD {
FRowsptr<T> minusFullRow(FRowsptr<T> fullRow);
T timesFullColumn(FColsptr<T> fullCol);
T timesFullColumn(FullColumn<T>* fullCol);
FRowsptr<T> timesFullMatrix(std::shared_ptr<FullMatrix<T>> fullMat);
FRowsptr<T> timesTransposeFullMatrix(std::shared_ptr<FullMatrix<T>> fullMat);
void equalSelfPlusFullRowTimes(FRowsptr<T> fullRow, double factor);
void equalFullRow(FRowsptr<T> fullRow);
FColsptr<T> transpose();
FRowsptr<T> copy();
void atiplusFullRow(int j, FRowsptr<T> fullRow);
std::shared_ptr<FullMatrix<T>> transposeTimesFullRow(FRowsptr<T> fullRow);
std::ostream& printOn(std::ostream& s) const override;
};
std::shared_ptr<FullMatrixDouble> transposeTimesFullRow(FRowsptr<double> fullRow);
FRowsptr<double> timesTransposeFullMatrix(std::shared_ptr<FullMatrixDouble> fullMat);
// FRowsptr<std::shared_ptr<FullMatrixDouble>> timesTransposeFullMatrixForFMFMDsptr(std::shared_ptr<FullMatrixFullMatrixDouble> fullMat);
FRowsptr<double> timesFullMatrix(std::shared_ptr<FullMatrixDouble> fullMat);
};
template<>
inline FRowDsptr FullRow<double>::times(double a)
@@ -143,6 +144,7 @@ namespace MbD {
}
s << "}";
return s;
}
};
}

15
OndselSolver/FullVector.h
View File

@@ -66,7 +66,7 @@ namespace MbD {
inline double FullVector<double>::sumOfSquares()
{
double sum = 0.0;
for (int i = 0; i < this->size(); i++)
for (int i = 0; i < (int)this->size(); i++)
{
double element = this->at(i);
sum += element * element;
@@ -87,7 +87,7 @@ namespace MbD {
template<>
inline void FullVector<double>::zeroSelf()
{
for (int i = 0; i < this->size(); i++) {
for (int i = 0; i < (int)this->size(); i++) {
this->at(i) = 0.0;
}
}
@@ -126,7 +126,7 @@ namespace MbD {
inline double FullVector<double>::maxMagnitude()
{
double max = 0.0;
for (int i = 0; i < this->size(); i++)
for (int i = 0; i < (int)this->size(); i++)
{
double element = this->at(i);
if (element < 0.0) element = -element;
@@ -168,7 +168,7 @@ namespace MbD {
template<>
inline void FullVector<double>::conditionSelfWithTol(double tol)
{
for (int i = 0; i < this->size(); i++)
for (int i = 0; i < (int)this->size(); i++)
{
double element = this->at(i);
if (element < 0.0) element = -element;
@@ -178,15 +178,16 @@ namespace MbD {
template<typename T>
inline void FullVector<T>::conditionSelfWithTol(double tol)
{
assert(false);
return;
assert(false && tol != tol); // clang++ flips out with warnings if you don't use 'tol'
// but suppressing that warning breaks Visual Studio.
return; // Visual Studio demands the unused return
}
template<typename T>
inline std::ostream& FullVector<T>::printOn(std::ostream& s) const
{
s << "FullVec{";
s << this->at(0);
for (int i = 1; i < this->size(); i++)
for (int i = 1; i < (int)this->size(); i++)
{
s << ", " << this->at(i);
}

3
OndselSolver/GearConstraintIqcJc.cpp
View File

@@ -62,7 +62,8 @@ void MbD::GearConstraintIqcJc::calc_ppGpXIpEI()
void MbD::GearConstraintIqcJc::calc_ppGpXIpXI()
{
ppGpXIpXI = orbitJeIe->ppvaluepXJpXJ()->plusFullMatrix(orbitIeJe->ppvaluepXIpXI()->times(this->ratio()));
ppGpXIpXI = orbitJeIe->ppvaluepXJpXJ()
->plusFullMatrix(orbitIeJe->ppvaluepXIpXI()->times(this->ratio()));
}
void MbD::GearConstraintIqcJc::calcPostDynCorrectorIteration()

2
OndselSolver/GeneralSpline.cpp
View File

@@ -119,7 +119,7 @@ void MbD::GeneralSpline::computeDerivatives()
}
auto solver = CREATE<GESpMatParPvMarkoFast>::With();
auto derivsVector = solver->solvewithsaveOriginal(matrix, bvector, false);
derivs = std::make_shared<FullMatrix<double>>(n, p);
derivs = std::make_shared<FullMatrixDouble>(n, p);
auto hmaxpowers = std::make_shared<FullColumn<double>>(p);
for (int j = 0; j < p; j++)
{

6
OndselSolver/Item.h
View File

@@ -10,8 +10,12 @@
#include <string>
#include <vector>
#include "FullColumn.ref.h"
#include "FullRow.ref.h"
#include "DiagonalMatrix.ref.h"
#include "FullMatrix.ref.h"
#include "FullColumn.h"
#include "FullRow.h"
#include "FullMatrix.h"
#include "DiagonalMatrix.h"
#include "SparseMatrix.h"

2
OndselSolver/LDUFullMat.cpp
View File

@@ -116,7 +116,7 @@ FMatDsptr LDUFullMat::inversesaveOriginal(FMatDsptr fullMat, bool saveOriginal)
this->decomposesaveOriginal(fullMat, saveOriginal);
rightHandSideB = std::make_shared<FullColumn<double>>(m);
auto matrixAinverse = std::make_shared <FullMatrix<double>>(m, n);
auto matrixAinverse = std::make_shared <FullMatrixDouble>(m, n);
for (int j = 0; j < n; j++)
{
rightHandSideB->zeroSelf();

6
OndselSolver/MBDynBody.cpp
View File

@@ -60,7 +60,7 @@ void MbD::MBDynBody::readInertiaMatrix(std::vector<std::string>& args)
{
auto parser = std::make_shared<SymbolicParser>();
parser->variables = mbdynVariables();
aJmat = std::make_shared<FullMatrix<double>>(3, 3);
aJmat = std::make_shared<FullMatrixDouble>(3, 3);
auto& str = args.at(0);
if (str.find("diag") != std::string::npos) {
args.erase(args.begin());
@@ -89,7 +89,7 @@ void MbD::MBDynBody::createASMT()
if (aJmat->isDiagonalToWithin(1.0e-6)) {
asmtMassMarker->setMomentOfInertias(aJmat->asDiagonalMatrix());
asmtMassMarker->setPosition3D(rPcmP);
asmtMassMarker->setRotationMatrix(FullMatrix<double>::identitysptr(3));
asmtMassMarker->setRotationMatrix(FullMatrixDouble::identitysptr(3));
auto asmtPart = asmtAssembly()->partPartialNamed(nodeName);
asmtPart->setPrincipalMassMarker(asmtMassMarker);
}
@@ -98,7 +98,7 @@ void MbD::MBDynBody::createASMT()
solver->setm(mass);
solver->setJPP(aJmat);
solver->setrPoP(rPcmP);
solver->setAPo(FullMatrix<double>::identitysptr(3));
solver->setAPo(FullMatrixDouble::identitysptr(3));
solver->setrPcmP(rPcmP);
solver->calc();
asmtMassMarker->setMomentOfInertias(solver->aJpp);

10
OndselSolver/MBDynItem.cpp
View File

@@ -155,7 +155,7 @@ FColDsptr MbD::MBDynItem::readBasicPosition(std::vector<std::string>& args)
FMatDsptr MbD::MBDynItem::readOrientation(std::vector<std::string>& args)
{
auto aAOf = FullMatrix<double>::identitysptr(3);
auto aAOf = FullMatrixDouble::identitysptr(3);
if (args.empty()) return aAOf;
auto& str = args.at(0);
if (str.find("reference") != std::string::npos) {
@@ -202,7 +202,7 @@ FMatDsptr MbD::MBDynItem::readBasicOrientation(std::vector<std::string>& args)
}
if (str.find("eye") != std::string::npos) {
args.erase(args.begin());
auto aAFf = FullMatrix<double>::identitysptr(3);
auto aAFf = FullMatrixDouble::identitysptr(3);
return aAFf;
}
auto iss = std::istringstream(str);
@@ -251,7 +251,7 @@ FMatDsptr MbD::MBDynItem::readBasicOrientation(std::vector<std::string>& args)
else {
assert(false);
}
auto aAFf = FullMatrix<double>::identitysptr(3);
auto aAFf = FullMatrixDouble::identitysptr(3);
aAFf->atijputFullColumn(0, 0, vecX);
aAFf->atijputFullColumn(0, 1, vecY);
aAFf->atijputFullColumn(0, 2, vecZ);
@@ -300,13 +300,13 @@ FMatDsptr MbD::MBDynItem::readBasicOrientation(std::vector<std::string>& args)
else {
assert(false);
}
auto aAFf = FullMatrix<double>::identitysptr(3);
auto aAFf = FullMatrixDouble::identitysptr(3);
aAFf->atijputFullColumn(0, 0, vecX);
aAFf->atijputFullColumn(0, 1, vecY);
aAFf->atijputFullColumn(0, 2, vecZ);
return aAFf;
}
auto aAFf = FullMatrix<double>::identitysptr(3);
auto aAFf = FullMatrixDouble::identitysptr(3);
for (int i = 0; i < 3; i++)
{
auto rowi = aAFf->at(i);

2
OndselSolver/MBDynJoint.cpp
View File

@@ -118,7 +118,7 @@ void MbD::MBDynJoint::parseMBDyn(std::string line)
mkr1->owner = this;
mkr1->nodeStr = "Assembly";
mkr1->rPmP = std::make_shared<FullColumn<double>>(3);
mkr1->aAPm = FullMatrix<double>::identitysptr(3);
mkr1->aAPm = FullMatrixDouble::identitysptr(3);
readMarkerJ(arguments);
return;
}

2
OndselSolver/MBDynMarker.cpp
View File

@@ -11,7 +11,7 @@ using namespace MbD;
void MbD::MBDynMarker::parseMBDyn(std::vector<std::string>& args)
{
rPmP = std::make_shared<FullColumn<double>>(3);
aAPm = FullMatrix<double>::identitysptr(3);
aAPm = FullMatrixDouble::identitysptr(3);
if (args.empty()) return;
auto& str = args.at(0);
if (str.find("reference") != std::string::npos) {

2
OndselSolver/MBDynStructural.cpp
View File

@@ -12,7 +12,7 @@ using namespace MbD;
MbD::MBDynStructural::MBDynStructural()
{
rOfO = std::make_shared<FullColumn<double>>(3);
aAOf = FullMatrix<double>::identitysptr(3);
aAOf = FullMatrixDouble::identitysptr(3);
vOfO = std::make_shared<FullColumn<double>>(3);
omeOfO = std::make_shared<FullColumn<double>>(3);
}

2
OndselSolver/MarkerFrame.cpp
View File

@@ -31,7 +31,7 @@ System* MarkerFrame::root()
void MarkerFrame::initialize()
{
prOmOpE = std::make_shared<FullMatrix<double>>(3, 4);
prOmOpE = std::make_shared<FullMatrixDouble>(3, 4);
pAOmpE = std::make_shared<FullColumn<FMatDsptr>>(4);
endFrames = std::make_shared<std::vector<EndFrmsptr>>();
auto endFrm = CREATE<EndFrameqc>::With();

4
OndselSolver/MarkerFrame.h
View File

@@ -72,9 +72,9 @@ namespace MbD {
PartFrame* partFrame; //Use raw pointer when pointing backwards.
FColDsptr rpmp = std::make_shared<FullColumn<double>>(3);
FMatDsptr aApm = FullMatrix<double>::identitysptr(3);
FMatDsptr aApm = FullMatrixDouble::identitysptr(3);
FColDsptr rOmO = std::make_shared<FullColumn<double>>(3);
FMatDsptr aAOm = FullMatrix<double>::identitysptr(3);
FMatDsptr aAOm = FullMatrixDouble::identitysptr(3);
FMatDsptr prOmOpE;
FColFMatDsptr pAOmpE;
FMatFColDsptr pprOmOpEpE;

22
OndselSolver/MomentOfInertiaSolver.cpp
View File

@@ -7,7 +7,7 @@ using namespace MbD;
void MbD::MomentOfInertiaSolver::example1()
{
auto aJpp = std::make_shared<FullMatrix<double>>(ListListD{
auto aJpp = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
{ 0, 2, 0 },
{ 0, 0, 3 }
@@ -179,10 +179,10 @@ void MbD::MomentOfInertiaSolver::calcJoo()
if (!rPoP) {
rPoP = rPcmP;
aAPo = FullMatrix<double>::identitysptr(3);
aAPo = FullMatrixDouble::identitysptr(3);
}
auto rocmPtilde = FullMatrix<double>::tildeMatrix(rPcmP->minusFullColumn(rPoP));
auto rPoPtilde = FullMatrix<double>::tildeMatrix(rPoP);
auto rocmPtilde = FullMatrixDouble::tildeMatrix(rPcmP->minusFullColumn(rPoP));
auto rPoPtilde = FullMatrixDouble::tildeMatrix(rPoP);
auto term1 = aJPP;
auto term21 = rPoPtilde->timesFullMatrix(rPoPtilde);
auto term22 = rPoPtilde->timesFullMatrix(rocmPtilde);
@@ -197,7 +197,7 @@ void MbD::MomentOfInertiaSolver::calcJpp()
{
//"aJcmP = aJPP + mass*(rPcmPTilde*rPcmPTilde)"
auto rPcmPtilde = FullMatrix<double>::tildeMatrix(rPcmP);
auto rPcmPtilde = FullMatrixDouble::tildeMatrix(rPcmP);
aJcmP = aJPP->plusFullMatrix(rPcmPtilde->timesFullMatrix(rPcmPtilde)->times(m));
aJcmP->symLowerWithUpper();
aJcmP->conditionSelfWithTol(aJcmP->maxMagnitude() * 1.0e-6);
@@ -217,7 +217,7 @@ void MbD::MomentOfInertiaSolver::calcAPp()
auto lam2 = aJpp->at(2);
if (lam0 == lam1) {
if (lam1 == lam2) {
aAPp = FullMatrix<double>::identitysptr(3);
aAPp = FullMatrixDouble::identitysptr(3);
}
else {
eigenvector1 = eigenvectorFor(lam1);
@@ -238,7 +238,7 @@ void MbD::MomentOfInertiaSolver::calcAPp()
if (eigenvector1->at(1) < 0.0) eigenvector1->negateSelf();
eigenvector2 = eigenvector0->cross(eigenvector1);
}
aAPp = std::make_shared<FullMatrix<double>>(3, 3);
aAPp = std::make_shared<FullMatrixDouble>(3, 3);
aAPp->atijputFullColumn(0, 0, eigenvector0);
aAPp->atijputFullColumn(0, 1, eigenvector1);
aAPp->atijputFullColumn(0, 2, eigenvector2);
@@ -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)

6
OndselSolver/OrbitAnglezIeqcJec.cpp
View File

@@ -138,9 +138,9 @@ void MbD::OrbitAngleZIeqcJec::initialize()
OrbitAngleZIecJec::initialize();
pthezpXI = std::make_shared<FullRow<double>>(3);
pthezpEI = std::make_shared<FullRow<double>>(4);
ppthezpXIpXI = std::make_shared<FullMatrix<double>>(3, 3);
ppthezpXIpEI = std::make_shared<FullMatrix<double>>(3, 4);
ppthezpEIpEI = std::make_shared<FullMatrix<double>>(4, 4);
ppthezpXIpXI = std::make_shared<FullMatrixDouble>(3, 3);
ppthezpXIpEI = std::make_shared<FullMatrixDouble>(3, 4);
ppthezpEIpEI = std::make_shared<FullMatrixDouble>(4, 4);
}
FMatDsptr MbD::OrbitAngleZIeqcJec::ppvaluepEIpEI()

14
OndselSolver/OrbitAnglezIeqcJeqc.cpp
View File

@@ -242,13 +242,13 @@ void MbD::OrbitAngleZIeqcJeqc::initialize()
OrbitAngleZIeqcJec::initialize();
pthezpXJ = std::make_shared<FullRow<double>>(3);
pthezpEJ = std::make_shared<FullRow<double>>(4);
ppthezpXIpXJ = std::make_shared<FullMatrix<double>>(3, 3);
ppthezpXIpEJ = std::make_shared<FullMatrix<double>>(3, 4);
ppthezpEIpXJ = std::make_shared<FullMatrix<double>>(4, 3);
ppthezpEIpEJ = std::make_shared<FullMatrix<double>>(4, 4);
ppthezpXJpXJ = std::make_shared<FullMatrix<double>>(3, 3);
ppthezpXJpEJ = std::make_shared<FullMatrix<double>>(3, 4);
ppthezpEJpEJ = std::make_shared<FullMatrix<double>>(4, 4);
ppthezpXIpXJ = std::make_shared<FullMatrixDouble>(3, 3);
ppthezpXIpEJ = std::make_shared<FullMatrixDouble>(3, 4);
ppthezpEIpXJ = std::make_shared<FullMatrixDouble>(4, 3);
ppthezpEIpEJ = std::make_shared<FullMatrixDouble>(4, 4);
ppthezpXJpXJ = std::make_shared<FullMatrixDouble>(3, 3);
ppthezpXJpEJ = std::make_shared<FullMatrixDouble>(3, 4);
ppthezpEJpEJ = std::make_shared<FullMatrixDouble>(4, 4);
}
FMatDsptr MbD::OrbitAngleZIeqcJeqc::ppvaluepEIpEJ()

30
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;
@@ -33,18 +35,18 @@ void Part::initialize()
partFrame = CREATE<PartFrame>::With();
partFrame->setPart(this);
pTpE = std::make_shared<FullColumn<double>>(4);
ppTpEpE = std::make_shared<FullMatrix<double>>(4, 4);
ppTpEpEdot = std::make_shared<FullMatrix<double>>(4, 4);
ppTpEpE = std::make_shared<FullMatrixDouble>(4, 4);
ppTpEpEdot = std::make_shared<FullMatrixDouble>(4, 4);
}
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();

6
OndselSolver/RackPinConstraintIqcJc.cpp
View File

@@ -53,12 +53,14 @@ void MbD::RackPinConstraintIqcJc::calc_pGpXI()
void MbD::RackPinConstraintIqcJc::calc_ppGpEIpEI()
{
ppGpEIpEI = xIeJeIe->ppvaluepEIpEI()->plusFullMatrix(thezIeJe->ppvaluepEIpEI()->times(pitchRadius));
ppGpEIpEI = xIeJeIe->ppvaluepEIpEI()
->plusFullMatrix(thezIeJe->ppvaluepEIpEI()->times(pitchRadius));
}
void MbD::RackPinConstraintIqcJc::calc_ppGpXIpEI()
{
ppGpXIpEI = xIeJeIe->ppvaluepXIpEI()->plusFullMatrix(thezIeJe->ppvaluepXIpEI()->times(pitchRadius));
ppGpXIpEI = xIeJeIe->ppvaluepXIpEI()
->plusFullMatrix(thezIeJe->ppvaluepXIpEI()->times(pitchRadius));
}
void MbD::RackPinConstraintIqcJc::calcPostDynCorrectorIteration()

9
OndselSolver/RackPinConstraintIqcJqc.cpp
View File

@@ -30,17 +30,20 @@ void MbD::RackPinConstraintIqcJqc::calc_pGpXJ()
void MbD::RackPinConstraintIqcJqc::calc_ppGpEIpEJ()
{
ppGpEIpEJ = xIeJeIe->ppvaluepEIpEJ()->plusFullMatrix(thezIeJe->ppvaluepEIpEJ()->times(pitchRadius));
ppGpEIpEJ = xIeJeIe->ppvaluepEIpEJ()
->plusFullMatrix(thezIeJe->ppvaluepEIpEJ()->times(pitchRadius));
}
void MbD::RackPinConstraintIqcJqc::calc_ppGpEIpXJ()
{
ppGpEIpXJ = xIeJeIe->ppvaluepEIpXJ()->plusFullMatrix(thezIeJe->ppvaluepEIpXJ()->times(pitchRadius));
ppGpEIpXJ = xIeJeIe->ppvaluepEIpXJ()
->plusFullMatrix(thezIeJe->ppvaluepEIpXJ()->times(pitchRadius));
}
void MbD::RackPinConstraintIqcJqc::calc_ppGpEJpEJ()
{
ppGpEJpEJ = xIeJeIe->ppvaluepEJpEJ()->plusFullMatrix(thezIeJe->ppvaluepEJpEJ()->times(pitchRadius));
ppGpEJpEJ = xIeJeIe->ppvaluepEJpEJ()
->plusFullMatrix(thezIeJe->ppvaluepEJpEJ()->times(pitchRadius));
}
void MbD::RackPinConstraintIqcJqc::calcPostDynCorrectorIteration()

2
OndselSolver/RedundantConstraint.cpp
View File

@@ -105,7 +105,7 @@ void RedundantConstraint::setqsuddotlam(FColDsptr col)
void RedundantConstraint::discontinuityAtaddTypeTo(double t, std::shared_ptr<std::vector<DiscontinuityType>> disconTypes)
{
//"Reactivate all contraints."
//"Reactivate all constraints."
assert(false);
//| newSelf |
//newSelf : = self constraint.

10
OndselSolver/RowTypeMatrix.cpp
View File

@@ -9,13 +9,3 @@
#include "RowTypeMatrix.h"
using namespace MbD;
template<typename T>
int RowTypeMatrix<T>::nrow() {
return (int) this->size();
}
template<typename T>
int RowTypeMatrix<T>::ncol() {
return this->at(0)->numberOfElements();
}

8
OndselSolver/RowTypeMatrix.h
View File

@@ -24,8 +24,12 @@ namespace MbD {
virtual void zeroSelf() override = 0;
//double maxMagnitude() override;
int numberOfElements() override;
int nrow();
int ncol();
int nrow() {
return (int) this->size();
}
int ncol() {
return this->at(0)->numberOfElements();
}
};
template<typename T>

6
OndselSolver/ScrewConstraintIqcJc.cpp
View File

@@ -55,12 +55,14 @@ void MbD::ScrewConstraintIqcJc::calc_pGpXI()
void MbD::ScrewConstraintIqcJc::calc_ppGpEIpEI()
{
ppGpEIpEI = zIeJeIe->ppvaluepEIpEI()->times(2.0 * M_PI)->minusFullMatrix(thezIeJe->ppvaluepEIpEI()->times(pitch));
ppGpEIpEI = zIeJeIe->ppvaluepEIpEI()->times(2.0 * M_PI)
->minusFullMatrix(thezIeJe->ppvaluepEIpEI()->times(pitch));
}
void MbD::ScrewConstraintIqcJc::calc_ppGpXIpEI()
{
ppGpXIpEI = zIeJeIe->ppvaluepXIpEI()->times(2.0 * M_PI)->minusFullMatrix(thezIeJe->ppvaluepXIpEI()->times(pitch));
ppGpXIpEI = zIeJeIe->ppvaluepXIpEI()->times(2.0 * M_PI)
->minusFullMatrix(thezIeJe->ppvaluepXIpEI()->times(pitch));
}
void MbD::ScrewConstraintIqcJc::calcPostDynCorrectorIteration()

9
OndselSolver/ScrewConstraintIqcJqc.cpp
View File

@@ -32,17 +32,20 @@ void MbD::ScrewConstraintIqcJqc::calc_pGpXJ()
void MbD::ScrewConstraintIqcJqc::calc_ppGpEIpEJ()
{
ppGpEIpEJ = zIeJeIe->ppvaluepEIpEJ()->times(2.0 * M_PI)->minusFullMatrix(thezIeJe->ppvaluepEIpEJ()->times(pitch));
ppGpEIpEJ = zIeJeIe->ppvaluepEIpEJ()->times(2.0 * M_PI)
->minusFullMatrix(thezIeJe->ppvaluepEIpEJ()->times(pitch));
}
void MbD::ScrewConstraintIqcJqc::calc_ppGpEIpXJ()
{
ppGpEIpXJ = zIeJeIe->ppvaluepEIpXJ()->times(2.0 * M_PI)->minusFullMatrix(thezIeJe->ppvaluepEIpXJ()->times(pitch));
ppGpEIpXJ = zIeJeIe->ppvaluepEIpXJ()->times(2.0 * M_PI)
->minusFullMatrix(thezIeJe->ppvaluepEIpXJ()->times(pitch));
}
void MbD::ScrewConstraintIqcJqc::calc_ppGpEJpEJ()
{
ppGpEJpEJ = zIeJeIe->ppvaluepEJpEJ()->times(2.0 * M_PI)->minusFullMatrix(thezIeJe->ppvaluepEJpEJ()->times(pitch));
ppGpEJpEJ = zIeJeIe->ppvaluepEJpEJ()->times(2.0 * M_PI)
->minusFullMatrix(thezIeJe->ppvaluepEJpEJ()->times(pitch));
}
void MbD::ScrewConstraintIqcJqc::calcPostDynCorrectorIteration()

17
OndselSolver/SparseMatrix.h
View File

@@ -13,6 +13,7 @@
#include "RowTypeMatrix.h"
#include "SparseRow.h"
#include "DiagonalMatrix.h"
#include "FullMatrix.h"
namespace MbD {
template<typename T>
@@ -47,10 +48,10 @@ namespace MbD {
void zeroSelf() override;
void atijplusFullRow(int i, int j, FRowsptr<T> fullRow);
void atijplusFullColumn(int i, int j, FColsptr<T> fullCol);
void atijplusFullMatrix(int i, int j, FMatsptr<T> fullMat);
void atijminusFullMatrix(int i, int j, FMatsptr<T> fullMat);
void atijplusTransposeFullMatrix(int i, int j, FMatsptr<T> fullMat);
void atijplusFullMatrixtimes(int i, int j, FMatsptr<T> fullMat, T factor);
void atijplusFullMatrix(int i, int j, FMatDsptr fullMat);
void atijminusFullMatrix(int i, int j, FMatDsptr fullMat);
void atijplusTransposeFullMatrix(int i, int j, FMatDsptr fullMat);
void atijplusFullMatrixtimes(int i, int j, FMatDsptr fullMat, T factor);
void atijplusNumber(int i, int j, double value);
void atijminusNumber(int i, int j, double value);
void atijput(int i, int j, T value);
@@ -117,7 +118,7 @@ namespace MbD {
}
}
template<typename T>
inline void SparseMatrix<T>::atijplusFullMatrix(int i, int j, FMatsptr<T> fullMat)
inline void SparseMatrix<T>::atijplusFullMatrix(int i, int j, FMatDsptr fullMat)
{
for (int ii = 0; ii < fullMat->nrow(); ii++)
{
@@ -125,7 +126,7 @@ namespace MbD {
}
}
template<typename T>
inline void SparseMatrix<T>::atijminusFullMatrix(int i, int j, FMatsptr<T> fullMat)
inline void SparseMatrix<T>::atijminusFullMatrix(int i, int j, FMatDsptr fullMat)
{
for (int ii = 0; ii < fullMat->nrow(); ii++)
{
@@ -133,7 +134,7 @@ namespace MbD {
}
}
template<typename T>
inline void SparseMatrix<T>::atijplusTransposeFullMatrix(int i, int j, FMatsptr<T> fullMat)
inline void SparseMatrix<T>::atijplusTransposeFullMatrix(int i, int j, FMatDsptr fullMat)
{
for (int ii = 0; ii < fullMat->nrow(); ii++)
{
@@ -141,7 +142,7 @@ namespace MbD {
}
}
template<typename T>
inline void SparseMatrix<T>::atijplusFullMatrixtimes(int i, int j, FMatsptr<T> fullMat, T factor)
inline void SparseMatrix<T>::atijplusFullMatrixtimes(int i, int j, FMatDsptr fullMat, T factor)
{
for (int ii = 0; ii < fullMat->nrow(); ii++)
{

4
OndselSolver/StableBackwardDifference.cpp
View File

@@ -12,7 +12,7 @@ using namespace MbD;
void StableBackwardDifference::formTaylorMatrix()
{
//This form is numerically more stable and is prefered over the full Taylor Matrix.
//This form is numerically more stable and is preferred over the full Taylor Matrix.
//For method order 3:
//| (t1 - t) (t1 - t) ^ 2 / 2! (t1 - t) ^ 3 / 3!| |qd(t) | = | q(t1) - q(t) |
//| (t2 - t) (t2 - t) ^ 2 / 2! (t2 - t) ^ 3 / 3!| |qdd(t) | |q(t2) - q(t) |
@@ -28,7 +28,7 @@ void StableBackwardDifference::formTaylorMatrix()
void StableBackwardDifference::instantiateTaylorMatrix()
{
if (taylorMatrix == nullptr || (taylorMatrix->nrow() != (order))) {
taylorMatrix = std::make_shared<FullMatrix<double>>(order, order);
taylorMatrix = std::make_shared<FullMatrixDouble>(order, order);
}
}

6
OndselSolver/TranslationConstraintIqcJc.cpp
View File

@@ -30,8 +30,10 @@ void TranslationConstraintIqcJc::calcPostDynCorrectorIteration()
pGpXI = riIeqJeIeq->pvaluepXI();
pGpEI = (riIeqJeIeq->pvaluepEI())->plusFullRow(riIeqJeIeq->pvaluepEK());
ppGpXIpEI = riIeqJeIeq->ppvaluepXIpEK();
ppGpEIpEI = riIeqJeIeq->ppvaluepEIpEI()->plusFullMatrix(riIeqJeIeq->ppvaluepEIpEK())
->plusFullMatrix((riIeqJeIeq->ppvaluepEIpEK()->transpose()->plusFullMatrix(riIeqJeIeq->ppvaluepEKpEK())));
ppGpEIpEI = riIeqJeIeq->ppvaluepEIpEI()
->plusFullMatrix(riIeqJeIeq->ppvaluepEIpEK())
->plusFullMatrix((riIeqJeIeq->ppvaluepEIpEK()->
transpose()->plusFullMatrix(riIeqJeIeq->ppvaluepEKpEK())));
}
void TranslationConstraintIqcJc::useEquationNumbers()

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); });

0
OndselSolver/00backhoe.asmt → testapp/00backhoe.asmt
View File

10
testapp/CMakeLists.txt Normal file
View File

@@ -0,0 +1,10 @@
cmake_minimum_required(VERSION 3.16)
project(OndselSolverLibrary VERSION 1.0.1 DESCRIPTION "Assembly Constraints and Multibody Dynamics code")
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED True)
add_executable(testapp OndselSolver.cpp)
target_link_libraries(testapp OndselSolver)
target_include_directories(testapp PUBLIC "../OndselSolver")

42
OndselSolver/OndselSolver.cpp → testapp/OndselSolver.cpp
View File

@@ -13,33 +13,33 @@
*********************************************************************/
#include <filesystem>
#include "CADSystem.h"
#include "CREATE.h"
#include "GESpMatParPvPrecise.h"
#include "ASMTAssembly.h"
#include "MBDynSystem.h"
#include "MomentOfInertiaSolver.h"
#include "../OndselSolver/CADSystem.h"
#include "../OndselSolver/CREATE.h"
#include "../OndselSolver/GESpMatParPvPrecise.h"
#include "../OndselSolver/ASMTAssembly.h"
#include "../OndselSolver/MBDynSystem.h"
#include "../OndselSolver/MomentOfInertiaSolver.h"
using namespace MbD;
void runSpMat();
int main()
{
ASMTAssembly::readWriteFile("piston.asmt");
MBDynSystem::runFile("MBDynCase.mbd"); //To be completed
MBDynSystem::runFile("crank_slider.mbd"); //To be completed
//ASMTAssembly::runSinglePendulumSuperSimplified(); //Mass is missing
//ASMTAssembly::runSinglePendulumSuperSimplified2(); //DOF has infinite acceleration due to zero mass and inertias
ASMTAssembly::runSinglePendulumSimplified();
ASMTAssembly::runSinglePendulum();
ASMTAssembly::runFile("piston.asmt");
ASMTAssembly::runFile("00backhoe.asmt");
//ASMTAssembly::runFile("circular.asmt"); //Needs checking
//ASMTAssembly::runFile("cirpendu.asmt"); //Under constrained. Testing ICKine.
//ASMTAssembly::runFile("engine1.asmt"); //Needs checking
ASMTAssembly::runFile("fourbar.asmt");
//ASMTAssembly::runFile("fourbot.asmt"); //Very large but works
ASMTAssembly::runFile("wobpump.asmt");
// // ASMTAssembly::readWriteFile("piston.asmt");
// MBDynSystem::runFile("MBDynCase.mbd"); //To be completed
// MBDynSystem::runFile("crank_slider.mbd"); //To be completed
// //ASMTAssembly::runSinglePendulumSuperSimplified(); //Mass is missing
// //ASMTAssembly::runSinglePendulumSuperSimplified2(); //DOF has infinite acceleration due to zero mass and inertias
// ASMTAssembly::runSinglePendulumSimplified();
// ASMTAssembly::runSinglePendulum();
// ASMTAssembly::runFile("piston.asmt");
// ASMTAssembly::runFile("00backhoe.asmt");
// //ASMTAssembly::runFile("circular.asmt"); //Needs checking
// //ASMTAssembly::runFile("cirpendu.asmt"); //Under constrained. Testing ICKine.
// //ASMTAssembly::runFile("engine1.asmt"); //Needs checking
// ASMTAssembly::runFile("fourbar.asmt");
// //ASMTAssembly::runFile("fourbot.asmt"); //Very large but works
// ASMTAssembly::runFile("wobpump.asmt");
auto cadSystem = std::make_shared<CADSystem>();
cadSystem->runOndselSinglePendulum();

0
OndselSolver/OndselSolver.h → testapp/OndselSolver.h
View File

0
OndselSolver/assembly.asmt → testapp/assembly.asmt
View File

2
OndselSolver/circular.asmt → testapp/circular.asmt
View File

File diff suppressed because one or more lines are too long

0
OndselSolver/cirpendu.asmt → testapp/cirpendu.asmt
View File

0
OndselSolver/engine1.asmt → testapp/engine1.asmt
View File

0
OndselSolver/fourbar.asmt → testapp/fourbar.asmt
View File

0
OndselSolver/fourbot.asmt → testapp/fourbot.asmt
View File

2
OndselSolver/gyro.asmt → testapp/gyro.asmt
View File

File diff suppressed because one or more lines are too long

0
OndselSolver/piston.asmt → testapp/piston.asmt
View File

0
OndselSolver/robot.asmt → testapp/robot.asmt
View File

0
OndselSolver/wobpump.asmt → testapp/wobpump.asmt
View File