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

Second commit

Browse Source
This commit is contained in:
Aik-Siong Koh
2023-04-28 16:29:43 -06:00
parent dd01694484
commit 8ca8eb7122
18 changed files with 329 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
MbDCode/Array.h
View File

@@ -9,6 +9,14 @@ namespace MbD {
Array(){}
Array(int i) : std::vector<T>(i) {}
Array(std::initializer_list<T> list) : std::vector<T>{ list } {}
};
void copy(Array<T>* x);
};
template<typename T>
inline void Array<T>::copy(Array<T>* x)
{
for (int i = 0; i < x->size(); i++) {
this->at(i) = x->at(i);
}
}
}

1
MbDCode/CartesianFrame.h
View File

@@ -1,5 +1,6 @@
#pragma once
#include "Item.h"
namespace MbD {
class CartesianFrame :
public Item

6
MbDCode/EulerConstraint.h
View File

@@ -6,17 +6,15 @@
#include "FullRow.h"
namespace MbD {
using FullRowDPtr = std::shared_ptr<FullRow<double>>;
class EulerConstraint : public Constraint
{
public:
EulerConstraint() : Constraint() {
pGpE = std::make_shared<FullRow<double>>(4);
}
//pGpE iqE
FullRowDPtr pGpE; //partial derivative of G wrt pE
int iqE;
FullRow<double> pGpE = FullRow<double>(4); //partial derivative of G wrt pE
int iqE = -1;
};
}

15
MbDCode/FullColumn.cpp
View File

@@ -1 +1,16 @@
#include <sstream>
#include "FullColumn.h"
using namespace MbD;
std::string FullColumn<double>::toString() {
std::stringstream ss;
ss << "FullColumn { ";
for (int i = 0; i < this->size() - 1; i++) {
ss << this->at(i) << ", ";
}
ss << this->back() << " }";
return ss.str();
}

3
MbDCode/FullColumn.h
View File

@@ -1,11 +1,14 @@
#pragma once
#include "Vector.h"
namespace MbD {
template <typename T>
class FullColumn : public Vector<T>
{
public:
FullColumn(int i) : Vector<T>(i) {}
FullColumn(std::initializer_list<T> list) : Vector<T>{ list } {}
std::string toString();
};
}

31
MbDCode/MarkerFrame.cpp
View File

@@ -1 +1,32 @@
#include "MarkerFrame.h"
using namespace MbD;
MarkerFrame::MarkerFrame()
{
partFrame = nullptr;
auto endFrm = std::make_shared<EndFrameqc>();
std::string str = "EndFrame1";
endFrm->setName(str);
this->addEndFrame(endFrm);
}
void MarkerFrame::setPartFrame(PartFrame* partFrm)
{
partFrame = partFrm;
}
void MarkerFrame::setrpmp(FullColumn<double>* x)
{
rpmp->copy(x);
}
void MarkerFrame::setaApm(FullMatrix<double>* x)
{
aApm->copy(x);
}
void MarkerFrame::addEndFrame(std::shared_ptr<EndFrameqc> endFrm)
{
endFrm->setMarkerFrame(this);
endFrames.push_back(endFrm);
}

30
MbDCode/MarkerFrame.h
View File

@@ -1,9 +1,35 @@
#pragma once
#include <memory>
#include "CartesianFrame.h"
#include "PartFrame.h"
#include "FullColumn.h"
#include "FullMatrix.h"
#include "EndFrameqc.h"
namespace MbD {
class MarkerFrame :
public CartesianFrame
class PartFrame;
class EndFrameqc;
class MarkerFrame : public CartesianFrame
{
//partFrame rpmp aApm rOmO aAOm prOmOpE pAOmpE pprOmOpEpE ppAOmpEpE endFrames
public:
MarkerFrame();
void setPartFrame(PartFrame* partFrm);
void setrpmp(FullColumn<double>* x);
void setaApm(FullMatrix<double>* x);
void addEndFrame(std::shared_ptr<EndFrameqc> x);
PartFrame* partFrame;
FullColumn<double>* rpmp = new FullColumn<double>(3);
FullMatrix<double>* aApm = new FullMatrix<double>(3, 3);
FullColumn<double>* rOmO = new FullColumn<double>(3);
FullMatrix<double>* aAOm = new FullMatrix<double>(3, 3);
FullMatrix<double>* prOmOpE = new FullMatrix<double>(3, 4);
FullColumn<FullMatrix<double>>* pAOmpE = new FullColumn<FullMatrix<double>>(4);
std::vector<std::shared_ptr<EndFrameqc>> endFrames;
};
}

90
MbDCode/MbDCode.cpp
View File

@@ -1,17 +1,95 @@
#include <iostream>
#include "Item.h"
#include "System.h"
#include "FullColumn.h"
#include "FullMatrix.h"
#include "Part.h"
#include "MbDCode.h"
using namespace MbD;
int main()
{
std::cout << "Hello World!\n";
auto& TheSystem = System::getInstance();
System& TheSystem = System::getInstance();
std::string str = "TheSystem";
TheSystem.setName(str);
std::cout << TheSystem.getName();
//auto fixedPart = std::make_shared<Part>();
//str = "FixedPart";
//fixedPart->setName(str);
std::cout << "TheSystem.getName() " << TheSystem.getName() << std::endl;
FullColumn<double>* qX, * qE;
FullColumn<double>* rpmp;
FullMatrix<double>* aApm;
FullRow<double>* fullRow;
auto row = new FullRow<double>{ 0, 0, 0, 1 };
fullRow = new FullRow<double>(4);
fullRow->copy(row);
//
auto assembly1 = std::make_shared<Part>();
str = "Assembly1";
assembly1->setName(str);
std::cout << "assembly1->getName() " << assembly1->getName() << std::endl;
qX = new FullColumn<double>{ 0, 0, 0 };
qE = new FullColumn<double>{ 0, 0, 0, 1 };
assembly1->setqX(qX);
assembly1->setqE(qE);
std::cout << "assembly1->getqX() " << assembly1->getqX()->toString() << std::endl;
std::cout << "assembly1->getqE() " << assembly1->getqE()->toString() << std::endl;
TheSystem.addPart(assembly1);
{
auto marker1 = std::make_shared<MarkerFrame>();
str = "Marker1";
marker1->setName(str);
rpmp = new FullColumn<double>{ 0.38423366582893, 6.8384291794733e-9, -0.048029210642807 };
marker1->setrpmp(rpmp);
aApm = new FullMatrix<double>();
fullRow = new FullRow<double>{ 0.38423366582893, 6.8384291794733e-9, -0.048029210642807 };
aApm->push_back(fullRow);
fullRow = new FullRow<double>{ 0.38423366582893, 6.8384291794733e-9, -0.048029210642807 };
aApm->push_back(fullRow);
fullRow = new FullRow<double>{ 0.38423366582893, 6.8384291794733e-9, -0.048029210642807 };
aApm->push_back(fullRow);
marker1->setaApm(aApm);
assembly1->partFrame->addMarkerFrame(marker1);
//
auto marker2 = std::make_shared<MarkerFrame>();
str = "Marker2";
marker2->setName(str);
rpmp = new FullColumn<double>{ 0.0, 0.0, 0.0 };
marker2->setrpmp(rpmp);
aApm = new FullMatrix<double>();
fullRow = new FullRow<double>{ 1.0, 0.0, 0.0 };
aApm->push_back(fullRow);
fullRow = new FullRow<double>{ 0.0, 1.0, 0.0 };
aApm->push_back(fullRow);
fullRow = new FullRow<double>{ 0.0, 0.0, 1.0 };
aApm->push_back(fullRow);
marker2->setaApm(aApm);
assembly1->partFrame->addMarkerFrame(marker2);
}
//
auto part1 = std::make_shared<Part>();
str = "Part1";
part1->setName(str);
qX = new FullColumn<double>{ 0.38423366582893, 6.8384291794733e-9, -0.048029210642807 };
qE = new FullColumn<double>{ 0.0, 0.0, 1.4248456266393e-10, 1.0 };
part1->setqX(qX);
part1->setqE(qE);
TheSystem.parts.push_back(part1);
//
auto part2 = std::make_shared<Part>();
str = "Part2";
part2->setName(str);
qX = new FullColumn<double>{ 0.38423366582893, 0.49215308269277, 0.048029210642807 };
qE = new FullColumn<double>{ 0.0, 0.0, 0.89871701272344, 0.4385290538168 };
part2->setqX(qX);
part2->setqE(qE);
TheSystem.parts.push_back(part2);
//
auto part3 = std::make_shared<Part>();
str = "Part3";
part3->setName(str);
qX = new FullColumn<double>{ -1.284772285311e-18, 1.4645982581368, -4.788228906425e-17 };
qE = new FullColumn<double>{ 0.70710678118655, 0.70710678118655, 0.0, 0.0 };
part3->setqX(qX);
part3->setqE(qE);
TheSystem.parts.push_back(part3);
//
}

15
MbDCode/MbDCode.vcxproj
View File

@@ -131,8 +131,13 @@
<ClCompile Include="Array.cpp" />
<ClCompile Include="CartesianFrame.cpp" />
<ClCompile Include="Constraint.cpp" />
<ClCompile Include="DiagonalMatrix.cpp" />
<ClCompile Include="EndFramec.cpp" />
<ClCompile Include="EndFrameqc.cpp" />
<ClCompile Include="EndFrameqct.cpp" />
<ClCompile Include="EulerConstraint.cpp" />
<ClCompile Include="FullColumn.cpp" />
<ClCompile Include="FullMatrix.cpp" />
<ClCompile Include="FullRow.cpp" />
<ClCompile Include="Item.cpp" />
<ClCompile Include="Joint.cpp" />
@@ -141,27 +146,37 @@
<ClCompile Include="NewtonRaphson.cpp" />
<ClCompile Include="Part.cpp" />
<ClCompile Include="PartFrame.cpp" />
<ClCompile Include="RowTypeMatrix.cpp" />
<ClCompile Include="Solver.cpp" />
<ClCompile Include="System.cpp" />
<ClCompile Include="SystemSolver.cpp" />
<ClCompile Include="Vector.cpp" />
</ItemGroup>
<ItemGroup>
<ClInclude Include="AbsConstraint.h" />
<ClInclude Include="Array.h" />
<ClInclude Include="CartesianFrame.h" />
<ClInclude Include="Constraint.h" />
<ClInclude Include="DiagonalMatrix.h" />
<ClInclude Include="EndFramec.h" />
<ClInclude Include="EndFrameqc.h" />
<ClInclude Include="EndFrameqct.h" />
<ClInclude Include="EulerConstraint.h" />
<ClInclude Include="FullColumn.h" />
<ClInclude Include="FullMatrix.h" />
<ClInclude Include="FullRow.h" />
<ClInclude Include="Item.h" />
<ClInclude Include="Joint.h" />
<ClInclude Include="MarkerFrame.h" />
<ClInclude Include="MbDCode.h" />
<ClInclude Include="NewtonRaphson.h" />
<ClInclude Include="Part.h" />
<ClInclude Include="PartFrame.h" />
<ClInclude Include="RowTypeMatrix.h" />
<ClInclude Include="Solver.h" />
<ClInclude Include="System.h" />
<ClInclude Include="SystemSolver.h" />
<ClInclude Include="Vector.h" />
</ItemGroup>
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
<ImportGroup Label="ExtensionTargets">

39
MbDCode/MbDCode.vcxproj.filters
View File

@@ -66,6 +66,24 @@
<ClCompile Include="MarkerFrame.cpp">
<Filter>Source Files</Filter>
</ClCompile>
<ClCompile Include="DiagonalMatrix.cpp">
<Filter>Source Files</Filter>
</ClCompile>
<ClCompile Include="FullMatrix.cpp">
<Filter>Source Files</Filter>
</ClCompile>
<ClCompile Include="RowTypeMatrix.cpp">
<Filter>Source Files</Filter>
</ClCompile>
<ClCompile Include="EndFramec.cpp">
<Filter>Source Files</Filter>
</ClCompile>
<ClCompile Include="EndFrameqc.cpp">
<Filter>Source Files</Filter>
</ClCompile>
<ClCompile Include="EndFrameqct.cpp">
<Filter>Source Files</Filter>
</ClCompile>
</ItemGroup>
<ItemGroup>
<ClInclude Include="Array.h">
@@ -116,5 +134,26 @@
<ClInclude Include="MarkerFrame.h">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="DiagonalMatrix.h">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="FullMatrix.h">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="RowTypeMatrix.h">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="EndFramec.h">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="EndFrameqc.h">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="EndFrameqct.h">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="MbDCode.h">
<Filter>Header Files</Filter>
</ClInclude>
</ItemGroup>
</Project>

29
MbDCode/Part.cpp
View File

@@ -1 +1,30 @@
#include "Part.h"
#include "PartFrame.h"
#include "FullColumn.h"
using namespace MbD;
Part::Part() {
partFrame = std::make_shared<PartFrame>();
}
void Part::setqX(FullColumn<double>* x) {
partFrame.get()->setqX(x);
}
FullColumn<double>* Part::getqX() {
return partFrame.get()->getqX();
}
void Part::setqE(FullColumn<double>* x) {
partFrame.get()->setqE(x);
}
FullColumn<double>* Part::getqE() {
return partFrame.get()->getqE();
}
void Part::setSystem(System& sys)
{
//May be needed in the future
}

20
MbDCode/Part.h
View File

@@ -2,26 +2,24 @@
#include <memory>
#include "Item.h"
#include "System.h"
#include "PartFrame.h"
#include "FullColumn.h"
namespace MbD {
class System;
class PartFrame;
class Part : public Item
{
//ToDo: ipX ipE m aJ partFrame pX pXdot pE pEdot mX mE mEdot pTpE ppTpEpE ppTpEpEdot
public:
Part() {
partFrame = std::make_shared<PartFrame>();
}
void setqX(FullColumn<double>* x) {
partFrame.get()->setqX(x);
}
FullColumn<double>* getqX() {
return partFrame.get()->getqX();
}
//ToDo: Needed members ipX ipE m aJ partFrame pX pXdot pE pEdot mX mE mEdot pTpE ppTpEpE ppTpEpEdot
Part();
void setqX(FullColumn<double>* x);
FullColumn<double>* getqX();
void setqE(FullColumn<double>* x);
FullColumn<double>* getqE();
void setSystem(System& sys);
std::shared_ptr<PartFrame> partFrame;
};
}

39
MbDCode/PartFrame.cpp
View File

@@ -1,12 +1,37 @@
#include "Part.h"
#include "PartFrame.h"
#include "AbsConstraint.h"
#include "MarkerFrame.h"
namespace MbD {
using namespace MbD;
PartFrame::PartFrame()
{
aGabs = std::vector<std::shared_ptr<AbsConstraint>>();
markerFrames = std::vector<std::shared_ptr<MarkerFrame>>();
}
}
PartFrame::PartFrame()
{
aGeu = std::make_shared<EulerConstraint>();
aGabs = std::vector<std::shared_ptr<AbsConstraint>>();
markerFrames = std::vector<std::shared_ptr<MarkerFrame>>();
}
void PartFrame::setqX(FullColumn<double>* x) {
qX->copy(x);
}
FullColumn<double>* PartFrame::getqX() {
return qX;
}
void PartFrame::setqE(FullColumn<double>* x) {
qE->copy(x);
}
FullColumn<double>* PartFrame::getqE() {
return qE;
}
void PartFrame::setPart(std::shared_ptr<Part> x) {
part = x;
}
std::shared_ptr<Part> PartFrame::getPart() {
return part.lock();
}
void MbD::PartFrame::addMarkerFrame(std::shared_ptr<MarkerFrame> markerFrame)
{
markerFrame->setPartFrame(this);
markerFrames.push_back(markerFrame);
}

27
MbDCode/PartFrame.h
View File

@@ -15,25 +15,22 @@ namespace MbD {
class PartFrame : public CartesianFrame
{
//ToDo: part iqX iqE qX qE qXdot qEdot qXddot qEddot aGeu aGabs markerFrames
public:
PartFrame();
void setqX(FullColumn<double>* x) {
qX = x;
}
FullColumn<double>* getqX() {
return qX;
}
void setPart(std::shared_ptr<Part> x) {
part = x;
}
std::shared_ptr<Part> getPart() {
return part.lock();
}
//part iqX iqE qX qE qXdot qEdot qXddot qEddot aGeu aGabs markerFrames
void setqX(FullColumn<double>* x);
FullColumn<double>* getqX();
void setqE(FullColumn<double>* x);
FullColumn<double>* getqE();
void setPart(std::shared_ptr<Part> x);
std::shared_ptr<Part> getPart();
void addMarkerFrame(std::shared_ptr<MarkerFrame> x);
std::weak_ptr<Part> part;
int iqX, iqE; //Position index of frame variables qX and qE in system list of variables
FullColumn<double>* qX;
FullColumn<double>* qE;
FullColumn<double>* qX = new FullColumn<double>(3);
FullColumn<double>* qE = new FullColumn<double>(4);
std::shared_ptr<EulerConstraint> aGeu;
std::vector<std::shared_ptr<AbsConstraint>> aGabs;
std::vector<std::shared_ptr<MarkerFrame>> markerFrames;

12
MbDCode/System.cpp
View File

@@ -1 +1,13 @@
#include "System.h"
void MbD::System::addPart(std::shared_ptr<Part> part)
{
part->setSystem(*this);
parts.push_back(part);
}
MbD::System::System() {
parts = std::vector<std::shared_ptr<Part>>();
jointsMotions = std::vector<std::shared_ptr<Joint>>();
systemSolver = std::make_shared<SystemSolver>(*this);
}

8
MbDCode/System.h
View File

@@ -8,6 +8,7 @@
#include "SystemSolver.h"
namespace MbD {
class Part;
class SystemSolver;
class System : public Item
{
@@ -22,12 +23,9 @@ namespace MbD {
std::vector<std::shared_ptr<Joint>> jointsMotions;
bool hasChanged = false;
std::shared_ptr<SystemSolver> systemSolver;
void addPart(std::shared_ptr<Part> part);
private:
System() {
parts = std::vector<std::shared_ptr<Part>>();
jointsMotions = std::vector<std::shared_ptr<Joint>>();
systemSolver = std::make_shared<SystemSolver>(*this);
}
System();
//System() = default; // Private constructor
System(const System&) = delete;
System& operator=(const System&) = delete; // Deleted copy assignment

4
MbDCode/Vector.cpp
View File

@@ -1 +1,5 @@
#include "Vector.h"
using namespace MbD;

1
MbDCode/Vector.h
View File

@@ -1,5 +1,6 @@
#pragma once
#include "Array.h"
namespace MbD {
template <typename T>
class Vector : public Array<T>