Merge pull request #37 from Ondsel-Development/TemplateIssue

Template issue

CMakeLists.txt

@@ -46,36 +46,50 @@ set(ONDSELSOLVER_SRC
         OndselSolver/ArcSine.cpp
         OndselSolver/ArcTan.cpp
         OndselSolver/ArcTan2.cpp
+        OndselSolver/ASMTAngleJoint.cpp
         OndselSolver/ASMTAnimationParameters.cpp
         OndselSolver/ASMTAssembly.cpp
+        OndselSolver/ASMTCompoundJoint.cpp
         OndselSolver/ASMTConstantGravity.cpp
+        OndselSolver/ASMTConstantVelocityJoint.cpp
         OndselSolver/ASMTConstraintSet.cpp
         OndselSolver/ASMTCylindricalJoint.cpp
+        OndselSolver/ASMTCylSphJoint.cpp
         OndselSolver/ASMTExtrusion.cpp
         OndselSolver/ASMTFixedJoint.cpp
         OndselSolver/ASMTForceTorque.cpp
+        OndselSolver/ASMTGearJoint.cpp
         OndselSolver/ASMTGeneralMotion.cpp
+        OndselSolver/ASMTInPlaneJoint.cpp
         OndselSolver/ASMTItem.cpp
         OndselSolver/ASMTItemIJ.cpp
         OndselSolver/ASMTJoint.cpp
         OndselSolver/ASMTKinematicIJ.cpp
+        OndselSolver/ASMTLineInPlaneJoint.cpp
         OndselSolver/ASMTMarker.cpp
         OndselSolver/ASMTMotion.cpp
         OndselSolver/ASMTNoRotationJoint.cpp
+        OndselSolver/ASMTParallelAxesJoint.cpp
         OndselSolver/ASMTPart.cpp
+        OndselSolver/ASMTPerpendicularJoint.cpp
+        OndselSolver/ASMTPlanarJoint.cpp
         OndselSolver/ASMTPointInLineJoint.cpp
         OndselSolver/ASMTPointInPlaneJoint.cpp
         OndselSolver/ASMTPrincipalMassMarker.cpp
+        OndselSolver/ASMTRackPinionJoint.cpp
         OndselSolver/ASMTRefCurve.cpp
         OndselSolver/ASMTRefItem.cpp
         OndselSolver/ASMTRefPoint.cpp
         OndselSolver/ASMTRefSurface.cpp
+        OndselSolver/ASMTRevCylJoint.cpp
         OndselSolver/ASMTRevoluteJoint.cpp
         OndselSolver/ASMTRotationalMotion.cpp
+        OndselSolver/ASMTScrewJoint.cpp
         OndselSolver/ASMTSimulationParameters.cpp
         OndselSolver/ASMTSpatialContainer.cpp
         OndselSolver/ASMTSpatialItem.cpp
         OndselSolver/ASMTSphericalJoint.cpp
+        OndselSolver/ASMTSphSphJoint.cpp
         OndselSolver/ASMTTime.cpp
         OndselSolver/ASMTTranslationalJoint.cpp
         OndselSolver/ASMTTranslationalMotion.cpp
@@ -223,6 +237,7 @@ set(ONDSELSOLVER_SRC
         OndselSolver/MBDynData.cpp
         OndselSolver/MBDynDrive.cpp
         OndselSolver/MBDynElement.cpp
+        OndselSolver/MBDynGravity.cpp
         OndselSolver/MBDynInitialValue.cpp
         OndselSolver/MBDynItem.cpp
         OndselSolver/MBDynJoint.cpp
@@ -338,36 +353,50 @@ set(ONDSELSOLVER_HEADERS
         OndselSolver/ArcSine.h
         OndselSolver/ArcTan.h
         OndselSolver/ArcTan2.h
+        OndselSolver/ASMTAngleJoint.h
         OndselSolver/ASMTAnimationParameters.h
         OndselSolver/ASMTAssembly.h
+        OndselSolver/ASMTCylSphJoint.h
+        OndselSolver/ASMTCompoundJoint.h
         OndselSolver/ASMTConstantGravity.h
+        OndselSolver/ASMTConstantVelocityJoint.h
         OndselSolver/ASMTConstraintSet.h
         OndselSolver/ASMTCylindricalJoint.h
         OndselSolver/ASMTExtrusion.h
         OndselSolver/ASMTFixedJoint.h
         OndselSolver/ASMTForceTorque.h
+        OndselSolver/ASMTGearJoint.h
         OndselSolver/ASMTGeneralMotion.h
+        OndselSolver/ASMTInPlaneJoint.h
         OndselSolver/ASMTItem.h
         OndselSolver/ASMTItemIJ.h
         OndselSolver/ASMTJoint.h
         OndselSolver/ASMTKinematicIJ.h
+        OndselSolver/ASMTLineInPlaneJoint.h
         OndselSolver/ASMTMarker.h
         OndselSolver/ASMTMotion.h
         OndselSolver/ASMTNoRotationJoint.h
+        OndselSolver/ASMTParallelAxesJoint.h
         OndselSolver/ASMTPart.h
+        OndselSolver/ASMTPerpendicularJoint.h
+        OndselSolver/ASMTPlanarJoint.h
         OndselSolver/ASMTPointInLineJoint.h
         OndselSolver/ASMTPointInPlaneJoint.h
         OndselSolver/ASMTPrincipalMassMarker.h
+        OndselSolver/ASMTRackPinionJoint.h
         OndselSolver/ASMTRefCurve.h
         OndselSolver/ASMTRefItem.h
         OndselSolver/ASMTRefPoint.h
         OndselSolver/ASMTRefSurface.h
+        OndselSolver/ASMTRevCylJoint.h
         OndselSolver/ASMTRevoluteJoint.h
         OndselSolver/ASMTRotationalMotion.h
+        OndselSolver/ASMTScrewJoint.h
         OndselSolver/ASMTSimulationParameters.h
         OndselSolver/ASMTSpatialContainer.h
         OndselSolver/ASMTSpatialItem.h
         OndselSolver/ASMTSphericalJoint.h
+        OndselSolver/ASMTSphSphJoint.h
         OndselSolver/ASMTTime.h
         OndselSolver/ASMTTranslationalJoint.h
         OndselSolver/ASMTTranslationalMotion.h
@@ -516,6 +545,7 @@ set(ONDSELSOLVER_HEADERS
         OndselSolver/MBDynControlData.h
         OndselSolver/MBDynData.h
         OndselSolver/MBDynElement.h
+        OndselSolver/MBDynGravity.h
         OndselSolver/MBDynInitialValue.h
         OndselSolver/MBDynItem.h
         OndselSolver/MBDynJoint.h

OndselSolver/ASMTAssembly.cpp

@@ -11,6 +11,7 @@
 #include <fstream>	
 #include <algorithm>
 #include <numeric>
+#include <iomanip>
 
 #include "ASMTAssembly.h"
 #include "CREATE.h"
@@ -25,7 +26,6 @@
 #include "ASMTFixedJoint.h"
 #include "ASMTGeneralMotion.h"
 #include "ASMTUniversalJoint.h"
-#include "ExternalSystem.h"
 #include "ASMTPointInPlaneJoint.h"
 #include "ASMTPrincipalMassMarker.h"
 #include "ASMTForceTorque.h"
@@ -33,14 +33,8 @@
 #include "ASMTSimulationParameters.h"
 #include "ASMTAnimationParameters.h"
 #include "Part.h"
-#include "ASMTRefPoint.h"
-#include "ASMTRefCurve.h"
-#include "ASMTRefSurface.h"
 #include "ASMTTime.h"
-#include "SystemSolver.h"
 #include "ASMTItemIJ.h"
-#include "ASMTKinematicIJ.h"
-#include <iomanip>
 #include "ASMTAngleJoint.h"
 #include "ASMTConstantVelocityJoint.h"
 #include "ASMTCylSphJoint.h"
@@ -56,6 +50,12 @@
 #include "ASMTRackPinionJoint.h"
 #include "ASMTScrewJoint.h"
 #include "SimulationStoppingError.h"
+#include "ASMTKinematicIJ.h"
+#include "ASMTRefPoint.h"
+#include "ASMTRefCurve.h"
+#include "ASMTRefSurface.h"
+#include "ExternalSystem.h"
+#include "SystemSolver.h"
 
 using namespace MbD;
 
@@ -263,7 +263,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<FullMatrixDouble>(ListListD{
+	auto rotMat = std::make_shared<FullMatrix<double>>(ListListD{
 		{ 1, 0, 0 },
 		{ 0, 1, 0 },
 		{ 0, 0, 1 }
@@ -277,11 +277,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>(ListD{ 0, 0, 0 });
+	auto aJ = std::make_shared<DiagonalMatrix<double>>(ListD{ 0, 0, 0 });
 	massMarker->setMomentOfInertias(aJ);
 	pos3D = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
 	massMarker->setPosition3D(pos3D);
-	rotMat = std::make_shared<FullMatrixDouble>(ListListD{
+	rotMat = std::make_shared<FullMatrix<double>>(ListListD{
 		{ 1, 0, 0 },
 		{ 0, 1, 0 },
 		{ 0, 0, 1 }
@@ -294,7 +294,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<FullMatrixDouble>(ListListD{
+	rotMat = std::make_shared<FullMatrix<double>>(ListListD{
 		{ 1, 0, 0 },
 		{ 0, 1, 0 },
 		{ 0, 0, 1 }
@@ -307,7 +307,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<FullMatrixDouble>(ListListD{
+	rotMat = std::make_shared<FullMatrix<double>>(ListListD{
 		{ 1, 0, 0 },
 		{ 0, 1, 0 },
 		{ 0, 0, 1 }
@@ -322,11 +322,11 @@ void MbD::ASMTAssembly::runSinglePendulum()
 	massMarker = std::make_shared<ASMTPrincipalMassMarker>();
 	massMarker->setMass(0.2);
 	massMarker->setDensity(10.0);
-	aJ = std::make_shared<DiagonalMatrix>(ListD{ 8.3333333333333e-4, 0.016833333333333, 0.017333333333333 });
+	aJ = std::make_shared<DiagonalMatrix<double>>(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<FullMatrixDouble>(ListListD{
+	rotMat = std::make_shared<FullMatrix<double>>(ListListD{
 		{ 1, 0, 0 },
 		{ 0, 1, 0 },
 		{ 0, 0, 1 }
@@ -339,7 +339,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<FullMatrixDouble>(ListListD{
+	rotMat = std::make_shared<FullMatrix<double>>(ListListD{
 		{ 1, 0, 0 },
 		{ 0, 1, 0 },
 		{ 0, 0, 1 }
@@ -1052,9 +1052,9 @@ void MbD::ASMTAssembly::initprincipalMassMarker()
 	principalMassMarker = std::make_shared<ASMTPrincipalMassMarker>();
 	principalMassMarker->mass = 0.0;
 	principalMassMarker->density = 0.0;
-	principalMassMarker->momentOfInertias = std::make_shared<DiagonalMatrix>(3, 0);
+	principalMassMarker->momentOfInertias = std::make_shared<DiagonalMatrix<double>>(3, 0);
 	//principalMassMarker->position3D = std::make_shared<FullColumn<double>>(3, 0);
-	//principalMassMarker->rotationMatrix = FullMatrixDouble>::identitysptr(3);
+	//principalMassMarker->rotationMatrix = FullMatrix<double>>::identitysptr(3);
 }
 
 std::shared_ptr<ASMTSpatialContainer> MbD::ASMTAssembly::spatialContainerAt(std::shared_ptr<ASMTAssembly> self, std::string& longname)

OndselSolver/ASMTAssembly.h

@@ -10,33 +10,25 @@
 #include <fstream>	
 
 #include "ASMTSpatialContainer.h"
-#include "FullColumn.h"
-#include "FullMatrix.h"
-#include "MBDynSystem.h"
-#include "ASMTTime.h"
+//Required for initialization
 #include "ASMTConstantGravity.h"
 #include "ASMTSimulationParameters.h"
 #include "ASMTAnimationParameters.h"
+#include "ASMTTime.h"
+#include "Units.h"
 
 namespace MbD {
-	class ASMTRefPoint;
-	class ASMTRefCurve;
-	class ASMTRefSurface;
 	class ASMTPart;
 	class ASMTKinematicIJ;
 	class ASMTConstraintSet;
 	class ASMTForceTorque;
-	class ASMTConstantGravity;
-	// class ASMTSimulationParameters;
-	// class ASMTAnimationParameters;
 	class ASMTJoint;
 	class ASMTMotion;
-	class Units;
-	class ASMTTime;
 	class SystemSolver;
 	class ASMTItemIJ;
+	class MBDynSystem;
 
-	class ASMTAssembly : public ASMTSpatialContainer
+	class EXPORT ASMTAssembly : public ASMTSpatialContainer
 	{
 		//
 	public:

OndselSolver/ASMTAtPointJoint.cpp

@@ -1 +1,3 @@
 #include "ASMTAtPointJoint.h"
+
+using namespace MbD;

OndselSolver/ASMTInLineJoint.cpp

@@ -1,2 +1,4 @@
 
 #include "ASMTInLineJoint.h"
+
+using namespace MbD;

OndselSolver/ASMTInLineJoint.h

@@ -11,7 +11,7 @@
 #include "ASMTJoint.h"
 
 namespace MbD {
-    class ASMTInLineJoint : public ASMTJoint
+    class EXPORT ASMTInLineJoint : public ASMTJoint
     {
         //
     public:

OndselSolver/ASMTInPlaneJoint.cpp

@@ -2,6 +2,8 @@
 #include "ASMTInPlaneJoint.h"
 #include "InPlaneJoint.h"
 
+using namespace MbD;
+
 void MbD::ASMTInPlaneJoint::parseASMT(std::vector<std::string>& lines)
 {
 	ASMTJoint::parseASMT(lines);

OndselSolver/ASMTInPlaneJoint.h

@@ -11,7 +11,7 @@
 #include "ASMTJoint.h"
 
 namespace MbD {
-    class ASMTInPlaneJoint : public ASMTJoint
+    class EXPORT ASMTInPlaneJoint : public ASMTJoint
     {
         //
     public:

OndselSolver/ASMTItem.cpp

@@ -151,6 +151,7 @@ void MbD::ASMTItem::deleteMbD()
 
 void MbD::ASMTItem::createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits)
 {
+	noop();
 	assert(false);
 }
 
@@ -184,6 +185,7 @@ std::shared_ptr<Constant> MbD::ASMTItem::sptrConstant(double value)
 
 void MbD::ASMTItem::storeOnLevel(std::ofstream& os, int level)
 {
+	noop();
 	assert(false);
 }
 

OndselSolver/ASMTKinematicIJ.h

@@ -11,7 +11,7 @@
 #include "ASMTItemIJ.h"
 
 namespace MbD {
-    class ASMTKinematicIJ : public ASMTItemIJ
+    class EXPORT ASMTKinematicIJ : public ASMTItemIJ
     {
         //
     public:

OndselSolver/ASMTPrincipalMassMarker.cpp

@@ -31,7 +31,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<FullMatrixDouble>(3);
+	rotationMatrix = std::make_shared<FullMatrix<double>>(3);
 	for (int i = 0; i < 3; i++)
 	{
 		auto row = readRowOfDoubles(lines[0]);
@@ -44,7 +44,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>(3);
+	momentOfInertias = std::make_shared<DiagonalMatrix<double>>(3);
 	auto row = readRowOfDoubles(lines[0]);
 	lines.erase(lines.begin());
 	for (int i = 0; i < 3; i++)
@@ -75,7 +75,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>(ListD{ a, b, c });
+	momentOfInertias = std::make_shared<DiagonalMatrix<double>>(ListD{ a, b, c });
 }
 
 void MbD::ASMTPrincipalMassMarker::storeOnLevel(std::ofstream& os, int level)

OndselSolver/ASMTPrincipalMassMarker.h

@@ -27,7 +27,7 @@ namespace MbD {
 
 		double mass = 1.0;
 		double density = 10.0;
-		DiagMatDsptr momentOfInertias = std::make_shared<DiagonalMatrix>(ListD{ 1.0, 2.0, 3.0 });
+		DiagMatDsptr momentOfInertias = std::make_shared<DiagonalMatrix<double>>(ListD{ 1.0, 2.0, 3.0 });
 
 	};
 }

OndselSolver/ASMTRevoluteJoint.cpp

@@ -16,9 +16,9 @@ std::shared_ptr<Joint> MbD::ASMTRevoluteJoint::mbdClassNew()
 {
 	return CREATE<RevoluteJoint>::With();
 }
-
-void MbD::ASMTRevoluteJoint::storeOnTimeSeries(std::ofstream& os)
-{
-	os << "RevoluteJointSeries\t" << fullName("") << std::endl;
-	ASMTItemIJ::storeOnTimeSeries(os);
-}
+//
+//void MbD::ASMTRevoluteJoint::storeOnTimeSeries(std::ofstream& os)
+//{
+//	os << "RevoluteJointSeries\t" << fullName("") << std::endl;
+//	ASMTItemIJ::storeOnTimeSeries(os);
+//}

OndselSolver/ASMTRevoluteJoint.h

@@ -16,7 +16,7 @@ namespace MbD {
         //
     public:
         std::shared_ptr<Joint> mbdClassNew() override;
-        void storeOnTimeSeries(std::ofstream& os) override;
+        //void storeOnTimeSeries(std::ofstream& os) override;
 
     };
 }

OndselSolver/ASMTSpatialItem.cpp

@@ -48,7 +48,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<FullMatrixDouble>(3, 0);
+	rotationMatrix = std::make_shared<FullMatrix<double>>(3, 0);
 	for (int i = 0; i < 3; i++)
 	{
 		auto& row = rotationMatrix->at(i);
@@ -87,7 +87,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<FullMatrixDouble>(ListListD{
+	rotationMatrix = std::make_shared<FullMatrix<double>>(ListListD{
 		{ v11, v12, v13 },
 		{ v21, v22, v23 },
 		{ v31, v32, v33 }

OndselSolver/ASMTSpatialItem.h

@@ -35,7 +35,7 @@ namespace MbD {
 		FMatDsptr getRotationMatrix(int i);
 
 		FColDsptr position3D = std::make_shared<FullColumn<double>>(3);
-		FMatDsptr rotationMatrix = std::make_shared<FullMatrixDouble>(ListListD{
+		FMatDsptr rotationMatrix = std::make_shared<FullMatrix<double>>(ListListD{
 				{ 1, 0, 0 },
 				{ 0, 1, 0 },
 				{ 0, 0, 1 }

OndselSolver/AngleZIeqcJec.cpp

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

OndselSolver/AngleZIeqcJeqc.cpp

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

OndselSolver/AnyPosICNewtonRaphson.cpp

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

OndselSolver/CADSystem.cpp

@@ -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>(ListD{ 0, 0, 0 });
+	assembly1->aJ = std::make_shared<DiagonalMatrix<double>>(ListD{ 0, 0, 0 });
 	qX = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
-	aAap = std::make_shared<FullMatrixDouble>(ListListD{
+	aAap = std::make_shared<FullMatrix<double>>(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<FullMatrixDouble>(ListListD{
+		aApm = std::make_shared<FullMatrix<double>>(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<FullMatrixDouble>(ListListD{
+		aApm = std::make_shared<FullMatrix<double>>(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>(ListD{ 1, 1, 1 });
+	crankPart1->aJ = std::make_shared<DiagonalMatrix<double>>(ListD{ 1, 1, 1 });
 	qX = std::make_shared<FullColumn<double>>(ListD{ 0.4, 0.0, -0.05 });
-	aAap = std::make_shared<FullMatrixDouble>(ListListD{
+	aAap = std::make_shared<FullMatrix<double>>(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<FullMatrixDouble>(ListListD{
+		aApm = std::make_shared<FullMatrix<double>>(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<FullMatrixDouble>(ListListD{
+		aApm = std::make_shared<FullMatrix<double>>(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>(ListD{ 0, 0, 0 });
+	assembly1->aJ = std::make_shared<DiagonalMatrix<double>>(ListD{ 0, 0, 0 });
 	qX = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
-	aAap = std::make_shared<FullMatrixDouble>(ListListD{
+	aAap = std::make_shared<FullMatrix<double>>(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<FullMatrixDouble>(ListListD{
+		aApm = std::make_shared<FullMatrix<double>>(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<FullMatrixDouble>(ListListD{
+		aApm = std::make_shared<FullMatrix<double>>(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>(ListD{ 1, 1, 1 });
+	crankPart1->aJ = std::make_shared<DiagonalMatrix<double>>(ListD{ 1, 1, 1 });
 	qX = std::make_shared<FullColumn<double>>(ListD{ 0.4, 0.0, -0.05 });
-	aAap = std::make_shared<FullMatrixDouble>(ListListD{
+	aAap = std::make_shared<FullMatrix<double>>(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<FullMatrixDouble>(ListListD{
+		aApm = std::make_shared<FullMatrix<double>>(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<FullMatrixDouble>(ListListD{
+		aApm = std::make_shared<FullMatrix<double>>(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>(ListD{ 1, 1, 1 });
+	conrodPart2->aJ = std::make_shared<DiagonalMatrix<double>>(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<FullMatrixDouble>(ListListD{
+		aApm = std::make_shared<FullMatrix<double>>(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<FullMatrixDouble>(ListListD{
+		aApm = std::make_shared<FullMatrix<double>>(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>(ListD{ 0, 0, 0 });
+	assembly1->aJ = std::make_shared<DiagonalMatrix<double>>(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<FullMatrixDouble>(ListListD{
+		aApm = std::make_shared<FullMatrix<double>>(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<FullMatrixDouble>(ListListD{
+		aApm = std::make_shared<FullMatrix<double>>(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>(ListD{ 1, 1, 1 });
+	crankPart1->aJ = std::make_shared<DiagonalMatrix<double>>(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<FullMatrixDouble>(ListListD{
+		aApm = std::make_shared<FullMatrix<double>>(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<FullMatrixDouble>(ListListD{
+		aApm = std::make_shared<FullMatrix<double>>(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>(ListD{ 1, 1, 1 });
+	conrodPart2->aJ = std::make_shared<DiagonalMatrix<double>>(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<FullMatrixDouble>(ListListD{
+		aApm = std::make_shared<FullMatrix<double>>(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<FullMatrixDouble>(ListListD{
+		aApm = std::make_shared<FullMatrix<double>>(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>(ListD{ 1, 1, 1 });
+	pistonPart3->aJ = std::make_shared<DiagonalMatrix<double>>(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<FullMatrixDouble>(ListListD{
+		aApm = std::make_shared<FullMatrix<double>>(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<FullMatrixDouble>(ListListD{
+		aApm = std::make_shared<FullMatrix<double>>(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>(ListD{ 0, 0, 0 });
+	assembly1->aJ = std::make_shared<DiagonalMatrix<double>>(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<FullMatrixDouble>(ListListD{
+		aApm = std::make_shared<FullMatrix<double>>(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<FullMatrixDouble>(ListListD{
+		aApm = std::make_shared<FullMatrix<double>>(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>(ListD{ 1.7381980042084e-4, 0.003511159968501, 0.0036154518487535 });
+	crankPart1->aJ = std::make_shared<DiagonalMatrix<double>>(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<FullMatrixDouble>(ListListD{
+		aApm = std::make_shared<FullMatrix<double>>(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<FullMatrixDouble>(ListListD{
+		aApm = std::make_shared<FullMatrix<double>>(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>(ListD{ 2.6072970063126e-4, 0.011784982468533, 0.011941420288912 });
+	conrodPart2->aJ = std::make_shared<DiagonalMatrix<double>>(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<FullMatrixDouble>(ListListD{
+		aApm = std::make_shared<FullMatrix<double>>(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<FullMatrixDouble>(ListListD{
+		aApm = std::make_shared<FullMatrix<double>>(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>(ListD{ 0.19449049546716, 0.23028116340971, 0.23028116340971 });
+	pistonPart3->aJ = std::make_shared<DiagonalMatrix<double>>(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<FullMatrixDouble>(ListListD{
+		aApm = std::make_shared<FullMatrix<double>>(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<FullMatrixDouble>(ListListD{
+		aApm = std::make_shared<FullMatrix<double>>(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}

OndselSolver/ConstVelConstraintIqcJc.cpp

@@ -19,7 +19,7 @@ using namespace MbD;
 MbD::ConstVelConstraintIqcJc::ConstVelConstraintIqcJc(EndFrmsptr frmi, EndFrmsptr frmj) : ConstVelConstraintIJ(frmi, frmj)
 {
 	pGpEI = std::make_shared<FullRow<double>>(4);
-	ppGpEIpEI = std::make_shared<FullMatrixDouble>(4, 4);
+	ppGpEIpEI = std::make_shared<FullMatrix<double>>(4, 4);
 }
 
 void MbD::ConstVelConstraintIqcJc::calcPostDynCorrectorIteration()
@@ -99,7 +99,7 @@ void MbD::ConstVelConstraintIqcJc::initialize()
 {
 	ConstVelConstraintIJ::initialize();
 	pGpEI = std::make_shared<FullRow<double>>(4);
-	ppGpEIpEI = std::make_shared<FullMatrixDouble>(4, 4);
+	ppGpEIpEI = std::make_shared<FullMatrix<double>>(4, 4);
 }
 
 void MbD::ConstVelConstraintIqcJc::useEquationNumbers()

OndselSolver/ConstVelConstraintIqcJqc.cpp

@@ -16,8 +16,8 @@ using namespace MbD;
 MbD::ConstVelConstraintIqcJqc::ConstVelConstraintIqcJqc(EndFrmsptr frmi, EndFrmsptr frmj) : ConstVelConstraintIqcJc(frmi, frmj)
 {
 	pGpEJ = std::make_shared<FullRow<double>>(4);
-	ppGpEIpEJ = std::make_shared<FullMatrixDouble>(4, 4);
-	ppGpEJpEJ = std::make_shared<FullMatrixDouble>(4, 4);
+	ppGpEIpEJ = std::make_shared<FullMatrix<double>>(4, 4);
+	ppGpEJpEJ = std::make_shared<FullMatrix<double>>(4, 4);
 }
 
 void MbD::ConstVelConstraintIqcJqc::calcPostDynCorrectorIteration()
@@ -121,8 +121,8 @@ void MbD::ConstVelConstraintIqcJqc::initialize()
 {
 	ConstVelConstraintIqcJc::initialize();
 	pGpEJ = std::make_shared<FullRow<double>>(4);
-	ppGpEIpEJ = std::make_shared<FullMatrixDouble>(4, 4);
-	ppGpEJpEJ = std::make_shared<FullMatrixDouble>(4, 4);
+	ppGpEIpEJ = std::make_shared<FullMatrix<double>>(4, 4);
+	ppGpEJpEJ = std::make_shared<FullMatrix<double>>(4, 4);
 }
 
 void MbD::ConstVelConstraintIqcJqc::useEquationNumbers()

OndselSolver/DiagonalMatrix.cpp

@@ -7,90 +7,5 @@
  ***************************************************************************/
  
 #include "DiagonalMatrix.h"
-#include "FullMatrix.h"
 
-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;
-    }
-}
+using namespace MbD;

OndselSolver/DiagonalMatrix.h

@@ -8,26 +8,30 @@
 
 #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 {
-	class DiagonalMatrix : public Array<double>
+	template<typename T>
+	class DiagonalMatrix;
+	template<typename T>
+	using DiagMatsptr = std::shared_ptr<DiagonalMatrix<T>>;
+	using DiagMatDsptr = std::shared_ptr<DiagonalMatrix<double>>;
+
+	template<typename T>
+	class DiagonalMatrix : public Array<T>
 	{
 		//
 	public:
-		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);
+		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);
 		int nrow() {
 			return (int)this->size();
 		}
@@ -40,6 +44,109 @@ 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;
+	}
 }
 

OndselSolver/DiagonalMatrix.ref.h

@@ -1,9 +0,0 @@
-#pragma once
-
-#include <memory>
-
-namespace MbD {
-    class DiagonalMatrix;
-    using DiagMatsptr = std::shared_ptr<DiagonalMatrix>;
-    using DiagMatDsptr = std::shared_ptr<DiagonalMatrix>;
-}

OndselSolver/DifferenceOperator.cpp

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

OndselSolver/DirectionCosineIecJec.cpp

@@ -25,7 +25,7 @@ namespace MbD {
     {
         aAjOIe = frmI->aAjOe(axisI);
         aAjOJe = frmJ->aAjOe(axisJ);
-        aAijIeJe = aAjOIe->dotVec(aAjOJe);
+        aAijIeJe = aAjOIe->dot(aAjOJe);
     }
 
     double MbD::DirectionCosineIecJec::value()

OndselSolver/DirectionCosineIeqcJec.cpp

@@ -24,7 +24,7 @@ void DirectionCosineIeqcJec::initialize()
 {
 	DirectionCosineIecJec::initialize();
 	pAijIeJepEI = std::make_shared<FullRow<double>>(4);
-	ppAijIeJepEIpEI = std::make_shared<FullMatrixDouble>(4, 4);
+	ppAijIeJepEIpEI = std::make_shared<FullMatrix<double>>(4, 4);
 }
 
 void DirectionCosineIeqcJec::initializeGlobally()
@@ -56,7 +56,7 @@ void DirectionCosineIeqcJec::calcPostDynCorrectorIteration()
 		auto& ppAjOIepEIipEI = ppAjOIepEIpEI->at(i);
 		for (int j = 0; j < 4; j++)
 		{
-			ppAijIeJepEIipEI->at(j) = ppAjOIepEIipEI->at(j)->dotVec(aAjOJe);
+			ppAijIeJepEIipEI->at(j) = ppAjOIepEIipEI->at(j)->dot(aAjOJe);
 		}
 	}	
 	ppAijIeJepEIpEI->symLowerWithUpper();

OndselSolver/DirectionCosineIeqcJeqc.cpp

@@ -24,8 +24,8 @@ void DirectionCosineIeqcJeqc::initialize()
 {
 	DirectionCosineIeqcJec::initialize();
 	pAijIeJepEJ = std::make_shared<FullRow<double>>(4);
-	ppAijIeJepEIpEJ = std::make_shared<FullMatrixDouble>(4, 4);
-	ppAijIeJepEJpEJ = std::make_shared<FullMatrixDouble>(4, 4);
+	ppAijIeJepEIpEJ = std::make_shared<FullMatrix<double>>(4, 4);
+	ppAijIeJepEJpEJ = std::make_shared<FullMatrix<double>>(4, 4);
 }
 
 void DirectionCosineIeqcJeqc::initializeGlobally()
@@ -45,7 +45,7 @@ void DirectionCosineIeqcJeqc::calcPostDynCorrectorIteration()
 	pAjOJepEJT = std::static_pointer_cast<EndFrameqc>(frmJ)->pAjOepET(axisJ);
 	for (int i = 0; i < 4; i++)
 	{
-		pAijIeJepEJ->at(i) = aAjOIe->dotVec(pAjOJepEJT->at(i));
+		pAijIeJepEJ->at(i) = aAjOIe->dot(pAjOJepEJT->at(i));
 	}
 	for (int i = 0; i < 4; i++)
 	{
@@ -61,7 +61,7 @@ void DirectionCosineIeqcJeqc::calcPostDynCorrectorIteration()
 		auto& ppAjOJepEJipEJ = ppAjOJepEJpEJ->at(i);
 		for (int j = 0; j < 4; j++)
 		{
-			ppAijIeJepEJipEJ->at(j) = aAjOIe->dotVec(ppAjOJepEJipEJ->at(j));
+			ppAijIeJepEJipEJ->at(j) = aAjOIe->dot(ppAjOJepEJipEJ->at(j));
 		}
 	}
 	ppAijIeJepEJpEJ->symLowerWithUpper();

OndselSolver/DirectionCosineIeqctJeqc.cpp

@@ -60,7 +60,7 @@ void DirectionCosineIeqctJeqc::preVelIC()
 {
 	Item::preVelIC();
 	auto pAjOIept = std::static_pointer_cast<EndFrameqct>(frmI)->pAjOept(axisI);
-	pAijIeJept = pAjOIept->dotVec(aAjOJe);
+	pAijIeJept = pAjOIept->dot(aAjOJe);
 }
 
 double DirectionCosineIeqctJeqc::pvaluept()
@@ -82,7 +82,7 @@ void DirectionCosineIeqctJeqc::preAccIC()
 	}
 	for (int i = 0; i < 4; i++)
 	{
-		ppAijIeJepEJpt->atiput(i, pAjOIept->dotVec(pAjOJepEJT->at(i)));
+		ppAijIeJepEJpt->atiput(i, pAjOIept->dot(pAjOJepEJT->at(i)));
 	}
-	ppAijIeJeptpt = ppAjOIeptpt->dotVec(aAjOJe);
+	ppAijIeJeptpt = ppAjOIeptpt->dot(aAjOJe);
 }

OndselSolver/DispCompIecJecIe.cpp

@@ -23,7 +23,7 @@ void MbD::DispCompIecJecIe::calc_value()
 {
 	aAjOIe = frmI->aAjOe(axis);
 	rIeJeO = frmJ->rOeO->minusFullColumn(frmI->rOeO);
-	riIeJeIe = aAjOIe->dotVec(rIeJeO);
+	riIeJeIe = aAjOIe->dot(rIeJeO);
 }
 
 void MbD::DispCompIecJecIe::calcPostDynCorrectorIteration()

OndselSolver/DispCompIecJecKeqc.cpp

@@ -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<FullMatrixDouble>(4, 4);
+    ppriIeJeKepEKpEK = std::make_shared<FullMatrix<double>>(4, 4);
 }
 
 void DispCompIecJecKeqc::initializeGlobally()
@@ -37,7 +37,7 @@ void DispCompIecJecKeqc::calcPostDynCorrectorIteration()
 	auto efrmKqc = std::static_pointer_cast<EndFrameqc>(efrmK);
 	aAjOKe = efrmKqc->aAjOe(axisK);
 	rIeJeO = frmJqc->rOeO->minusFullColumn(frmIqc->rOeO);
-	riIeJeKe = aAjOKe->dotVec(rIeJeO);
+	riIeJeKe = aAjOKe->dot(rIeJeO);
 	pAjOKepEKT = efrmKqc->pAjOepET(axisK);
 	ppAjOKepEKpEK = efrmKqc->ppAjOepEpE(axisK);
 	for (int i = 0; i < 4; i++)
@@ -45,10 +45,10 @@ void DispCompIecJecKeqc::calcPostDynCorrectorIteration()
 		priIeJeKepEK->at(i) = ((pAjOKepEKT->at(i))->dot(rIeJeO));
 		auto& ppAjOKepEKipEK = ppAjOKepEKpEK->at(i);
 		auto& ppriIeJeKepEKipEK = ppriIeJeKepEKpEK->at(i);
-		ppriIeJeKepEKipEK->at(i) = ((ppAjOKepEKipEK->at(i))->dotVec(rIeJeO));
+		ppriIeJeKepEKipEK->at(i) = ((ppAjOKepEKipEK->at(i))->dot(rIeJeO));
 		for (int j = i + 1; j < 4; j++)
 		{
-			auto ppriIeJeKepEKipEKj = (ppAjOKepEKipEK->at(i))->dotVec(rIeJeO);
+			auto ppriIeJeKepEKipEKj = (ppAjOKepEKipEK->at(i))->dot(rIeJeO);
 			ppriIeJeKepEKipEK->at(j) = ppriIeJeKepEKipEKj;
 			ppriIeJeKepEKpEK->at(j)->at(i) = ppriIeJeKepEKipEKj;
 		}

OndselSolver/DispCompIeqcJecIe.cpp

@@ -19,8 +19,8 @@ MbD::DispCompIeqcJecIe::DispCompIeqcJecIe(EndFrmsptr frmi, EndFrmsptr frmj, int
 {
 	priIeJeIepXI = std::make_shared<FullRow<double>>(3);
 	priIeJeIepEI = std::make_shared<FullRow<double>>(4);
-	ppriIeJeIepXIpEI = std::make_shared<FullMatrixDouble>(3, 4);
-	ppriIeJeIepEIpEI = std::make_shared<FullMatrixDouble>(4, 4);
+	ppriIeJeIepXIpEI = std::make_shared<FullMatrix<double>>(3, 4);
+	ppriIeJeIepEIpEI = std::make_shared<FullMatrix<double>>(4, 4);
 }
 
 void MbD::DispCompIeqcJecIe::calc_ppvaluepEIpEI()
@@ -35,10 +35,10 @@ void MbD::DispCompIeqcJecIe::calc_ppvaluepEIpEI()
 		auto ppriIeJeIepEIipEI = ppriIeJeIepEIpEI->at(i);
 		for (int j = i; j < 4; j++)
 		{
-			auto term1 = ppAjOIepEIipEI->at(j)->dotVec(rIeJeO);
+			auto term1 = ppAjOIepEIipEI->at(j)->dot(rIeJeO);
 			auto mterm2 = pAjOIepEIT->at(i)->dot(mprIeJeOpEIT->at(j));
 			auto mterm3 = (i == j) ? mterm2 : pAjOIepEIT->at(j)->dot(mprIeJeOpEIT->at(i));
-			auto mterm4 = aAjOIe->dotVec(mpprIeJeOpEIipEI->at(j));
+			auto mterm4 = aAjOIe->dot(mpprIeJeOpEIipEI->at(j));
 			ppriIeJeIepEIipEI->atiput(j, term1 - mterm2 - mterm3 - mterm4);
 		}
 	}
@@ -64,7 +64,7 @@ void MbD::DispCompIeqcJecIe::calc_pvaluepEI()
 	auto mprIeJeOpEIT = frmIeqc->prOeOpE->transpose();
 	for (int i = 0; i < 4; i++)
 	{
-		priIeJeIepEI->atiput(i, pAjOIepEIT->at(i)->dot(rIeJeO) - aAjOIe->dotVec(mprIeJeOpEIT->at(i)));
+		priIeJeIepEI->atiput(i, pAjOIepEIT->at(i)->dot(rIeJeO) - aAjOIe->dot(mprIeJeOpEIT->at(i)));
 	}
 }
 
@@ -91,8 +91,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<FullMatrixDouble>(3, 4);
-	ppriIeJeIepEIpEI = std::make_shared<FullMatrixDouble>(4, 4);
+	ppriIeJeIepXIpEI = std::make_shared<FullMatrix<double>>(3, 4);
+	ppriIeJeIepEIpEI = std::make_shared<FullMatrix<double>>(4, 4);
 }
 
 void MbD::DispCompIeqcJecIe::initializeGlobally()

OndselSolver/DispCompIeqcJecKeqc.cpp

@@ -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<FullMatrixDouble>(4, 4);
-	ppriIeJeKepXIpEK = std::make_shared<FullMatrixDouble>(3, 4);
-	ppriIeJeKepEIpEK = std::make_shared<FullMatrixDouble>(4, 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);
 }
 
 void DispCompIeqcJecKeqc::calcPostDynCorrectorIteration()
@@ -41,7 +41,7 @@ void DispCompIeqcJecKeqc::calcPostDynCorrectorIteration()
 	}
 	for (int i = 0; i < 4; i++)
 	{
-		priIeJeKepEI->at(i) = 0.0 - (aAjOKe->dotVec(mprIeJeOpEIT->at(i)));
+		priIeJeKepEI->at(i) = 0.0 - (aAjOKe->dot(mprIeJeOpEIT->at(i)));
 	}
 	for (int i = 0; i < 3; i++)
 	{
@@ -55,10 +55,10 @@ void DispCompIeqcJecKeqc::calcPostDynCorrectorIteration()
 	{
 		auto& mpprIeJeOpEIipEI = mpprIeJeOpEIpEI->at(i);
 		auto& ppriIeJeKepEIipEI = ppriIeJeKepEIpEI->at(i);
-		ppriIeJeKepEIipEI->at(i) = 0.0 - (aAjOKe->dotVec(mpprIeJeOpEIipEI->at(i)));
+		ppriIeJeKepEIipEI->at(i) = 0.0 - (aAjOKe->dot(mpprIeJeOpEIipEI->at(i)));
 		for (int j = 0; j < 4; j++)
 		{
-			auto ppriIeJeKepEIipEIj = 0.0 - (aAjOKe->dotVec(mpprIeJeOpEIipEI->at(j)));
+			auto ppriIeJeKepEIipEIj = 0.0 - (aAjOKe->dot(mpprIeJeOpEIipEI->at(j)));
 			ppriIeJeKepEIipEI->at(j) = ppriIeJeKepEIipEIj;
 			ppriIeJeKepEIpEI->at(j)->at(i) = ppriIeJeKepEIipEIj;
 		}

OndselSolver/DispCompIeqcJeqcIe.cpp

@@ -19,9 +19,9 @@ MbD::DispCompIeqcJeqcIe::DispCompIeqcJeqcIe(EndFrmsptr frmi, EndFrmsptr frmj, in
 {
 	priIeJeIepXJ = std::make_shared<FullRow<double>>(3);
 	priIeJeIepEJ = std::make_shared<FullRow<double>>(4);
-	ppriIeJeIepEIpXJ = std::make_shared<FullMatrixDouble>(4, 3);
-	ppriIeJeIepEIpEJ = std::make_shared<FullMatrixDouble>(4, 4);
-	ppriIeJeIepEJpEJ = std::make_shared<FullMatrixDouble>(4, 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);
 }
 
 void MbD::DispCompIeqcJeqcIe::calc_ppvaluepEIpEJ()
@@ -46,7 +46,7 @@ void MbD::DispCompIeqcJeqcIe::calc_ppvaluepEJpEJ()
 		auto ppriIeJeIepEJipEJ = ppriIeJeIepEJpEJ->at(i);
 		for (int j = i; j < 4; j++)
 		{
-			auto term1 = aAjOIe->dotVec(pprIeJeOpEJipEJ->at(j));
+			auto term1 = aAjOIe->dot(pprIeJeOpEJipEJ->at(j));
 			ppriIeJeIepEJipEJ->atiput(j, term1);
 		}
 	}
@@ -59,7 +59,7 @@ void MbD::DispCompIeqcJeqcIe::calc_pvaluepEJ()
 	auto prIeJeOpEJT = frmJeqc->prOeOpE->transpose();
 	for (int i = 0; i < 4; i++)
 	{
-		priIeJeIepEJ->atiput(i, aAjOIe->dotVec(prIeJeOpEJT->at(i)));
+		priIeJeIepEJ->atiput(i, aAjOIe->dot(prIeJeOpEJT->at(i)));
 	}
 }
 
@@ -88,9 +88,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<FullMatrixDouble>(4, 3);
-	ppriIeJeIepEIpEJ = std::make_shared<FullMatrixDouble>(4, 4);
-	ppriIeJeIepEJpEJ = std::make_shared<FullMatrixDouble>(4, 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);
 }
 
 FMatDsptr MbD::DispCompIeqcJeqcIe::ppvaluepEIpEJ()

OndselSolver/DispCompIeqcJeqcKeqc.cpp

@@ -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<FullMatrixDouble>(4, 4);
-	ppriIeJeKepXJpEK = std::make_shared<FullMatrixDouble>(3, 4);
-	ppriIeJeKepEJpEK = std::make_shared<FullMatrixDouble>(4, 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);
 }
 
 void DispCompIeqcJeqcKeqc::calcPostDynCorrectorIteration()
@@ -41,7 +41,7 @@ void DispCompIeqcJeqcKeqc::calcPostDynCorrectorIteration()
 	}
 	for (int i = 0; i < 4; i++)
 	{
-		priIeJeKepEJ->atiput(i, aAjOKe->dotVec(prIeJeOpEJT->at(i)));
+		priIeJeKepEJ->atiput(i, aAjOKe->dot(prIeJeOpEJT->at(i)));
 	}
 	for (int i = 0; i < 3; i++)
 	{
@@ -55,10 +55,10 @@ void DispCompIeqcJeqcKeqc::calcPostDynCorrectorIteration()
 	{
 		auto& pprIeJeOpEJipEJ = pprIeJeOpEJpEJ->at(i);
 		auto& ppriIeJeKepEJipEJ = ppriIeJeKepEJpEJ->at(i);
-		ppriIeJeKepEJipEJ->atiput(i, aAjOKe->dotVec(pprIeJeOpEJipEJ->at(i)));
+		ppriIeJeKepEJipEJ->atiput(i, aAjOKe->dot(pprIeJeOpEJipEJ->at(i)));
 		for (int j = 0; j < 4; j++)
 		{
-			auto ppriIeJeKepEJipEJj = (aAjOKe->dotVec(pprIeJeOpEJipEJ->at(j)));
+			auto ppriIeJeKepEJipEJj = (aAjOKe->dot(pprIeJeOpEJipEJ->at(j)));
 			ppriIeJeKepEJipEJ->atiput(j, ppriIeJeKepEJipEJj);
 			ppriIeJeKepEJpEJ->atijput(j, i, ppriIeJeKepEJipEJj);
 		}

OndselSolver/DispCompIeqcJeqcKeqct.cpp

@@ -47,7 +47,7 @@ void DispCompIeqcJeqcKeqct::preVelIC()
 {
 	Item::preVelIC();
 	auto pAjOKept = std::static_pointer_cast<EndFrameqct>(efrmK)->pAjOept(axisK);
-	priIeJeKept = pAjOKept->dotVec(rIeJeO);
+	priIeJeKept = pAjOKept->dot(rIeJeO);
 }
 
 double DispCompIeqcJeqcKeqct::pvaluept()
@@ -100,9 +100,9 @@ void DispCompIeqcJeqcKeqct::preAccIC()
 	}
 	for (int i = 0; i < 4; i++)
 	{
-		ppriIeJeKepEIpt->atiput(i, pAjOKept->dotVec(prIeJeOpEIT->at(i)));
-		ppriIeJeKepEJpt->atiput(i, pAjOKept->dotVec(prIeJeOpEJT->at(i)));
+		ppriIeJeKepEIpt->atiput(i, pAjOKept->dot(prIeJeOpEIT->at(i)));
+		ppriIeJeKepEJpt->atiput(i, pAjOKept->dot(prIeJeOpEJT->at(i)));
 		ppriIeJeKepEKpt->atiput(i, ppAjOKepEKTpt->at(i)->dot(rIeJeO));
 	}
-	ppriIeJeKeptpt = ppAjOKeptpt->dotVec(rIeJeO);
+	ppriIeJeKeptpt = ppAjOKeptpt->dot(rIeJeO);
 }

OndselSolver/DispCompIeqctJeqcIe.cpp

@@ -38,8 +38,8 @@ void MbD::DispCompIeqctJeqcIe::calc_ppvaluepEIpt()
 		auto mprIeJeOpEITi = mprIeJeOpEIT->at(i);
 		auto mpprIeJeOpEITpti = mpprIeJeOpEITpt->at(i);
 		auto ppriIeJeIepEIpti = ppAjOIepEITpti->dot(rIeJeO) - pAjOIepEITi->dot(mprIeJeOpt) -
-                pAjOIept->dotVec(mprIeJeOpEITi) -
-                                aAjOIe->dotVec(mpprIeJeOpEITpti);
+                pAjOIept->dot(mprIeJeOpEITi) -
+                                aAjOIe->dot(mpprIeJeOpEITpti);
 		ppriIeJeIepEIpt->atiput(i, ppriIeJeIepEIpti);
 	}
 }
@@ -52,7 +52,7 @@ void MbD::DispCompIeqctJeqcIe::calc_ppvaluepEJpt()
 	auto prIeJeOpEJT = frmJeqct->prOeOpE->transpose();
 	for (int i = 0; i < 4; i++)
 	{
-		ppriIeJeIepEJpt->atiput(i, pAjOIept->dotVec(prIeJeOpEJT->at(i)));
+		ppriIeJeIepEJpt->atiput(i, pAjOIept->dot(prIeJeOpEJT->at(i)));
 	}
 }
 
@@ -63,8 +63,8 @@ void MbD::DispCompIeqctJeqcIe::calc_ppvalueptpt()
 	auto ppAjOIeptpt = frmIeqct->ppAjOeptpt(axis);
 	auto mprIeJeOpt = frmIeqct->prOeOpt;
 	auto mpprIeJeOptpt = frmIeqct->pprOeOptpt;
-	ppriIeJeIeptpt = ppAjOIeptpt->dotVec(rIeJeO) - pAjOIept->dotVec(mprIeJeOpt) - pAjOIept->dotVec(mprIeJeOpt) -
-                     aAjOIe->dotVec(mpprIeJeOptpt);
+	ppriIeJeIeptpt = ppAjOIeptpt->dot(rIeJeO) - pAjOIept->dot(mprIeJeOpt) - pAjOIept->dot(mprIeJeOpt) -
+                     aAjOIe->dot(mpprIeJeOptpt);
 }
 
 void MbD::DispCompIeqctJeqcIe::calc_ppvaluepXIpt()
@@ -92,7 +92,7 @@ void MbD::DispCompIeqctJeqcIe::calc_pvaluept()
 	auto frmIeqct = std::static_pointer_cast<EndFrameqct>(frmI);
 	auto pAjOIept = frmIeqct->pAjOept(axis);
 	auto mprIeJeOpt = frmIeqct->prOeOpt;
-	priIeJeIept = pAjOIept->dotVec(rIeJeO) - aAjOIe->dotVec(mprIeJeOpt);
+	priIeJeIept = pAjOIept->dot(rIeJeO) - aAjOIe->dot(mprIeJeOpt);
 }
 
 void MbD::DispCompIeqctJeqcIe::calcPostDynCorrectorIteration()

OndselSolver/DispCompIeqctJeqcKeqct.cpp

@@ -23,7 +23,7 @@ void DispCompIeqctJeqcKeqct::preVelIC()
 {
 	DispCompIeqcJeqcKeqct::preVelIC();
 	auto& mprIeJeOpt = std::static_pointer_cast<EndFrameqct>(frmI)->prOeOpt;
-	priIeJeKept -= aAjOKe->dotVec(mprIeJeOpt);
+	priIeJeKept -= aAjOKe->dot(mprIeJeOpt);
 }
 
 void DispCompIeqctJeqcKeqct::preAccIC()
@@ -36,8 +36,8 @@ void DispCompIeqctJeqcKeqct::preAccIC()
 	auto& mpprIeJeOptpt = efrmIqct->pprOeOptpt;
 	for (int i = 0; i < 4; i++)
 	{
-		ppriIeJeKepEIpt->atiminusNumber(i, aAjOKe->dotVec(mpprIeJeOpEITpt->at(i)));
+		ppriIeJeKepEIpt->atiminusNumber(i, aAjOKe->dot(mpprIeJeOpEITpt->at(i)));
 		ppriIeJeKepEKpt->atiminusNumber(i, pAjOKepEKT->at(i)->dot(mprIeJeOpt));
 	}
-	ppriIeJeKeptpt += -(2.0 * pAjOKept->dotVec(mprIeJeOpt)) - aAjOKe->dotVec(mpprIeJeOptpt);
+	ppriIeJeKeptpt += -(2.0 * pAjOKept->dot(mprIeJeOpt)) - aAjOKe->dot(mpprIeJeOptpt);
 }

OndselSolver/DistIeqcJec.cpp

@@ -41,7 +41,7 @@ void MbD::DistIeqcJec::calcPrivate()
 			pprIeJepXIipXI->atiput(j, element / rIeJe);
 		}
 	}
-	pprIeJepXIpEI = std::make_shared<FullMatrixDouble>(3, 4);
+	pprIeJepXIpEI = std::make_shared<FullMatrix<double>>(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<FullMatrixDouble>(4, 4);
+	pprIeJepEIpEI = std::make_shared<FullMatrix<double>>(4, 4);
 	for (int i = 0; i < 4; i++)
 	{
 		auto& pprIeJepEIipEI = pprIeJepEIpEI->at(i);
@@ -63,7 +63,7 @@ void MbD::DistIeqcJec::calcPrivate()
 		for (int j = 0; j < 4; j++)
 		{
 			auto element = mprIeJeOpEIiT->dot(mprIeJeOpEIT->at(j))
-                           - mpprIeJeOpEIipEI->at(j)->dotVec(rIeJeO) - prIeJepEIi * prIeJepEI->at(j);
+                           - mpprIeJeOpEIipEI->at(j)->dot(rIeJeO) - prIeJepEIi * prIeJepEI->at(j);
 			pprIeJepEIipEI->atiput(j, element / rIeJe);
 		}
 	}
@@ -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<FullMatrixDouble>(3, 3);
-	pprIeJepXIpEI = std::make_shared<FullMatrixDouble>(3, 4);
-	pprIeJepEIpEI = std::make_shared<FullMatrixDouble>(4, 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);
 }
 
 FMatDsptr MbD::DistIeqcJec::ppvaluepEIpEI()

OndselSolver/DistIeqcJeqc.cpp

@@ -111,7 +111,7 @@ void MbD::DistIeqcJeqc::calcPrivate()
 		for (int j = 0; j < 4; j++)
 		{
 			auto element = prIeJeOpEJiT->dot(prIeJeOpEJT->at(j))
-                           + pprIeJeOpEJipEJ->at(j)->dotVec(rIeJeO) - prIeJepEJi * prIeJepEJ->at(j);
+                           + pprIeJeOpEJipEJ->at(j)->dot(rIeJeO) - prIeJepEJi * prIeJepEJ->at(j);
 			pprIeJepEJipEJ->atiput(j, element / rIeJe);
 		}
 	}
@@ -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<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);
+	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);
 }
 
 FMatDsptr MbD::DistIeqcJeqc::ppvaluepEIpEJ()

OndselSolver/DistxyIeqcJec.cpp

@@ -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<FullMatrixDouble>(3, 3);
-	ppdistxypXIpEI = std::make_shared<FullMatrixDouble>(3, 4);
-	ppdistxypEIpEI = std::make_shared<FullMatrixDouble>(4, 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);
 }
 
 FMatDsptr MbD::DistxyIeqcJec::ppvaluepEIpEI()

OndselSolver/DistxyIeqcJeqc.cpp

@@ -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<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);
+	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);
 }
 
 FMatDsptr MbD::DistxyIeqcJeqc::ppvaluepEIpEJ()

OndselSolver/EndFramec.h

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

OndselSolver/EndFrameqc.cpp

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

OndselSolver/EndFrameqct.cpp

@@ -9,6 +9,7 @@
 #include "EndFrameqct.h"
 #include "MarkerFrame.h"
 #include "System.h"
+#include "Symbolic.h"
 #include "Time.h"
 #include "EulerParameters.h"
 #include "CREATE.h"
@@ -16,312 +17,352 @@
 #include "EulerAngleszxzDot.h"
 #include "EulerAngleszxzDDot.h"
 
-namespace MbD {
-    template class EulerParameters<double>;
+using namespace MbD;
 
-    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);
-    }
-    bool EndFrameqct::isEndFrameqc()
-    {
-        return false;
-    }
+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 = 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);
+}
+
+bool MbD::EndFrameqct::isEndFrameqc()
+{
+	return false;
 }

OndselSolver/EndFrameqct2.cpp

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

OndselSolver/EulerAngles.cpp

@@ -7,76 +7,3 @@
  ***************************************************************************/
  
 #include "EulerAngles.h"
-
-namespace MbD {
-    template<typename T>
-    inline void EulerAngles<T>::initialize()
-    {
-        rotOrder = std::make_shared<FullColumn<int>>(3);
-        rotOrder->at(0) = 0;
-        rotOrder->at(1) = 1;
-        rotOrder->at(2) = 2;
-    }
-    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);
-    }
-    // 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;
-    }
-}
-

OndselSolver/EulerAngles.h

@@ -16,6 +16,8 @@
 #include "Symbolic.h"
 
 namespace MbD {
+	//template<typename T>
+	//class EulerAnglesDot;
 
 	template<typename T>
 	class EulerAngles : public EulerArray<T>
@@ -27,23 +29,87 @@ 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 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);
-    template<typename T>
 
-    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<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;
+	}
 }
 

OndselSolver/EulerAnglesDDot.h

@@ -47,13 +47,13 @@ namespace MbD {
 			auto angleDot = aEulerAnglesDot->at(i)->getValue();
 			auto angleDDot = this->at(i)->getValue();
 			if (axis == 1) {
-				cAddot->atiput(i, FullMatrixDouble::rotatexrotDotrotDDot(angle, angleDot, angleDDot));
+				cAddot->atiput(i, FullMatrix<double>::rotatexrotDotrotDDot(angle, angleDot, angleDDot));
 			}
 			else if (axis == 2) {
-				cAddot->atiput(i, FullMatrixDouble::rotateyrotDotrotDDot(angle, angleDot, angleDDot));
+				cAddot->atiput(i, FullMatrix<double>::rotateyrotDotrotDDot(angle, angleDot, angleDDot));
 			}
 			else if (axis == 3) {
-				cAddot->atiput(i, FullMatrixDouble::rotatezrotDotrotDDot(angle, angleDot, angleDDot));
+				cAddot->atiput(i, FullMatrix<double>::rotatezrotDotrotDDot(angle, angleDot, angleDDot));
 			}
 			else {
 				throw std::runtime_error("Euler angle rotation order must be any permutation of 1,2,3 without consecutive repeats.");

OndselSolver/EulerAnglesDot.h

@@ -56,13 +56,13 @@ namespace MbD {
 			auto angle = aEulerAngles->at(i)->getValue();
 			auto angleDot = this->at(i)->getValue();
 			if (axis == 1) {
-				cAdot->atiput(i, FullMatrixDouble::rotatexrotDot(angle, angleDot));
+				cAdot->atiput(i, FullMatrix<double>::rotatexrotDot(angle, angleDot));
 			}
 			else if (axis == 2) {
-				cAdot->atiput(i, FullMatrixDouble::rotateyrotDot(angle, angleDot));
+				cAdot->atiput(i, FullMatrix<double>::rotateyrotDot(angle, angleDot));
 			}
 			else if (axis == 3) {
-				cAdot->atiput(i, FullMatrixDouble::rotatezrotDot(angle, angleDot));
+				cAdot->atiput(i, FullMatrix<double>::rotatezrotDot(angle, angleDot));
 			}
 			else {
 				throw std::runtime_error("Euler angle rotation order must be any permutation of 1,2,3 without consecutive repeats.");

OndselSolver/EulerAngleszxz.h

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

OndselSolver/EulerAngleszxzDDot.h

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

OndselSolver/EulerAngleszxzDot.h

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

OndselSolver/EulerParameters.cpp

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

OndselSolver/EulerParameters.h

@@ -8,12 +8,9 @@
  
 #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 {
 
@@ -42,10 +39,11 @@ namespace MbD {
 			this->initialize();
 			this->calc();
 		}
-		static std::shared_ptr<FullMatrixFullColumnDouble> ppApEpEtimesColumn(FColDsptr col);
+
+		static std::shared_ptr<FullMatrix<FColsptr<T>>> ppApEpEtimesColumn(FColDsptr col);
 		static FMatDsptr pCpEtimesColumn(FColDsptr col);
 		static FMatDsptr pCTpEtimesColumn(FColDsptr col);
-		static std::shared_ptr<FullMatrixFullMatrixDouble> ppApEpEtimesMatrix(FMatDsptr mat);
+		static std::shared_ptr<FullMatrix<FMatsptr<T>>> ppApEpEtimesMatrix(FMatDsptr mat);
 
 
 		void initialize() override;
@@ -59,4 +57,295 @@ namespace MbD {
 		FMatDsptr aC;
 		FColFMatDsptr pApE;
 	};
+
+	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++)
+		{
+			pApE->at(i) = std::make_shared<FullMatrix<double>>(3, 3);
+		}
+	}
+	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();
+	}
 }
+

OndselSolver/EulerParameters.ref.h

@@ -1,6 +0,0 @@
-#pragma once
-
-namespace MbD {
-    template<typename T>
-    class EulerParameters;
-}

OndselSolver/EulerParametersDot.h

@@ -55,13 +55,13 @@ namespace MbD {
 	template<typename T>
 	inline void EulerParametersDot<T>::initialize()
 	{
-		aAdot = std::make_shared<FullMatrixDouble>(3, 3);
-		aBdot = std::make_shared<FullMatrixDouble>(3, 4);
-		aCdot = std::make_shared<FullMatrixDouble>(3, 4);
+		aAdot = std::make_shared<FullMatrix<double>>(3, 3);
+		aBdot = std::make_shared<FullMatrix<double>>(3, 4);
+		aCdot = std::make_shared<FullMatrix<double>>(3, 4);
 		pAdotpE = std::make_shared<FullColumn<FMatDsptr>>(4);
 		for (int i = 0; i < 4; i++)
 		{
-			pAdotpE->at(i) = std::make_shared<FullMatrixDouble>(3, 3);
+			pAdotpE->at(i) = std::make_shared<FullMatrix<double>>(3, 3);
 		}
 	}
 

OndselSolver/FullColumn.cpp

@@ -9,196 +9,5 @@
 #include <sstream>
 
 #include "FullColumn.h"
-#include "FullRow.h"
-#include "FullMatrix.h"
 
-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>::plusFullColumntimes(FColsptr<T> fullCol, double factor)
-    {
-        assert(false);
-        return FColsptr<T>();
-    }
-    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((T) - 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->dotVec(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 = (int)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>;
-    template<typename T>
-    double FullColumn<T>::dot(std::shared_ptr<FullVector<T>> vec)
-    {
-        int n = (int)this->size();
-        double answer = 0.0;
-        for (int i = 0; i < n; i++) {
-            answer += this->at(i) * vec->at(i);
-        }
-        return answer;
-    }
-    template<typename T>
-    std::shared_ptr<FullVector<T>> FullColumn<T>::dot(std::shared_ptr<std::vector<std::shared_ptr<FullColumn<T>>>> vecvec)
-    {
-        int ncol = (int)this->size();
-        auto nelem = vecvec->at(0)->size();
-        auto answer = std::make_shared<FullVector<T>>(nelem);
-        for (int k = 0; k < nelem; k++) {
-            auto sum = (T)0;
-            for (int i = 0; i < ncol; i++)
-            {
-                sum += this->at(i) * vecvec->at(i)->at(k);
-            }
-            answer->at(k) = sum;
-        }
-        return answer;
-    }
-}
+using namespace MbD;

OndselSolver/FullColumn.h

@@ -5,19 +5,24 @@
  *                                                                         *
  *   See LICENSE file for details about copyright.                         *
  ***************************************************************************/
-
+ 
 #pragma once
 
 #include <string>
 #include <sstream> 
 
 #include "FullVector.h"
-#include "FullColumn.ref.h"
-#include "FullRow.ref.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>
@@ -31,7 +36,6 @@ namespace MbD {
 		FullColumn(typename std::vector<T>::iterator begin, typename std::vector<T>::iterator end) : FullVector<T>(begin, end) {}
 		FullColumn(std::initializer_list<T> list) : FullVector<T>{ list } {}
 		FColsptr<T> plusFullColumn(FColsptr<T> fullCol);
-		FColsptr<T> plusFullColumntimes(FColsptr<T> fullCol, double factor);
 		FColsptr<T> minusFullColumn(FColsptr<T> fullCol);
 		FColsptr<T> times(T a);
 		FColsptr<T> negated();
@@ -43,44 +47,203 @@ namespace MbD {
 		FColsptr<T> copy();
 		FRowsptr<T> transpose();
 		void atiplusFullColumntimes(int i, FColsptr<T> fullCol, T factor);
-		T transposeTimesFullColumn(const FColsptr<T> fullCol);
+		T transposeTimesFullColumn(const FColsptr<T> fullCol);		
 		void equalSelfPlusFullColumntimes(FColsptr<T> fullCol, T factor);
 		FColsptr<T> cross(FColsptr<T> fullCol);
 		FColsptr<T> simplified();
 		double dot(std::shared_ptr<FullVector<T>> vec);
-		std::shared_ptr<FullColumn<T>> cloneFcSptr();
-		double dotVec(std::shared_ptr<FullVector<T>> vec);
 		std::shared_ptr<FullVector<T>> dot(std::shared_ptr<std::vector<std::shared_ptr<FullColumn<T>>>> vecvec);
 
 		std::ostream& printOn(std::ostream& s) const override;
 	};
-    // 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 < this->size(); i++)
-        {
-            s << ", " << this->at(i);
-        }
-        s << "}";
-        return s;
-    }
-    template<typename T>
-    std::shared_ptr<FullColumn<T>> FullColumn<T>::cloneFcSptr()
-    {
-        return std::make_shared<FullColumn<T>>(*this);
-    }
-    template<typename T>
-    double FullColumn<T>::dotVec(std::shared_ptr<FullVector<T>> vec)
-    {
-        int n = (int)this->size();
-        double answer = 0.0;
-        for (int i = 0; i < n; i++) {
-            answer += this->at(i) * vec->at(i);
-        }
-        return answer;
-    }
-
+	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 double FullColumn<T>::dot(std::shared_ptr<FullVector<T>> vec)
+	{
+		int n = (int)this->size();
+		double answer = 0.0;
+		for (int i = 0; i < n; i++) {
+			answer += this->at(i) * vec->at(i);
+		}
+		return answer;
+	}
+	template<typename T>
+	inline std::shared_ptr<FullVector<T>> FullColumn<T>::dot(std::shared_ptr<std::vector<std::shared_ptr<FullColumn<T>>>> vecvec)
+	{
+		int ncol = (int)this->size();
+		auto nelem = vecvec->at(0)->size();
+		auto answer = std::make_shared<FullVector<T>>(nelem);
+		for (int k = 0; k < nelem; k++) {
+			auto sum = 0.0;
+			for (int i = 0; i < ncol; i++)
+			{
+				sum += this->at(i) * vecvec->at(i)->at(k);
+			}
+			answer->at(k) = sum;
+		}
+		return answer;
+	}
+	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;
+	}
 }
+

OndselSolver/FullColumn.ref.h

@@ -1,14 +0,0 @@
-#pragma once
-
-#include <algorithm>
-#include <memory>
-
-namespace MbD {
-    template<typename T>
-    class FullColumn;
-
-    using FColDsptr = std::shared_ptr<FullColumn<double>>;
-
-    template<typename T>
-    using FColsptr = std::shared_ptr<FullColumn<T>>;
-}

OndselSolver/FullMatrix.cpp

@@ -7,678 +7,6 @@
  ***************************************************************************/
 
 #include "FullMatrix.h"
-#include "FullColumn.h"
-#include "FullRow.h"
-#include "DiagonalMatrix.h"
-#include "EulerParameters.h"
 
-namespace MbD {
-    FColsptr<double> FullMatrixDouble::timesFullColumn(FColsptr<double> fullCol)
-    {
-        return this->timesFullColumn(fullCol.get());
-//        auto nrow = this->nrow();
-//        auto answer = std::make_shared<FullColumn<double>>(nrow);
-//        for (int i = 0; i < nrow; i++)
-//        {
-//            answer->at(i) = this->at(i)->timesFullColumn(fullCol);
-//        }
-//        return answer;
-    }
-    FColsptr<double> FullMatrixDouble::timesFullColumn(FullColumn<double>* fullCol) // local
-    {
-        //"a*b = a(i,j)b(j) sum j."
-        auto nrow = this->nrow();
-        auto answer = std::make_shared<FullColumn<double>>(nrow);
-        for (int i = 0; i < nrow; i++)
-        {
-            answer->at(i) = this->at(i)->timesFullColumn(fullCol);
-        }
-        return answer;
-    }
-    std::shared_ptr<FullMatrixDouble> FullMatrixDouble::rotatex(double the)
-    {
-        auto sthe = std::sin(the);
-        auto cthe = std::cos(the);
-        auto rotMat = std::make_shared<FullMatrixDouble>(3, 3);
-        auto row0 = rotMat->at(0);
-        row0->atiput(0, 1.0);
-        row0->atiput(1, 0.0);
-        row0->atiput(2, 0.0);
-        auto row1 = rotMat->at(1);
-        row1->atiput(0, 0.0);
-        row1->atiput(1, cthe);
-        row1->atiput(2, -sthe);
-        auto row2 = rotMat->at(2);
-        row2->atiput(0, 0.0);
-        row2->atiput(1, sthe);
-        row2->atiput(2, cthe);
-        return rotMat;
-    }
-    std::shared_ptr<FullMatrixDouble> FullMatrixDouble::rotatey(double the)
-    {
-        auto sthe = std::sin(the);
-        auto cthe = std::cos(the);
-        auto rotMat = std::make_shared<FullMatrixDouble>(3, 3);
-        auto row0 = rotMat->at(0);
-        row0->atiput(0, cthe);
-        row0->atiput(1, 0.0);
-        row0->atiput(2, sthe);
-        auto row1 = rotMat->at(1);
-        row1->atiput(0, 0.0);
-        row1->atiput(1, 1.0);
-        row1->atiput(2, 0.0);
-        auto row2 = rotMat->at(2);
-        row2->atiput(0, -sthe);
-        row2->atiput(1, 0.0);
-        row2->atiput(2, cthe);
-        return rotMat;
-    }
-    std::shared_ptr<FullMatrixDouble> FullMatrixDouble::rotatez(double the)
-    {
-        auto sthe = std::sin(the);
-        auto cthe = std::cos(the);
-        auto rotMat = std::make_shared<FullMatrixDouble>(3, 3);
-        auto row0 = rotMat->at(0);
-        row0->atiput(0, cthe);
-        row0->atiput(1, -sthe);
-        row0->atiput(2, 0.0);
-        auto row1 = rotMat->at(1);
-        row1->atiput(0, sthe);
-        row1->atiput(1, cthe);
-        row1->atiput(2, 0.0);
-        auto row2 = rotMat->at(2);
-        row2->atiput(0, 0.0);
-        row2->atiput(1, 0.0);
-        row2->atiput(2, 1.0);
-        return rotMat;
-    }
-    std::shared_ptr<FullMatrixDouble> FullMatrixDouble::rotatexrotDot(double the, double thedot)
-    {
-        auto sthe = std::sin(the);
-        auto cthe = std::cos(the);
-        auto sthedot = cthe * thedot;
-        auto cthedot = -sthe * thedot;
-        auto rotMat = std::make_shared<FullMatrixDouble>(3, 3);
-        auto row0 = rotMat->at(0);
-        row0->atiput(0, 0.0);
-        row0->atiput(1, 0.0);
-        row0->atiput(2, 0.0);
-        auto row1 = rotMat->at(1);
-        row1->atiput(0, 0.0);
-        row1->atiput(1, cthedot);
-        row1->atiput(2, -sthedot);
-        auto row2 = rotMat->at(2);
-        row2->atiput(0, 0.0);
-        row2->atiput(1, sthedot);
-        row2->atiput(2, cthedot);
-        return rotMat;
-    }
-    std::shared_ptr<FullMatrixDouble> FullMatrixDouble::rotateyrotDot(double the, double thedot)
-    {
-        auto sthe = std::sin(the);
-        auto cthe = std::cos(the);
-        auto sthedot = cthe * thedot;
-        auto cthedot = -sthe * thedot;
-        auto rotMat = std::make_shared<FullMatrixDouble>(3, 3);
-        auto row0 = rotMat->at(0);
-        row0->atiput(0, cthedot);
-        row0->atiput(1, 0.0);
-        row0->atiput(2, sthedot);
-        auto row1 = rotMat->at(1);
-        row1->atiput(0, 0.0);
-        row1->atiput(1, 0.0);
-        row1->atiput(2, 0.0);
-        auto row2 = rotMat->at(2);
-        row2->atiput(0, -sthedot);
-        row2->atiput(1, 0.0);
-        row2->atiput(2, cthedot);
-        return rotMat;
-    }
-    std::shared_ptr<FullMatrixDouble> FullMatrixDouble::rotatezrotDot(double the, double thedot)
-    {
-        auto sthe = std::sin(the);
-        auto cthe = std::cos(the);
-        auto sthedot = cthe * thedot;
-        auto cthedot = -sthe * thedot;
-        auto rotMat = std::make_shared<FullMatrixDouble>(3, 3);
-        auto row0 = rotMat->at(0);
-        row0->atiput(0, cthedot);
-        row0->atiput(1, -sthedot);
-        row0->atiput(2, 0.0);
-        auto row1 = rotMat->at(1);
-        row1->atiput(0, sthedot);
-        row1->atiput(1, cthedot);
-        row1->atiput(2, 0.0);
-        auto row2 = rotMat->at(2);
-        row2->atiput(0, 0.0);
-        row2->atiput(1, 0.0);
-        row2->atiput(2, 0.0);
-        return rotMat;
-    }
-    std::shared_ptr<FullMatrixDouble> FullMatrixDouble::rotatexrotDotrotDDot(double the, double thedot, double theddot)
-    {
-        auto sthe = std::sin(the);
-        auto cthe = std::cos(the);
-        auto sthedot = cthe * thedot;
-        auto cthedot = -sthe * thedot;
-        auto stheddot = cthedot * thedot + (cthe * theddot);
-        auto ctheddot = -(sthedot * thedot) - (sthe * theddot);
-        auto rotMat = std::make_shared<FullMatrixDouble>(3, 3);
-        auto row0 = rotMat->at(0);
-        row0->atiput(0, 0.0);
-        row0->atiput(1, 0.0);
-        row0->atiput(2, 0.0);
-        auto row1 = rotMat->at(1);
-        row1->atiput(0, 0.0);
-        row1->atiput(1, ctheddot);
-        row1->atiput(2, -stheddot);
-        auto row2 = rotMat->at(2);
-        row2->atiput(0, 0.0);
-        row2->atiput(1, stheddot);
-        row2->atiput(2, ctheddot);
-        return rotMat;
-    }
-    std::shared_ptr<FullMatrixDouble> FullMatrixDouble::rotateyrotDotrotDDot(double the, double thedot, double theddot)
-    {
-        auto sthe = std::sin(the);
-        auto cthe = std::cos(the);
-        auto sthedot = cthe * thedot;
-        auto cthedot = -sthe * thedot;
-        auto stheddot = cthedot * thedot + (cthe * theddot);
-        auto ctheddot = -(sthedot * thedot) - (sthe * theddot);
-        auto rotMat = std::make_shared<FullMatrixDouble>(3, 3);
-        auto row0 = rotMat->at(0);
-        row0->atiput(0, ctheddot);
-        row0->atiput(1, 0.0);
-        row0->atiput(2, stheddot);
-        auto row1 = rotMat->at(1);
-        row1->atiput(0, 0.0);
-        row1->atiput(1, 0.0);
-        row1->atiput(2, 0.0);
-        auto row2 = rotMat->at(2);
-        row2->atiput(0, -stheddot);
-        row2->atiput(1, 0.0);
-        row2->atiput(2, ctheddot);
-        return rotMat;
-    }
-    std::shared_ptr<FullMatrixDouble> FullMatrixDouble::rotatezrotDotrotDDot(double the, double thedot, double theddot)
-    {
-        auto sthe = std::sin(the);
-        auto cthe = std::cos(the);
-        auto sthedot = cthe * thedot;
-        auto cthedot = -sthe * thedot;
-        auto stheddot = cthedot * thedot + (cthe * theddot);
-        auto ctheddot = -(sthedot * thedot) - (sthe * theddot);
-        auto rotMat = std::make_shared<FullMatrixDouble>(3, 3);
-        auto row0 = rotMat->at(0);
-        row0->atiput(0, ctheddot);
-        row0->atiput(1, -stheddot);
-        row0->atiput(2, 0.0);
-        auto row1 = rotMat->at(1);
-        row1->atiput(0, stheddot);
-        row1->atiput(1, ctheddot);
-        row1->atiput(2, 0.0);
-        auto row2 = rotMat->at(2);
-        row2->atiput(0, 0.0);
-        row2->atiput(1, 0.0);
-        row2->atiput(2, 0.0);
-        return rotMat;
-    }
-    std::shared_ptr<FullMatrixDouble> FullMatrixDouble::identitysptr(int n)
-    {
-        auto mat = std::make_shared<FullMatrixDouble>(n, n);
-        mat->identity();
-        return mat;
-    }
-    std::shared_ptr<MbD::FullMatrixFullMatrixDouble> FullMatrixFullMatrixDouble::identitysptr(int n)
-    {
-        auto mat = std::make_shared<FullMatrixFullMatrixDouble>(n, n);
-        mat->identity();
-        return mat;
-    }
-    std::shared_ptr<FullMatrixDouble> FullMatrixDouble::tildeMatrix(FColDsptr col)
-    {
-        //"tildeMatrix is skew symmetric matrix related to angular velocity and cross product."
-        if (col->size() != 3) throw std::runtime_error("Column is not of dimension 3");
-        auto tilde = std::make_shared<FullMatrixDouble>(3, 3);
-        auto c0 = col->at(0);
-        auto c1 = col->at(1);
-        auto c2 = col->at(2);
-        tilde->atijput(0, 0, 0.0);
-        tilde->atijput(1, 1, 0.0);
-        tilde->atijput(2, 2, 0.0);
-        tilde->atijput(1, 2, -c0);
-        tilde->atijput(0, 2, c1);
-        tilde->atijput(0, 1, -c2);
-        tilde->atijput(1, 0, c2);
-        tilde->atijput(2, 0, -c1);
-        tilde->atijput(2, 1, c0);
-        return tilde;
-    }
-    void FullMatrixDouble::zeroSelf()
-    {
-        for (int i = 0; i < this->size(); i++) {
-            this->at(i)->zeroSelf();
-        }
-    }
-    void FullMatrixFullMatrixDouble::zeroSelf()
-    {
-        for (int i = 0; i < this->size(); i++) {
-            this->at(i)->zeroSelf();
-        }
-    }
-    void FullMatrixFullColumnDouble::zeroSelf()
-    {
-        for (int i = 0; i < this->size(); i++) {
-            this->at(i)->zeroSelf();
-        }
-    }
-    void FullMatrixDouble::identity() {
-        this->zeroSelf();
-        for (int i = 0; i < this->size(); i++) {
-            this->at(i)->at(i) = 1.0;
-        }
-    }
-    void FullMatrixFullMatrixDouble::identity() {
-        assert(false);
-//    this->zeroSelf();
-//    for (int i = 0; i < this->size(); i++) {
-//        this->at(i)->at(i) = 1.0;
-//    }
-    }
-    // TODO: should there be a FullMatrixFullColumnDouble version of this?
-    FColsptr<double> FullMatrixDouble::column(int j) {
-        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)->at(j);
-        }
-        return answer;
-    }
-    std::shared_ptr<FullMatrixDouble> FullMatrixDouble::timesFullMatrix(std::shared_ptr<FullMatrixDouble> fullMat)
-    {
-        int m = this->nrow();
-        auto answer = std::make_shared<FullMatrixDouble>(m);
-        for (int i = 0; i < m; i++) {
-            answer->at(i) = this->at(i)->timesFullMatrix(fullMat);
-        }
-        return answer;
-    }
-    std::shared_ptr<FullMatrixDouble> FullMatrixDouble::timesTransposeFullMatrix(std::shared_ptr<FullMatrixDouble> fullMat)
-    {
-        int nrow = this->nrow();
-        auto answer = std::make_shared<FullMatrixDouble>(nrow);
-        for (int i = 0; i < nrow; i++) {
-            answer->at(i) = this->at(i)->timesTransposeFullMatrix(fullMat);
-        }
-        return answer;
-    }
-//    std::shared_ptr<FullMatrixFullMatrixDouble> FullMatrixFullMatrixDouble::timesTransposeFullMatrix(std::shared_ptr<FullMatrixFullMatrixDouble> fullMat)
-//    {
-//        int nrow = this->nrow();
-//        auto answer = std::make_shared<FullMatrixFullMatrixDouble>(nrow);
-//        for (int i = 0; i < nrow; i++) {
-//            answer->at(i) = this->at(i)->timesTransposeFullMatrixForFMFMDsptr(fullMat);
-//        }
-//        return answer;
-//    }
-    std::shared_ptr<FullMatrixDouble> FullMatrixDouble::times(double a)
-    {
-        int m = this->nrow();
-        auto answer = std::make_shared<FullMatrixDouble>(m);
-        for (int i = 0; i < m; i++) {
-            // auto x = this->at(i);
-            answer->at(i) = this->at(i)->times(a);
-        }
-        return answer;
-    }
-    std::shared_ptr<FullMatrixFullMatrixDouble> FullMatrixFullMatrixDouble::times(double a)
-    {
-        // TODO: correct action?
-        assert(false);
-        return std::make_shared<FullMatrixFullMatrixDouble>();
-    }
-    std::shared_ptr<FullMatrixFullColumnDouble> FullMatrixFullColumnDouble::times(double a)
-    {
-        // TODO: correct action?
-        assert(false);
-        return std::make_shared<FullMatrixFullColumnDouble>();
-    }
-    std::shared_ptr<FullMatrixDouble> FullMatrixDouble::transposeTimesFullMatrix(std::shared_ptr<FullMatrixDouble> fullMat)
-    {
-        return this->transpose()->timesFullMatrix(fullMat);
-    }
-    std::shared_ptr<FullMatrixDouble> FullMatrixDouble::plusFullMatrix(std::shared_ptr<FullMatrixDouble> fullMat)
-    {
-        int n = (int)this->size();
-        auto answer = std::make_shared<FullMatrixDouble>(n);
-        for (int i = 0; i < n; i++) {
-            answer->at(i) = this->at(i)->plusFullRow(fullMat->at(i));
-        }
-        return answer;
-    }
-    std::shared_ptr<FullMatrixDouble> FullMatrixDouble::minusFullMatrix(std::shared_ptr<FullMatrixDouble> fullMat)
-    {
-        int n = (int)this->size();
-        auto answer = std::make_shared<FullMatrixDouble>(n);
-        for (int i = 0; i < n; i++) {
-            answer->at(i) = this->at(i)->minusFullRow(fullMat->at(i));
-        }
-        return answer;
-    }
-    std::shared_ptr<FullMatrixDouble> FullMatrixDouble::transpose() {
-        int nrow = this->nrow();
-        auto ncol = this->ncol();
-        auto answer = std::make_shared<FullMatrixDouble>(ncol, nrow);
-        for (int i = 0; i < nrow; i++) {
-            auto& row = this->at(i);
-            for (int j = 0; j < ncol; j++) {
-                answer->at(j)->at(i) = row->at(j);
-            }
-        }
-        return answer;
-    }
-    std::shared_ptr<FullMatrixDouble> FullMatrixDouble::negated()
-    {
-        return this->times(-1.0);
-    }
-    void FullMatrixDouble::symLowerWithUpper()
-    {
-        int n = (int)this->size();
-        for (int i = 0; i < n; i++) {
-            for (int j = i + 1; j < n; j++) {
-                this->at(j)->at(i) = this->at(i)->at(j);
-            }
-        }
-    }
-    void FullMatrixFullColumnDouble::symLowerWithUpper()
-    {
-        int n = (int)this->size();
-        for (int i = 0; i < n; i++) {
-            for (int j = i + 1; j < n; j++) {
-                this->at(j)->at(i) = this->at(i)->at(j);
-            }
-        }
-    }
-    void FullMatrixDouble::atiput(int i, FRowsptr<double> fullRow)
-    {
-        this->at(i) = fullRow;
-    }
-    void FullMatrixDouble::atijput(int i, int j, double value)
-    {
-        this->at(i)->atiput(j, value);
-    }
-    void FullMatrixDouble::atijputFullColumn(int i1, int j1, FColsptr<double> fullCol)
-    {
-        for (int ii = 0; ii < fullCol->size(); ii++)
-        {
-            this->at(i1 + ii)->at(j1) = fullCol->at(ii);
-        }
-    }
-    void FullMatrixDouble::atijplusFullRow(int i, int j, FRowsptr<double> fullRow)
-    {
-        this->at(i)->atiplusFullRow(j, fullRow);
-    }
-    void FullMatrixDouble::atijplusNumber(int i, int j, double value)
-    {
-        auto rowi = this->at(i);
-        rowi->at(j) += value;
-    }
-    void FullMatrixDouble::atijminusNumber(int i, int j, double value)
-    {
-        auto rowi = this->at(i);
-        rowi->at(j) -= value;
-    }
-    double FullMatrixDouble::sumOfSquares()
-    {
-        double sum = 0.0;
-        for (int i = 0; i < this->size(); i++)
-        {
-            sum += this->at(i)->sumOfSquares();
-        }
-        return sum;
-    }
-    double FullMatrixFullMatrixDouble::sumOfSquares()
-    {
-        double sum = 0.0;
-        for (int i = 0; i < this->size(); i++)
-        {
-            sum += this->at(i)->sumOfSquares();
-        }
-        return sum;
-    }
-    double FullMatrixFullColumnDouble::sumOfSquares()
-    {
-        double sum = 0.0;
-        for (int i = 0; i < this->size(); i++)
-        {
-            sum += this->at(i)->sumOfSquares();
-        }
-        return sum;
-    }
-    std::shared_ptr<FullMatrixDouble> FullMatrixDouble::copy()
-    {
-        auto m = (int)this->size();
-        auto answer = std::make_shared<FullMatrixDouble>(m);
-        for (int i = 0; i < m; i++)
-        {
-            answer->at(i) = this->at(i)->copy();
-        }
-        return answer;
-    }
-    FullMatrixDouble FullMatrixDouble::operator+(const FullMatrixDouble fullMat)
-    {
-        int n = (int)this->size();
-        FullMatrixDouble answer(n);
-        for (int i = 0; i < n; i++) {
-            answer.at(i) = this->at(i)->plusFullRow(fullMat.at(i));
-        }
-        return answer;
-    }
-    FColsptr<double> FullMatrixDouble::transposeTimesFullColumn(FColsptr<double> fullCol)
-    {
-        auto sptr = std::make_shared<FullMatrixDouble>(*this);
-        return fullCol->transpose()->timesFullMatrix(sptr)->transpose();
-    }
-    void FullMatrixDouble::magnifySelf(double factor)
-    {
-        for (int i = 0; i < this->size(); i++) {
-            this->at(i)->magnifySelf(factor);
-        }
-    }
-    std::ostream& FullMatrixDouble::printOn(std::ostream& s) const
-    {
-        s << "FullMat[" << std::endl;
-        for (int i = 0; i < this->size(); i++)
-        {
-            s << *(this->at(i)) << std::endl;
-        }
-        s << "]";
-        return s;
-    }
-    std::shared_ptr<EulerParameters<double>> FullMatrixDouble::asEulerParameters()
-    {
-        //"Given [A], compute Euler parameter."
+using namespace MbD;
 
-        auto traceA = this->trace();
-        double dum = 0.0;
-        double dumSq = 0.0;
-        //auto qE = CREATE<EulerParameters<double>>::With(4); //Cannot use CREATE.h in subclasses of std::vector. Why?
-        auto qE = std::make_shared<EulerParameters<double>>(4);
-        qE->initialize();
-        auto OneMinusTraceDivFour = (1.0 - traceA) / 4.0;
-        for (int i = 0; i < 3; i++)
-        {
-            dumSq = this->at(i)->at(i) / 2.0 + OneMinusTraceDivFour;
-            dum = (dumSq > 0.0) ? std::sqrt(dumSq) : 0.0;
-            qE->atiput(i, dum);
-        }
-        dumSq = (1.0 + traceA) / 4.0;
-        dum = (dumSq > 0.0) ? std::sqrt(dumSq) : 0.0;
-        qE->atiput(3, dum);
-        double max = 0.0;
-        int maxE = -1;
-        for (int i = 0; i < 4; i++)
-        {
-            auto num = qE->at(i);
-            if (max < num) {
-                max = num;
-                maxE = i;
-            }
-        }
-
-        if (maxE == 0) {
-            auto FourE = 4.0 * qE->at(0);
-            qE->atiput(1, (this->at(0)->at(1) + this->at(1)->at(0)) / FourE);
-            qE->atiput(2, (this->at(0)->at(2) + this->at(2)->at(0)) / FourE);
-            qE->atiput(3, (this->at(2)->at(1) - this->at(1)->at(2)) / FourE);
-        }
-        else if (maxE == 1) {
-            auto FourE = 4.0 * qE->at(1);
-            qE->atiput(0, (this->at(0)->at(1) + this->at(1)->at(0)) / FourE);
-            qE->atiput(2, (this->at(1)->at(2) + this->at(2)->at(1)) / FourE);
-            qE->atiput(3, (this->at(0)->at(2) - this->at(2)->at(0)) / FourE);
-        }
-        else if (maxE == 2) {
-            auto FourE = 4.0 * qE->at(2);
-            qE->atiput(0, (this->at(0)->at(2) + this->at(2)->at(0)) / FourE);
-            qE->atiput(1, (this->at(1)->at(2) + this->at(2)->at(1)) / FourE);
-            qE->atiput(3, (this->at(1)->at(0) - this->at(0)->at(1)) / FourE);
-        }
-        else if (maxE == 3) {
-            auto FourE = 4.0 * qE->at(3);
-            qE->atiput(0, (this->at(2)->at(1) - this->at(1)->at(2)) / FourE);
-            qE->atiput(1, (this->at(0)->at(2) - this->at(2)->at(0)) / FourE);
-            qE->atiput(2, (this->at(1)->at(0) - this->at(0)->at(1)) / FourE);
-        }
-        qE->conditionSelf();
-        qE->calc();
-        return qE;
-    }
-    double FullMatrixDouble::trace()
-    {
-        double trace = 0.0;
-        for (int i = 0; i < this->size(); i++)
-        {
-            trace += this->at(i)->at(i);
-        }
-        return trace;
-    }
-    double FullMatrixDouble::maxMagnitude()
-    {
-        double max = 0.0;
-        for (int i = 0; i < this->size(); i++)
-        {
-            double element = this->at(i)->maxMagnitude();
-            if (max < element) max = element;
-        }
-        return max;
-    }
-    double FullMatrixFullMatrixDouble::maxMagnitude()
-    {
-        double max = 0.0;
-        for (int i = 0; i < this->size(); i++)
-        {
-            double element = this->at(i)->maxMagnitude();
-            if (max < element) max = element;
-        }
-        return max;
-    }
-    double FullMatrixFullColumnDouble::maxMagnitude()
-    {
-        double max = 0.0;
-        for (int i = 0; i < this->size(); i++)
-        {
-            double element = this->at(i)->maxMagnitude();
-            if (max < element) max = element;
-        }
-        return max;
-    }
-    FColsptr<double> FullMatrixDouble::bryantAngles()
-    {
-        auto answer = std::make_shared<FullColumn<double>>(3);
-        auto sthe1y = this->at(0)->at(2);
-        double the0x, the1y, the2z, cthe0x, sthe0x, y, x;
-        if (std::abs(sthe1y) > 0.9999) {
-            if (sthe1y > 0.0) {
-                the0x = std::atan2(this->at(1)->at(0), this->at(1)->at(1));
-                the1y = OS_M_PI / 2.0;
-                the2z = 0.0;
-            }
-            else {
-                the0x = std::atan2(this->at(2)->at(1), this->at(2)->at(0));
-                the1y = OS_M_PI / -2.0;
-                the2z = 0.0;
-            }
-        }
-        else {
-            the0x = std::atan2(-this->at(1)->at(2), this->at(2)->at(2));
-            cthe0x = std::cos(the0x);
-            sthe0x = std::sin(the0x);
-            y = sthe1y;
-            if (std::abs(cthe0x) > std::abs(sthe0x)) {
-                x = this->at(2)->at(2) / cthe0x;
-            }
-            else {
-                x = this->at(1)->at(2) / -sthe0x;
-            }
-            the1y = std::atan2(y, x);
-            the2z = std::atan2(-this->at(0)->at(1), this->at(0)->at(0));
-        }
-        answer->atiput(0, the0x);
-        answer->atiput(1, the1y);
-        answer->atiput(2, the2z);
-        return answer;
-    }
-    bool FullMatrixDouble::isDiagonal()
-    {
-        auto m = this->nrow();
-        auto n = this->ncol();
-        if (m != n) return false;
-        for (int i = 0; i < m; i++)
-        {
-            auto rowi = this->at(i);
-            for (int j = 0; j < n; j++)
-            {
-                if (i != j && rowi->at(j) != 0) return false;
-            }
-        }
-        return true;
-    }
-    bool FullMatrixDouble::isDiagonalToWithin(double ratio)
-    {
-        double maxMag = this->maxMagnitude();
-        auto tol = ratio * maxMag;
-        auto nrow = this->nrow();
-        if (nrow == this->ncol()) {
-            for (int i = 0; i < 3; i++)
-            {
-                for (int j = i + 1; j < 3; j++)
-                {
-                    if (std::abs(this->at(i)->at(j)) > tol) return false;
-                    if (std::abs(this->at(j)->at(i)) > tol) return false;
-                }
-            }
-            return true;
-        }
-        else {
-            return false;
-        }
-    }
-    std::shared_ptr<DiagonalMatrix> FullMatrixDouble::asDiagonalMatrix()
-    {
-        int nrow = this->nrow();
-        auto diagMat = std::make_shared<DiagonalMatrix>(nrow);
-        for (int i = 0; i < nrow; i++)
-        {
-            diagMat->atiput(i, this->at(i)->at(i));
-        }
-        return diagMat;
-    }
-    void FullMatrixDouble::conditionSelfWithTol(double tol)
-    {
-        for (auto row : *this) {
-            row->conditionSelfWithTol(tol);
-        }
-    }
-}

OndselSolver/FullMatrix.h

@@ -11,139 +11,731 @@
 #include "corecrt_math_defines.h"
 #include <memory>
 
-#include "FullMatrix.ref.h"
-#include "FullColumn.ref.h"
-#include "FullRow.ref.h"
-#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"
+#include "FullColumn.h"
+#include "FullRow.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;
+	using FMatDsptr = std::shared_ptr<FullMatrix<double>>;
+	template<typename T>
+	using FMatsptr = std::shared_ptr<FullMatrix<T>>;
+	template<typename T>
+	class FullColumn;
+	using FColDsptr = std::shared_ptr<FullColumn<double>>;
+	template<typename T>
+	class FullRow;
+	template<typename T>
+	class EulerParameters;
+	template<typename T>
+	class DiagonalMatrix;
 
-        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);
+	using FMatFColDsptr = std::shared_ptr<FullMatrix<FColDsptr>>;
+	using FMatFMatDsptr = std::shared_ptr<FullMatrix<FMatDsptr>>;
+	using FColFMatDsptr = std::shared_ptr<FullColumn<FMatDsptr>>;
 
-        void atiput(int i, FRowsptr<double> fullRow);
-        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)
+	template<typename T>
+	class FullMatrix : public RowTypeMatrix<FRowsptr<T>>
+	{
+	public:
+		FullMatrix() {}
+		FullMatrix(int m) : RowTypeMatrix<FRowsptr<T>>(m)
 		{
 		}
-        FullMatrixFullMatrixDouble(int m, int n) {
-            for (int i = 0; i < m; i++) {
-                auto row = std::make_shared<FullRow<FMatDsptr>>(n);
-                this->push_back(row);
-            }
-        }
-
-        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)
-		{
+		FullMatrix(int m, int n) {
+			for (int i = 0; i < m; i++) {
+				auto row = std::make_shared<FullRow<T>>(n);
+				this->push_back(row);
+			}
 		}
-        FullMatrixFullColumnDouble(int m, int n) {
-            for (int i = 0; i < m; i++) {
-                auto row = std::make_shared<FullRow<FColDsptr>>(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);
+		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);
 
-        double maxMagnitude() override;
-        void zeroSelf() override;
-        double sumOfSquares() override;
-        void symLowerWithUpper();
-        std::shared_ptr<FullMatrixFullColumnDouble> times(double a);
-    };
+		std::ostream& printOn(std::ostream& s) const override;
+	};
+	template<typename T>
+	inline FMatsptr<T> FullMatrix<T>::rotatex(T the)
+	{
+		auto sthe = std::sin(the);
+		auto cthe = std::cos(the);
+		auto rotMat = std::make_shared<FullMatrix<T>>(3, 3);
+		auto row0 = rotMat->at(0);
+		row0->atiput(0, 1.0);
+		row0->atiput(1, 0.0);
+		row0->atiput(2, 0.0);
+		auto row1 = rotMat->at(1);
+		row1->atiput(0, 0.0);
+		row1->atiput(1, cthe);
+		row1->atiput(2, -sthe);
+		auto row2 = rotMat->at(2);
+		row2->atiput(0, 0.0);
+		row2->atiput(1, sthe);
+		row2->atiput(2, cthe);
+		return rotMat;
+	}
+	template<typename T>
+	inline FMatsptr<T> FullMatrix<T>::rotatey(T the)
+	{
+		auto sthe = std::sin(the);
+		auto cthe = std::cos(the);
+		auto rotMat = std::make_shared<FullMatrix<T>>(3, 3);
+		auto row0 = rotMat->at(0);
+		row0->atiput(0, cthe);
+		row0->atiput(1, 0.0);
+		row0->atiput(2, sthe);
+		auto row1 = rotMat->at(1);
+		row1->atiput(0, 0.0);
+		row1->atiput(1, 1.0);
+		row1->atiput(2, 0.0);
+		auto row2 = rotMat->at(2);
+		row2->atiput(0, -sthe);
+		row2->atiput(1, 0.0);
+		row2->atiput(2, cthe);
+		return rotMat;
+	}
+	template<typename T>
+	inline FMatsptr<T> FullMatrix<T>::rotatez(T the)
+	{
+		auto sthe = std::sin(the);
+		auto cthe = std::cos(the);
+		auto rotMat = std::make_shared<FullMatrix<T>>(3, 3);
+		auto row0 = rotMat->at(0);
+		row0->atiput(0, cthe);
+		row0->atiput(1, -sthe);
+		row0->atiput(2, 0.0);
+		auto row1 = rotMat->at(1);
+		row1->atiput(0, sthe);
+		row1->atiput(1, cthe);
+		row1->atiput(2, 0.0);
+		auto row2 = rotMat->at(2);
+		row2->atiput(0, 0.0);
+		row2->atiput(1, 0.0);
+		row2->atiput(2, 1.0);
+		return rotMat;
+	}
+	template<typename T>
+	inline FMatsptr<T> FullMatrix<T>::rotatexrotDot(T the, T thedot)
+	{
+		auto sthe = std::sin(the);
+		auto cthe = std::cos(the);
+		auto sthedot = cthe * thedot;
+		auto cthedot = -sthe * thedot;
+		auto rotMat = std::make_shared<FullMatrix<T>>(3, 3);
+		auto row0 = rotMat->at(0);
+		row0->atiput(0, 0.0);
+		row0->atiput(1, 0.0);
+		row0->atiput(2, 0.0);
+		auto row1 = rotMat->at(1);
+		row1->atiput(0, 0.0);
+		row1->atiput(1, cthedot);
+		row1->atiput(2, -sthedot);
+		auto row2 = rotMat->at(2);
+		row2->atiput(0, 0.0);
+		row2->atiput(1, sthedot);
+		row2->atiput(2, cthedot);
+		return rotMat;
+	}
+	template<typename T>
+	inline FMatsptr<T> FullMatrix<T>::rotateyrotDot(T the, T thedot)
+	{
+		auto sthe = std::sin(the);
+		auto cthe = std::cos(the);
+		auto sthedot = cthe * thedot;
+		auto cthedot = -sthe * thedot;
+		auto rotMat = std::make_shared<FullMatrix<T>>(3, 3);
+		auto row0 = rotMat->at(0);
+		row0->atiput(0, cthedot);
+		row0->atiput(1, 0.0);
+		row0->atiput(2, sthedot);
+		auto row1 = rotMat->at(1);
+		row1->atiput(0, 0.0);
+		row1->atiput(1, 0.0);
+		row1->atiput(2, 0.0);
+		auto row2 = rotMat->at(2);
+		row2->atiput(0, -sthedot);
+		row2->atiput(1, 0.0);
+		row2->atiput(2, cthedot);
+		return rotMat;
+	}
+	template<typename T>
+	inline FMatsptr<T> FullMatrix<T>::rotatezrotDot(T the, T thedot)
+	{
+		auto sthe = std::sin(the);
+		auto cthe = std::cos(the);
+		auto sthedot = cthe * thedot;
+		auto cthedot = -sthe * thedot;
+		auto rotMat = std::make_shared<FullMatrix<T>>(3, 3);
+		auto row0 = rotMat->at(0);
+		row0->atiput(0, cthedot);
+		row0->atiput(1, -sthedot);
+		row0->atiput(2, 0.0);
+		auto row1 = rotMat->at(1);
+		row1->atiput(0, sthedot);
+		row1->atiput(1, cthedot);
+		row1->atiput(2, 0.0);
+		auto row2 = rotMat->at(2);
+		row2->atiput(0, 0.0);
+		row2->atiput(1, 0.0);
+		row2->atiput(2, 0.0);
+		return rotMat;
+	}
+	template<typename T>
+	inline FMatsptr<T> FullMatrix<T>::rotatexrotDotrotDDot(T the, T thedot, T theddot)
+	{
+		auto sthe = std::sin(the);
+		auto cthe = std::cos(the);
+		auto sthedot = cthe * thedot;
+		auto cthedot = -sthe * thedot;
+		auto stheddot = cthedot * thedot + (cthe * theddot);
+		auto ctheddot = -(sthedot * thedot) - (sthe * theddot);
+		auto rotMat = std::make_shared<FullMatrix<T>>(3, 3);
+		auto row0 = rotMat->at(0);
+		row0->atiput(0, 0.0);
+		row0->atiput(1, 0.0);
+		row0->atiput(2, 0.0);
+		auto row1 = rotMat->at(1);
+		row1->atiput(0, 0.0);
+		row1->atiput(1, ctheddot);
+		row1->atiput(2, -stheddot);
+		auto row2 = rotMat->at(2);
+		row2->atiput(0, 0.0);
+		row2->atiput(1, stheddot);
+		row2->atiput(2, ctheddot);
+		return rotMat;
+	}
+	template<typename T>
+	inline FMatsptr<T> FullMatrix<T>::rotateyrotDotrotDDot(T the, T thedot, T theddot)
+	{
+		auto sthe = std::sin(the);
+		auto cthe = std::cos(the);
+		auto sthedot = cthe * thedot;
+		auto cthedot = -sthe * thedot;
+		auto stheddot = cthedot * thedot + (cthe * theddot);
+		auto ctheddot = -(sthedot * thedot) - (sthe * theddot);
+		auto rotMat = std::make_shared<FullMatrix<T>>(3, 3);
+		auto row0 = rotMat->at(0);
+		row0->atiput(0, ctheddot);
+		row0->atiput(1, 0.0);
+		row0->atiput(2, stheddot);
+		auto row1 = rotMat->at(1);
+		row1->atiput(0, 0.0);
+		row1->atiput(1, 0.0);
+		row1->atiput(2, 0.0);
+		auto row2 = rotMat->at(2);
+		row2->atiput(0, -stheddot);
+		row2->atiput(1, 0.0);
+		row2->atiput(2, ctheddot);
+		return rotMat;
+	}
+	template<typename T>
+	inline FMatsptr<T> FullMatrix<T>::rotatezrotDotrotDDot(T the, T thedot, T theddot)
+	{
+		auto sthe = std::sin(the);
+		auto cthe = std::cos(the);
+		auto sthedot = cthe * thedot;
+		auto cthedot = -sthe * thedot;
+		auto stheddot = cthedot * thedot + (cthe * theddot);
+		auto ctheddot = -(sthedot * thedot) - (sthe * theddot);
+		auto rotMat = std::make_shared<FullMatrix<T>>(3, 3);
+		auto row0 = rotMat->at(0);
+		row0->atiput(0, ctheddot);
+		row0->atiput(1, -stheddot);
+		row0->atiput(2, 0.0);
+		auto row1 = rotMat->at(1);
+		row1->atiput(0, stheddot);
+		row1->atiput(1, ctheddot);
+		row1->atiput(2, 0.0);
+		auto row2 = rotMat->at(2);
+		row2->atiput(0, 0.0);
+		row2->atiput(1, 0.0);
+		row2->atiput(2, 0.0);
+		return rotMat;
+	}
+	template<typename T>
+	inline FMatsptr<T> FullMatrix<T>::identitysptr(int n)
+	{
+		auto mat = std::make_shared<FullMatrix<T>>(n, n);
+		mat->identity();
+		return mat;
+	}
+	template<typename T>
+	inline FMatsptr<T> FullMatrix<T>::tildeMatrix(FColDsptr col)
+	{
+		//"tildeMatrix is skew symmetric matrix related to angular velocity and cross product."
+		if (col->size() != 3) throw std::runtime_error("Column is not of dimension 3");
+		auto tilde = std::make_shared<FullMatrix<double>>(3, 3);
+		auto c0 = col->at(0);
+		auto c1 = col->at(1);
+		auto c2 = col->at(2);
+		tilde->atijput(0, 0, 0.0);
+		tilde->atijput(1, 1, 0.0);
+		tilde->atijput(2, 2, 0.0);
+		tilde->atijput(1, 2, -c0);
+		tilde->atijput(0, 2, c1);
+		tilde->atijput(0, 1, -c2);
+		tilde->atijput(1, 0, c2);
+		tilde->atijput(2, 0, -c1);
+		tilde->atijput(2, 1, c0);
+		return tilde;
+	}
+	template<>
+	inline void FullMatrix<double>::zeroSelf()
+	{
+		for (int i = 0; i < this->size(); i++) {
+			this->at(i)->zeroSelf();
+		}
+	}
+	template<>
+	inline void FullMatrix<double>::identity() {
+		this->zeroSelf();
+		for (int i = 0; i < this->size(); i++) {
+			this->at(i)->at(i) = 1.0;
+		}
+	}
+	template<typename T>
+	inline FColsptr<T> FullMatrix<T>::column(int j) {
+		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)->at(j);
+		}
+		return answer;
+	}
+	template<typename T>
+	inline FMatsptr<T> FullMatrix<T>::timesFullMatrix(FMatsptr<T> fullMat)
+	{
+		int m = this->nrow();
+		auto answer = std::make_shared<FullMatrix<T>>(m);
+		for (int i = 0; i < m; i++) {
+			answer->at(i) = this->at(i)->timesFullMatrix(fullMat);
+		}
+		return answer;
+	}
+	template<typename T>
+	inline FMatsptr<T> FullMatrix<T>::timesTransposeFullMatrix(FMatsptr<T> fullMat)
+	{
+		int nrow = this->nrow();
+		auto answer = std::make_shared<FullMatrix<T>>(nrow);
+		for (int i = 0; i < nrow; i++) {
+			answer->at(i) = this->at(i)->timesTransposeFullMatrix(fullMat);
+		}
+		return answer;
+	}
+	template<>
+	inline FMatDsptr FullMatrix<double>::times(double a)
+	{
+		int m = this->nrow();
+		auto answer = std::make_shared<FullMatrix<double>>(m);
+		for (int i = 0; i < m; i++) {
+			answer->at(i) = this->at(i)->times(a);
+		}
+		return answer;
+	}
+	template<typename T>
+	inline FMatsptr<T> FullMatrix<T>::times(T a)
+	{
+		assert(false);
+	}
+	template<typename T>
+	inline FMatsptr<T> FullMatrix<T>::transposeTimesFullMatrix(FMatsptr<T> fullMat)
+	{
+		return this->transpose()->timesFullMatrix(fullMat);
+	}
+	template<typename T>
+	inline FMatsptr<T> FullMatrix<T>::plusFullMatrix(FMatsptr<T> fullMat)
+	{
+		int n = (int)this->size();
+		auto answer = std::make_shared<FullMatrix<T>>(n);
+		for (int i = 0; i < n; i++) {
+			answer->at(i) = this->at(i)->plusFullRow(fullMat->at(i));
+		}
+		return answer;
+	}
+	template<typename T>
+	inline FMatsptr<T> FullMatrix<T>::minusFullMatrix(FMatsptr<T> fullMat)
+	{
+		int n = (int)this->size();
+		auto answer = std::make_shared<FullMatrix<T>>(n);
+		for (int i = 0; i < n; i++) {
+			answer->at(i) = this->at(i)->minusFullRow(fullMat->at(i));
+		}
+		return answer;
+	}
+	template<typename T>
+	inline FMatsptr<T> FullMatrix<T>::transpose()
+	{
+		int nrow = this->nrow();
+		auto ncol = this->ncol();
+		auto answer = std::make_shared<FullMatrix<T>>(ncol, nrow);
+		for (int i = 0; i < nrow; i++) {
+			auto& row = this->at(i);
+			for (int j = 0; j < ncol; j++) {
+				answer->at(j)->at(i) = row->at(j);
+			}
+		}
+		return answer;
+	}
+	template<typename T>
+	inline FMatsptr<T> FullMatrix<T>::negated()
+	{
+		return this->times(-1.0);
+	}
+	template<typename T>
+	inline void FullMatrix<T>::symLowerWithUpper()
+	{
+		int n = (int)this->size();
+		for (int i = 0; i < n; i++) {
+			for (int j = i + 1; j < n; j++) {
+				this->at(j)->at(i) = this->at(i)->at(j);
+			}
+		}
+	}
+	template<typename T>
+	inline void FullMatrix<T>::atiput(int i, FRowsptr<T> fullRow)
+	{
+		this->at(i) = fullRow;
+	}
+	template<typename T>
+	inline void FullMatrix<T>::atijput(int i, int j, T value)
+	{
+		this->at(i)->atiput(j, value);
+	}
+	template<typename T>
+	inline void FullMatrix<T>::atijputFullColumn(int i1, int j1, FColsptr<T> fullCol)
+	{
+		for (int ii = 0; ii < fullCol->size(); ii++)
+		{
+			this->at(i1 + ii)->at(j1) = fullCol->at(ii);
+		}
+	}
+	template<typename T>
+	inline void FullMatrix<T>::atijplusFullRow(int i, int j, FRowsptr<T> fullRow)
+	{
+		this->at(i)->atiplusFullRow(j, fullRow);
+	}
+	template<typename T>
+	inline void FullMatrix<T>::atijplusNumber(int i, int j, T value)
+	{
+		auto rowi = this->at(i);
+		rowi->at(j) += value;
+	}
+	template<typename T>
+	inline void FullMatrix<T>::atijminusNumber(int i, int j, T value)
+	{
+		auto rowi = this->at(i);
+		rowi->at(j) -= value;
+	}
+	template<>
+	inline double FullMatrix<double>::sumOfSquares()
+	{
+		double sum = 0.0;
+		for (int i = 0; i < this->size(); i++)
+		{
+			sum += this->at(i)->sumOfSquares();
+		}
+		return sum;
+	}
+	template<typename T>
+	inline double FullMatrix<T>::sumOfSquares()
+	{
+		assert(false);
+		return 0.0;
+	}
+	template<typename T>
+	inline void FullMatrix<T>::zeroSelf()
+	{
+		assert(false);
+	}
+	template<typename T>
+	inline FMatsptr<T> FullMatrix<T>::copy()
+	{
+		auto m = (int)this->size();
+		auto answer = std::make_shared<FullMatrix<T>>(m);
+		for (int i = 0; i < m; i++)
+		{
+			answer->at(i) = this->at(i)->copy();
+		}
+		return answer;
+	}
+	template<typename T>
+	inline FullMatrix<T> FullMatrix<T>::operator+(const FullMatrix<T> fullMat)
+	{
+		int n = (int)this->size();
+		auto answer = FullMatrix<T>(n);
+		for (int i = 0; i < n; i++) {
+			answer.at(i) = this->at(i)->plusFullRow(fullMat.at(i));
+		}
+		return answer;
+	}
+	template<typename T>
+	inline FColsptr<T> FullMatrix<T>::transposeTimesFullColumn(FColsptr<T> fullCol)
+	{
+		auto sptr = std::make_shared<FullMatrix<T>>(*this);
+		return fullCol->transpose()->timesFullMatrix(sptr)->transpose();
+	}
+	template<typename T>
+	inline void FullMatrix<T>::magnifySelf(T factor)
+	{
+		for (int i = 0; i < this->size(); i++) {
+			this->at(i)->magnifySelf(factor);
+		}
+	}
+	template<typename T>
+	inline std::ostream& FullMatrix<T>::printOn(std::ostream& s) const
+	{
+		s << "FullMat[" << std::endl;
+		for (int i = 0; i < this->size(); i++)
+		{
+			s << *(this->at(i)) << std::endl;
+		}
+		s << "]";
+		return s;
+	}
+	template<typename T>
+	inline std::shared_ptr<EulerParameters<T>> FullMatrix<T>::asEulerParameters()
+	{
+		//"Given [A], compute Euler parameter."
+
+		auto traceA = this->trace();
+		T dum = 0.0;
+		T dumSq = 0.0;
+		//auto qE = CREATE<EulerParameters<double>>::With(4); //Cannot use CREATE.h in subclasses of std::vector. Why?
+		auto qE = std::make_shared<EulerParameters<T>>(4);
+		qE->initialize();
+		auto OneMinusTraceDivFour = (1.0 - traceA) / 4.0;
+		for (int i = 0; i < 3; i++)
+		{
+			dumSq = this->at(i)->at(i) / 2.0 + OneMinusTraceDivFour;
+			dum = (dumSq > 0.0) ? std::sqrt(dumSq) : 0.0;
+			qE->atiput(i, dum);
+		}
+		dumSq = (1.0 + traceA) / 4.0;
+		dum = (dumSq > 0.0) ? std::sqrt(dumSq) : 0.0;
+		qE->atiput(3, dum);
+		T max = 0.0;
+		int maxE = -1;
+		for (int i = 0; i < 4; i++)
+		{
+			auto num = qE->at(i);
+			if (max < num) {
+				max = num;
+				maxE = i;
+			}
+		}
+
+		if (maxE == 0) {
+			auto FourE = 4.0 * qE->at(0);
+			qE->atiput(1, (this->at(0)->at(1) + this->at(1)->at(0)) / FourE);
+			qE->atiput(2, (this->at(0)->at(2) + this->at(2)->at(0)) / FourE);
+			qE->atiput(3, (this->at(2)->at(1) - this->at(1)->at(2)) / FourE);
+		}
+		else if (maxE == 1) {
+			auto FourE = 4.0 * qE->at(1);
+			qE->atiput(0, (this->at(0)->at(1) + this->at(1)->at(0)) / FourE);
+			qE->atiput(2, (this->at(1)->at(2) + this->at(2)->at(1)) / FourE);
+			qE->atiput(3, (this->at(0)->at(2) - this->at(2)->at(0)) / FourE);
+		}
+		else if (maxE == 2) {
+			auto FourE = 4.0 * qE->at(2);
+			qE->atiput(0, (this->at(0)->at(2) + this->at(2)->at(0)) / FourE);
+			qE->atiput(1, (this->at(1)->at(2) + this->at(2)->at(1)) / FourE);
+			qE->atiput(3, (this->at(1)->at(0) - this->at(0)->at(1)) / FourE);
+		}
+		else if (maxE == 3) {
+			auto FourE = 4.0 * qE->at(3);
+			qE->atiput(0, (this->at(2)->at(1) - this->at(1)->at(2)) / FourE);
+			qE->atiput(1, (this->at(0)->at(2) - this->at(2)->at(0)) / FourE);
+			qE->atiput(2, (this->at(1)->at(0) - this->at(0)->at(1)) / FourE);
+		}
+		qE->conditionSelf();
+		qE->calc();
+		return qE;
+	}
+	template<typename T>
+	inline T FullMatrix<T>::trace()
+	{
+		T trace = 0.0;
+		for (int i = 0; i < this->size(); i++)
+		{
+			trace += this->at(i)->at(i);
+		}
+		return trace;
+	}
+	template<typename T>
+	inline double FullMatrix<T>::maxMagnitude()
+	{
+		double max = 0.0;
+		for (int i = 0; i < this->size(); i++)
+		{
+			double element = this->at(i)->maxMagnitude();
+			if (max < element) max = element;
+		}
+		return max;
+	}
+	template<typename T>
+	inline FColsptr<T> FullMatrix<T>::bryantAngles()
+	{
+		auto answer = std::make_shared<FullColumn<T>>(3);
+		auto sthe1y = this->at(0)->at(2);
+		T the0x, the1y, the2z, cthe0x, sthe0x, y, x;
+		if (std::abs(sthe1y) > 0.9999) {
+			if (sthe1y > 0.0) {
+				the0x = std::atan2(this->at(1)->at(0), this->at(1)->at(1));
+				the1y = OS_M_PI / 2.0;
+				the2z = 0.0;
+			}
+			else {
+				the0x = std::atan2(this->at(2)->at(1), this->at(2)->at(0));
+				the1y = OS_M_PI / -2.0;
+				the2z = 0.0;
+			}
+		}
+		else {
+			the0x = std::atan2(-this->at(1)->at(2), this->at(2)->at(2));
+			cthe0x = std::cos(the0x);
+			sthe0x = std::sin(the0x);
+			y = sthe1y;
+			if (std::abs(cthe0x) > std::abs(sthe0x)) {
+				x = this->at(2)->at(2) / cthe0x;
+			}
+			else {
+				x = this->at(1)->at(2) / -sthe0x;
+			}
+			the1y = std::atan2(y, x);
+			the2z = std::atan2(-this->at(0)->at(1), this->at(0)->at(0));
+		}
+		answer->atiput(0, the0x);
+		answer->atiput(1, the1y);
+		answer->atiput(2, the2z);
+		return answer;
+	}
+	template<typename T>
+	inline bool FullMatrix<T>::isDiagonal()
+	{
+		auto m = this->nrow();
+		auto n = this->ncol();
+		if (m != n) return false;
+		for (int i = 0; i < m; i++)
+		{
+			auto rowi = this->at(i);
+			for (int j = 0; j < n; j++)
+			{
+				if (i != j && rowi->at(j) != 0) return false;
+			}
+		}
+		return true;
+	}
+	template<typename T>
+	inline bool FullMatrix<T>::isDiagonalToWithin(double ratio)
+	{
+		double maxMag = this->maxMagnitude();
+		auto tol = ratio * maxMag;
+		auto nrow = this->nrow();
+		if (nrow == this->ncol()) {
+			for (int i = 0; i < 3; i++)
+			{
+				for (int j = i + 1; j < 3; j++)
+				{
+					if (std::abs(this->at(i)->at(j)) > tol) return false;
+					if (std::abs(this->at(j)->at(i)) > tol) return false;
+				}
+			}
+			return true;
+		}
+		else {
+			return false;
+		}
+	}
+	template<typename T>
+	inline std::shared_ptr<DiagonalMatrix<T>> FullMatrix<T>::asDiagonalMatrix()
+	{
+		int nrow = this->nrow();
+		auto diagMat = std::make_shared<DiagonalMatrix<T>>(nrow);
+		for (int i = 0; i < nrow; i++)
+		{
+			diagMat->atiput(i, this->at(i)->at(i));
+		}
+		return diagMat;
+	}
+	template<typename T>
+	inline void FullMatrix<T>::conditionSelfWithTol(double tol)
+	{
+		for (auto row : *this) {
+			row->conditionSelfWithTol(tol);
+		}
+	}
+	template<typename T>
+	inline FColsptr<T> FullMatrix<T>::timesFullColumn(FColsptr<T> fullCol)
+	{
+		return this->timesFullColumn(fullCol.get());
+	}
+	template<typename T>
+	inline FColsptr<T> FullMatrix<T>::timesFullColumn(FullColumn<T>* fullCol)
+	{
+		//"a*b = a(i,j)b(j) sum j."
+		auto nrow = this->nrow();
+		auto answer = std::make_shared<FullColumn<T>>(nrow);
+		for (int i = 0; i < nrow; i++)
+		{
+			answer->at(i) = this->at(i)->timesFullColumn(fullCol);
+		}
+		return answer;
+	}
 }
+

OndselSolver/FullMatrix.ref.h

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

OndselSolver/FullRow.cpp

@@ -7,54 +7,5 @@
  ***************************************************************************/
  
 #include "FullRow.h"
-#include "FullMatrix.h"
 
-namespace MbD {
-    template<>
-    std::shared_ptr<FullMatrixDouble> FullRow<double>::transposeTimesFullRow(FRowsptr<double> fullRow)
-    {
-        //"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;
-    }
-    //template<typename T>
-    //inline FMatDsptr FullRow<T>::transposeTimesFullRow(FRowDsptr fullRow)
-    //{
-    //    //"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;
-    //}
-
-    template<>
-    FRowsptr<double> FullRow<double>::timesTransposeFullMatrix(std::shared_ptr<FullMatrixDouble> fullMat)
-    {
-        //"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;
-    }
-
-    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 < this->size(); j++)
-        {
-            answer->equalSelfPlusFullRowTimes(fullMat->at(j), this->at(j));
-        }
-        return answer;
-    }
-}
+using namespace MbD;

OndselSolver/FullRow.h

@@ -9,10 +9,23 @@
 #pragma once
 
 #include "FullVector.h"
-#include "FullMatrix.ref.h"
-#include "FullRow.ref.h"
+//#include "FullColumn.h"
 
 namespace MbD {
+	template<typename T>
+	class FullRow;
+	template<typename T>
+	using FRowsptr = std::shared_ptr<FullRow<T>>;
+	using FRowDsptr = std::shared_ptr<FullRow<double>>;
+	template<typename T>
+	class FullMatrix;
+	template<typename T>
+	using FMatsptr = std::shared_ptr<FullMatrix<T>>;
+	template<typename T>
+	class FullColumn;
+	template<typename T>
+	using FColsptr = std::shared_ptr<FullColumn<T>>;
+	using ListFRD = std::initializer_list<FRowDsptr>;
 
 	template<typename T>
 	class FullRow : public FullVector<T>
@@ -30,25 +43,20 @@ namespace MbD {
 		FRowsptr<T> minusFullRow(FRowsptr<T> fullRow);
 		T timesFullColumn(FColsptr<T> fullCol);
 		T timesFullColumn(FullColumn<T>* fullCol);
+		FRowsptr<T> timesFullMatrix(FMatsptr<T> fullMat);
+		FRowsptr<T> timesTransposeFullMatrix(FMatsptr<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);
-		FMatDsptr transposeTimesFullRow(FRowDsptr fullRow);
-		std::shared_ptr<FullRow<T>> clonesptr();
-		//double dot(std::shared_ptr<FullColumn<T>> vec);
-		//double dot(std::shared_ptr<FullRow<T>> vec);
+		FMatsptr<T> transposeTimesFullRow(FRowsptr<T> fullRow);
 		double dot(std::shared_ptr<FullVector<T>> vec);
 		std::shared_ptr<FullVector<T>> dot(std::shared_ptr<std::vector<std::shared_ptr<FullColumn<T>>>> vecvec);
 
-		
 		std::ostream& printOn(std::ostream& s) const override;
 
-        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)
@@ -73,7 +81,7 @@ namespace MbD {
 	template<typename T>
 	inline FRowsptr<T> FullRow<T>::plusFullRow(FRowsptr<T> fullRow)
 	{
-		int n = (int)this->size();
+		int n = (int) this->size();
 		auto answer = std::make_shared<FullRow<T>>(n);
 		for (int i = 0; i < n; i++) {
 			answer->at(i) = this->at(i) + fullRow->at(i);
@@ -83,7 +91,7 @@ namespace MbD {
 	template<typename T>
 	inline FRowsptr<T> FullRow<T>::minusFullRow(FRowsptr<T> fullRow)
 	{
-		int n = (int)this->size();
+		int n = (int) this->size();
 		auto answer = std::make_shared<FullRow<T>>(n);
 		for (int i = 0; i < n; i++) {
 			answer->at(i) = this->at(i) - fullRow->at(i);
@@ -106,6 +114,17 @@ namespace MbD {
 		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 void FullRow<T>::equalSelfPlusFullRowTimes(FRowsptr<T> fullRow, double factor)
 	{
 		this->equalSelfPlusFullVectortimes(fullRow, factor);
@@ -141,9 +160,16 @@ namespace MbD {
 		}
 	}
 	template<typename T>
-	inline std::shared_ptr<FullRow<T>> FullRow<T>::clonesptr()
+	inline FMatsptr<T> FullRow<T>::transposeTimesFullRow(FRowsptr<T> fullRow)
 	{
-		return std::make_shared<FullRow<T>>(*this);
+		//"a*b = a(i)b(j)"
+		auto nrow = (int)this->size();
+		auto answer = std::make_shared<FullMatrix<T>>(nrow);
+		for (int i = 0; i < nrow; i++)
+		{
+			answer->atiput(i, fullRow->times(this->at(i)));
+		}
+		return answer;
 	}
 	template<typename T>
 	inline double FullRow<T>::dot(std::shared_ptr<FullVector<T>> vec)
@@ -182,7 +208,17 @@ namespace MbD {
 		}
 		s << "}";
 		return s;
-	};
-
+	}
+	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++)
+		{
+			answer->equalSelfPlusFullRowTimes(fullMat->at(j), this->at(j));
+		}
+		return answer;
+			//return FRowsptr<T>();
+	}
 }
 

OndselSolver/FullRow.ref.h

@@ -1,15 +0,0 @@
-#pragma once
-
-#include "FullColumn.ref.h"
-#include <algorithm>
-
-namespace MbD {
-    template<typename T>
-    class FullRow;
-
-    template<typename T>
-    using FRowsptr = std::shared_ptr<FullRow<T>>;
-    using FRowDsptr = std::shared_ptr<FullRow<double>>;
-
-    using ListFRD = std::initializer_list<FRowDsptr>;
-}

OndselSolver/GeneralSpline.cpp

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

OndselSolver/Item.h

@@ -10,10 +10,6 @@
 
 #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"

OndselSolver/LDUFullMat.cpp

@@ -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 <FullMatrixDouble>(m, n);
+	auto matrixAinverse = std::make_shared <FullMatrix<double>>(m, n);
 	for (int j = 0; j < n; j++)
 	{
 		rightHandSideB->zeroSelf();

OndselSolver/MBDynBody.cpp

@@ -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<FullMatrixDouble>(3, 3);
+	aJmat = std::make_shared<FullMatrix<double>>(3, 3);
 	auto str = args.at(0);	//Must copy string
 	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(FullMatrixDouble::identitysptr(3));
+		asmtMassMarker->setRotationMatrix(FullMatrix<double>::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(FullMatrixDouble::identitysptr(3));
+		solver->setAPo(FullMatrix<double>::identitysptr(3));
 		solver->setrPcmP(rPcmP);
 		solver->calc();
 		asmtMassMarker->setMomentOfInertias(solver->aJpp);

OndselSolver/MBDynBody.h

@@ -14,7 +14,7 @@ namespace MbD {
     {
     public:
         void initialize() override;
-        void parseMBDyn(std::string line);
+        void parseMBDyn(std::string line) override;
         void readMass(std::vector<std::string>& args);
         void readInertiaMatrix(std::vector<std::string>& args);
         void createASMT() override;

OndselSolver/MBDynDrive.h

@@ -13,7 +13,7 @@ namespace MbD {
 	class MBDynDrive : public MBDynElement
 	{
 	public:
-        void parseMBDyn(std::string line);
+        void parseMBDyn(std::string line) override;
 		void readFunction(std::vector<std::string>& args);
 		void createASMT() override;
 

OndselSolver/MBDynGravity.cpp

@@ -48,6 +48,7 @@ void MbD::MBDynGravity::parseMBDyn(std::string line)
 void MbD::MBDynGravity::readFunction(std::vector<std::string>& args)
 {
 	assert(false);
+	noop();
 }
 
 void MbD::MBDynGravity::createASMT()

OndselSolver/MBDynGravity.h

@@ -13,7 +13,7 @@ namespace MbD {
 	class MBDynGravity : public MBDynElement
 	{
 	public:
-		void parseMBDyn(std::string line);
+		void parseMBDyn(std::string line) override;
 		void readFunction(std::vector<std::string>& args);
 		void createASMT() override;
 

OndselSolver/MBDynItem.cpp

@@ -34,6 +34,11 @@ void MbD::MBDynItem::parseMBDyn(std::vector<std::string>& lines)
 	assert(false);
 }
 
+void MbD::MBDynItem::parseMBDyn(std::string line)
+{
+	assert(false);
+}
+
 std::vector<std::string> MbD::MBDynItem::collectArgumentsFor(std::string title, std::string& statement)
 {
 	size_t previousPos = 0;
@@ -67,7 +72,7 @@ std::vector<std::string> MbD::MBDynItem::collectArgumentsFor(std::string title,
 			//Need to find matching '"'
 			auto it = std::find_if(arguments.begin() + 1, arguments.end(), [](const std::string& s) {
 				auto nn = std::count(s.begin(), s.end(), '"');
-				if ((nn % 2) == 1) return true;
+				return (nn % 2) == 1;
 				});
 			std::vector<std::string> needToCombineArgs(arguments.begin(), it + 1);
 			arguments.erase(arguments.begin(), it + 1);
@@ -242,7 +247,7 @@ FColDsptr MbD::MBDynItem::readBasicPosition(std::vector<std::string>& args)
 
 FMatDsptr MbD::MBDynItem::readOrientation(std::vector<std::string>& args)
 {
-	auto aAOf = FullMatrixDouble::identitysptr(3);
+	auto aAOf = FullMatrix<double>::identitysptr(3);
 	if (args.empty()) return aAOf;
 	auto str = args.at(0); //Must copy string
 	if (str.find("reference") != std::string::npos) {
@@ -292,7 +297,7 @@ FMatDsptr MbD::MBDynItem::readBasicOrientation(std::vector<std::string>& args)
 	}
 	if (str.find("eye") != std::string::npos) {
 		args.erase(args.begin());
-		auto aAFf = FullMatrixDouble::identitysptr(3);
+		auto aAFf = FullMatrix<double>::identitysptr(3);
 		return aAFf;
 	}
 	auto iss = std::istringstream(str);
@@ -341,7 +346,7 @@ FMatDsptr MbD::MBDynItem::readBasicOrientation(std::vector<std::string>& args)
 		else {
 			assert(false);
 		}
-		auto aAFf = FullMatrixDouble::identitysptr(3);
+		auto aAFf = FullMatrix<double>::identitysptr(3);
 		aAFf->atijputFullColumn(0, 0, vecX);
 		aAFf->atijputFullColumn(0, 1, vecY);
 		aAFf->atijputFullColumn(0, 2, vecZ);
@@ -390,13 +395,13 @@ FMatDsptr MbD::MBDynItem::readBasicOrientation(std::vector<std::string>& args)
 		else {
 			assert(false);
 		}
-		auto aAFf = FullMatrixDouble::identitysptr(3);
+		auto aAFf = FullMatrix<double>::identitysptr(3);
 		aAFf->atijputFullColumn(0, 0, vecX);
 		aAFf->atijputFullColumn(0, 1, vecY);
 		aAFf->atijputFullColumn(0, 2, vecZ);
 		return aAFf;
 	}
-	auto aAFf = FullMatrixDouble::identitysptr(3);
+	auto aAFf = FullMatrix<double>::identitysptr(3);
 	for (int i = 0; i < 3; i++)
 	{
 		auto& rowi = aAFf->at(i);

OndselSolver/MBDynItem.h

@@ -31,6 +31,7 @@ namespace MbD {
 		void noop();
 		//void setName(std::string str);
 		virtual void parseMBDyn(std::vector<std::string>& lines);
+		virtual void parseMBDyn(std::string line);
 		static std::vector<std::string> collectArgumentsFor(std::string title, std::string& statement);
 		std::vector<std::string>::iterator findLineWith(std::vector<std::string>& lines, std::vector<std::string>& tokens);
 		bool lineHasTokens(const std::string& line, std::vector<std::string>& tokens);

OndselSolver/MBDynJoint.cpp

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

OndselSolver/MBDynJoint.h

@@ -15,7 +15,7 @@ namespace MbD {
     {
     public:
         void initialize() override;
-        void parseMBDyn(std::string line);
+        void parseMBDyn(std::string line) override;
         void readMarkerI(std::vector<std::string>& args);
         void readMarkerJ(std::vector<std::string>& args);
         void readClampMarkerJ(std::vector<std::string>& args);

OndselSolver/MBDynMarker.cpp

@@ -11,7 +11,7 @@ using namespace MbD;
 void MbD::MBDynMarker::parseMBDyn(std::vector<std::string>& args)
 {
 	rPmP = std::make_shared<FullColumn<double>>(3);
-	aAPm = FullMatrixDouble::identitysptr(3);
+	aAPm = FullMatrix<double>::identitysptr(3);
 	if (args.empty()) return;
 	auto str = args.at(0); //Must copy string
 	if (str.find("reference") != std::string::npos) {
@@ -37,7 +37,7 @@ void MbD::MBDynMarker::parseMBDynClamp(std::vector<std::string>& args)
 	//rOmO = rOPO + aAOP*rPmP
 	//aAOm = aAOP * aAPm
 	auto rOmO = std::make_shared<FullColumn<double>>(3);
-	auto aAOm = FullMatrixDouble::identitysptr(3);
+	auto aAOm = FullMatrix<double>::identitysptr(3);
 	auto rOPO = readPosition(args);
 	auto aAOP = readOrientation(args);
 	rPmP = aAOP->transposeTimesFullColumn(rOmO->minusFullColumn(rOPO));

OndselSolver/MBDynReference.h

@@ -15,7 +15,7 @@ namespace MbD {
 	{
 	public:
 		void initialize() override;
-		void parseMBDyn(std::string line);
+		void parseMBDyn(std::string line) override;
 		void readVelocity(std::vector<std::string>& args);
 		void readOmega(std::vector<std::string>& args);
 

OndselSolver/MBDynStructural.cpp

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

OndselSolver/MBDynStructural.h

@@ -14,7 +14,7 @@ namespace MbD {
     {
     public:
         MBDynStructural();
-        void parseMBDyn(std::string line);
+        void parseMBDyn(std::string line) override;
         void readVelocity(std::vector<std::string>& args);
         void readOmega(std::vector<std::string>& args);
         void createASMT() override;

OndselSolver/MarkerFrame.cpp

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

OndselSolver/MarkerFrame.h

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

OndselSolver/MomentOfInertiaSolver.cpp

@@ -7,7 +7,7 @@ using namespace MbD;
 
 void MbD::MomentOfInertiaSolver::example1()
 {
-	auto aJpp = std::make_shared<FullMatrixDouble>(ListListD{
+	auto aJpp = std::make_shared<FullMatrix<double>>(ListListD{
 		{ 1, 0, 0 },
 		{ 0, 2, 0 },
 		{ 0, 0, 3 }
@@ -56,8 +56,8 @@ void MbD::MomentOfInertiaSolver::doFullPivoting(int p)
 				auto mag = std::abs(aij);
 				if (mag > max) {
 					max = mag;
-					pivotRow = (int)i;
-					pivotCol = (int)j;
+					pivotRow = i;
+					pivotCol = j;
 				}
 			}
 		}
@@ -179,10 +179,10 @@ void MbD::MomentOfInertiaSolver::calcJoo()
 
 	if (!rPoP) {
 		rPoP = rPcmP;
-		aAPo = FullMatrixDouble::identitysptr(3);
+		aAPo = FullMatrix<double>::identitysptr(3);
 	}
-	auto rocmPtilde = FullMatrixDouble::tildeMatrix(rPcmP->minusFullColumn(rPoP));
-	auto rPoPtilde = FullMatrixDouble::tildeMatrix(rPoP);
+	auto rocmPtilde = FullMatrix<double>::tildeMatrix(rPcmP->minusFullColumn(rPoP));
+	auto rPoPtilde = FullMatrix<double>::tildeMatrix(rPoP);
 	auto term1 = aJPP;
 	auto term21 = rPoPtilde->timesFullMatrix(rPoPtilde);
 	auto term22 = rPoPtilde->timesFullMatrix(rocmPtilde);
@@ -197,7 +197,7 @@ void MbD::MomentOfInertiaSolver::calcJpp()
 {
 	//"aJcmP = aJPP + mass*(rPcmPTilde*rPcmPTilde)"
 
-	auto rPcmPtilde = FullMatrixDouble::tildeMatrix(rPcmP);
+	auto rPcmPtilde = FullMatrix<double>::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 = FullMatrixDouble::identitysptr(3);
+			aAPp = FullMatrix<double>::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<FullMatrixDouble>(3, 3);
+	aAPp = std::make_shared<FullMatrix<double>>(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>();
+	auto sortedJ = std::make_shared<DiagonalMatrix<double>>();
 	sortedJ->push_back(aJcmP->at(0)->at(0));
 	sortedJ->push_back(aJcmP->at(1)->at(1));
 	sortedJ->push_back(aJcmP->at(2)->at(2));
@@ -326,7 +326,7 @@ void MbD::MomentOfInertiaSolver::calcJppFromDiagJcmP()
 			lam2 = average;
 		}
 	}
-	aJpp = std::make_shared<DiagonalMatrix>(ListD{ lam0, lam1, lam2 });
+	aJpp = std::make_shared<DiagonalMatrix<double>>(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 = OS_M_PI / 3.0;
-	auto sortedJ = std::make_shared<DiagonalMatrix>();
+	auto sortedJ = std::make_shared<DiagonalMatrix<double>>();
 	sortedJ->push_back(twoSqrtMinusp * std::cos(phiDiv3));
 	sortedJ->push_back(twoSqrtMinusp * -std::cos(phiDiv3 + piDiv3));
 	sortedJ->push_back(twoSqrtMinusp * -std::cos(phiDiv3 - piDiv3));
@@ -382,7 +382,7 @@ void MbD::MomentOfInertiaSolver::calcJppFromFullJcmP()
 			lam2 = average;
 		}
 	}
-	aJpp = std::make_shared<DiagonalMatrix>(ListD{ lam0, lam1, lam2 });
+	aJpp = std::make_shared<DiagonalMatrix<double>>(ListD{ lam0, lam1, lam2 });
 }
 
 double MbD::MomentOfInertiaSolver::modifiedArcCos(double val)

OndselSolver/OndselSolver.vcxproj

@@ -533,19 +533,14 @@
     <ClInclude Include="CREATE.h" />
     <ClInclude Include="CylindricalJoint.h" />
     <ClInclude Include="CylSphJoint.h" />
-    <ClInclude Include="DiagonalMatrix.ref.h" />
     <ClInclude Include="DifferentiatedGeneralSpline.h" />
     <ClInclude Include="EigenDecomposition.h" />
     <ClInclude Include="EndFrameqct2.h" />
     <ClInclude Include="EulerAngles.h" />
     <ClInclude Include="EulerAnglesDDot.h" />
     <ClInclude Include="EulerAnglesDot.h" />
-    <ClInclude Include="EulerParameters.ref.h" />
     <ClInclude Include="Exponential.h" />
     <ClInclude Include="ExternalSystem.h" />
-    <ClInclude Include="FullColumn.ref.h" />
-    <ClInclude Include="FullMatrix.ref.h" />
-    <ClInclude Include="FullRow.ref.h" />
     <ClInclude Include="FunctionFromData.h" />
     <ClInclude Include="FunctionXcParameter.h" />
     <ClInclude Include="FunctionXY.h" />
@@ -802,6 +797,9 @@
     <Media Include="..\testapp\MBDynCase2.mov" />
     <Media Include="..\testapp\SphericalHinge.mov" />
   </ItemGroup>
+  <ItemGroup>
+    <Text Include="..\CMakeLists.txt" />
+  </ItemGroup>
   <Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
   <ImportGroup Label="ExtensionTargets">
   </ImportGroup>

OndselSolver/OndselSolver.vcxproj.filters

@@ -1814,21 +1814,6 @@
     <ClInclude Include="..\testapp\OndselSolver.h">
       <Filter>Header Files</Filter>
     </ClInclude>
-    <ClInclude Include="FullRow.ref.h">
-      <Filter>Header Files</Filter>
-    </ClInclude>
-    <ClInclude Include="FullMatrix.ref.h">
-      <Filter>Header Files</Filter>
-    </ClInclude>
-    <ClInclude Include="FullColumn.ref.h">
-      <Filter>Header Files</Filter>
-    </ClInclude>
-    <ClInclude Include="EulerParameters.ref.h">
-      <Filter>Header Files</Filter>
-    </ClInclude>
-    <ClInclude Include="DiagonalMatrix.ref.h">
-      <Filter>Header Files</Filter>
-    </ClInclude>
     <ClInclude Include="MBDynGravity.h">
       <Filter>Header Files</Filter>
     </ClInclude>
@@ -1927,4 +1912,7 @@
     <Media Include="..\testapp\MBDynCase2.mov" />
     <Media Include="..\testapp\SphericalHinge.mov" />
   </ItemGroup>
+  <ItemGroup>
+    <Text Include="..\CMakeLists.txt" />
+  </ItemGroup>
 </Project>

OndselSolver/OrbitAngleZIeqcJec.cpp

@@ -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<FullMatrixDouble>(3, 3);
-	ppthezpXIpEI = std::make_shared<FullMatrixDouble>(3, 4);
-	ppthezpEIpEI = std::make_shared<FullMatrixDouble>(4, 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);
 }
 
 FMatDsptr MbD::OrbitAngleZIeqcJec::ppvaluepEIpEI()

OndselSolver/OrbitAngleZIeqcJeqc.cpp

@@ -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<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);
+	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);
 }
 
 FMatDsptr MbD::OrbitAngleZIeqcJeqc::ppvaluepEIpEJ()

OndselSolver/Part.cpp

@@ -35,18 +35,18 @@ void Part::initialize()
 	partFrame = CREATE<PartFrame>::With();
 	partFrame->setPart(this);
 	pTpE = std::make_shared<FullColumn<double>>(4);
-	ppTpEpE = std::make_shared<FullMatrixDouble>(4, 4);
-	ppTpEpEdot = std::make_shared<FullMatrixDouble>(4, 4);
+	ppTpEpE = std::make_shared<FullMatrix<double>>(4, 4);
+	ppTpEpEdot = std::make_shared<FullMatrix<double>>(4, 4);
 }
 
 void Part::initializeLocally()
 {
 	partFrame->initializeLocally();
 	if (m > 0) {
-		mX = std::make_shared<DiagonalMatrix>(3, m);
+		mX = std::make_shared<DiagonalMatrix<double>>(3, m);
 	}
 	else {
-		mX = std::make_shared<DiagonalMatrix>(3, 0.0);
+		mX = std::make_shared<DiagonalMatrix<double>>(3, 0.0);
 	}
 }
 
@@ -271,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>(3);
-	auto wqE = std::make_shared<DiagonalMatrix>(4);
+	auto wqX = std::make_shared<DiagonalMatrix<double>>(3);
+	auto wqE = std::make_shared<DiagonalMatrix<double>>(4);
 	if (mMax == 0) { mMax = 1.0; }
 	for (int i = 0; i < 3; i++)
 	{
@@ -319,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>(3);
-	auto wqEdot = std::make_shared<DiagonalMatrix>(4);
+	auto wqXdot = std::make_shared<DiagonalMatrix<double>>(3);
+	auto wqEdot = std::make_shared<DiagonalMatrix<double>>(4);
 	for (int i = 0; i < 3; i++)
 	{
 		wqXdot->at(i) = (maxw * m / maxInertia) + minw;

OndselSolver/Part.h

@@ -11,9 +11,6 @@
 #include <memory>
 
 #include "Item.h"
-#include "FullColumn.ref.h"
-#include "FullMatrix.ref.h"
-#include "DiagonalMatrix.ref.h"
 #include "EulerParametersDot.h"
 
 namespace MbD {

OndselSolver/RackPinConstraintIqcJc.cpp

@@ -18,8 +18,8 @@ MbD::RackPinConstraintIqcJc::RackPinConstraintIqcJc(EndFrmsptr frmi, EndFrmsptr
 {
 	pGpXI = std::make_shared<FullRow<double>>(3);
 	pGpEI = std::make_shared<FullRow<double>>(4);
-	ppGpXIpEI = std::make_shared<FullMatrixDouble>(3, 4);
-	ppGpEIpEI = std::make_shared<FullMatrixDouble>(4, 4);
+	ppGpXIpEI = std::make_shared<FullMatrix<double>>(3, 4);
+	ppGpEIpEI = std::make_shared<FullMatrix<double>>(4, 4);
 }
 
 void MbD::RackPinConstraintIqcJc::initxIeJeIe()

OndselSolver/RackPinConstraintIqcJqc.cpp

@@ -18,9 +18,9 @@ MbD::RackPinConstraintIqcJqc::RackPinConstraintIqcJqc(EndFrmsptr frmi, EndFrmspt
 {
 	pGpXJ = std::make_shared<FullRow<double>>(3);
 	pGpEJ = std::make_shared<FullRow<double>>(4);
-	ppGpEIpXJ = std::make_shared<FullMatrixDouble>(4, 3);
-	ppGpEIpEJ = std::make_shared<FullMatrixDouble>(4, 4);
-	ppGpEJpEJ = std::make_shared<FullMatrixDouble>(4, 4);
+	ppGpEIpXJ = std::make_shared<FullMatrix<double>>(4, 3);
+	ppGpEIpEJ = std::make_shared<FullMatrix<double>>(4, 4);
+	ppGpEJpEJ = std::make_shared<FullMatrix<double>>(4, 4);
 }
 
 void MbD::RackPinConstraintIqcJqc::initxIeJeIe()

OndselSolver/RowTypeMatrix.h

@@ -9,7 +9,6 @@
 #pragma once
 
 #include "Array.h"
-#include "FullRow.ref.h"
 
 namespace MbD {
 

OndselSolver/ScrewConstraintIqcJc.cpp

@@ -20,8 +20,8 @@ MbD::ScrewConstraintIqcJc::ScrewConstraintIqcJc(EndFrmsptr frmi, EndFrmsptr frmj
 {
 	pGpXI = std::make_shared<FullRow<double>>(3);
 	pGpEI = std::make_shared<FullRow<double>>(4);
-	ppGpXIpEI = std::make_shared<FullMatrixDouble>(3, 4);
-	ppGpEIpEI = std::make_shared<FullMatrixDouble>(4, 4);
+	ppGpXIpEI = std::make_shared<FullMatrix<double>>(3, 4);
+	ppGpEIpEI = std::make_shared<FullMatrix<double>>(4, 4);
 }
 
 void MbD::ScrewConstraintIqcJc::initzIeJeIe()

OndselSolver/ScrewConstraintIqcJqc.cpp

@@ -20,9 +20,9 @@ MbD::ScrewConstraintIqcJqc::ScrewConstraintIqcJqc(EndFrmsptr frmi, EndFrmsptr fr
 {
 	pGpXJ = std::make_shared<FullRow<double>>(3);
 	pGpEJ = std::make_shared<FullRow<double>>(4);
-	ppGpEIpXJ = std::make_shared<FullMatrixDouble>(4, 3);
-	ppGpEIpEJ = std::make_shared<FullMatrixDouble>(4, 4);
-	ppGpEJpEJ = std::make_shared<FullMatrixDouble>(4, 4);
+	ppGpEIpXJ = std::make_shared<FullMatrix<double>>(4, 3);
+	ppGpEIpEJ = std::make_shared<FullMatrix<double>>(4, 4);
+	ppGpEJpEJ = std::make_shared<FullMatrix<double>>(4, 4);
 }
 
 void MbD::ScrewConstraintIqcJqc::initzIeJeIe()

OndselSolver/StableBackwardDifference.cpp

@@ -48,7 +48,7 @@ FColDsptr MbD::StableBackwardDifference::derivativepresentpastpresentDerivativep
 void StableBackwardDifference::instantiateTaylorMatrix()
 {
 	if (taylorMatrix == nullptr || (taylorMatrix->nrow() != (order))) {
-		taylorMatrix = std::make_shared<FullMatrixDouble>(order, order);
+		taylorMatrix = std::make_shared<FullMatrix<double>>(order, order);
 	}
 }
 
@@ -79,7 +79,7 @@ FColDsptr MbD::StableBackwardDifference::derivativepresentpast(int deriv, FColDs
 	//"Answer ith derivative given present value and past values."
 
 	if (deriv == 0) {
-		return y->cloneFcSptr();
+		return std::static_pointer_cast<FullColumn<double>>(y->clonesptr());
 	}
 	else {
 		if (deriv <= order) {

OndselSolver/VelICSolver.cpp

@@ -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>(nqsu);
+		auto qsudotWeights = std::make_shared<DiagonalMatrix<double>>(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); });