diff --git a/OndselSolver/ASMTAssembly.cpp b/OndselSolver/ASMTAssembly.cpp
index 8578ea6..dc830b9 100644
--- a/OndselSolver/ASMTAssembly.cpp
+++ b/OndselSolver/ASMTAssembly.cpp
@@ -997,7 +997,7 @@ void MbD::ASMTAssembly::initprincipalMassMarker()
 	principalMassMarker->density = 0.0;
 	principalMassMarker->momentOfInertias = std::make_shared<DiagonalMatrix<double>>(3, 0);
 	//principalMassMarker->position3D = std::make_shared<FullColumn<double>>(3, 0);
-	//principalMassMarker->rotationMatrix = FullMatrix<double>>::identitysptr(3);
+	//principalMassMarker->rotationMatrix = FullMatrixDouble>::identitysptr(3);
 }
 
 std::shared_ptr<ASMTSpatialContainer> MbD::ASMTAssembly::spatialContainerAt(std::shared_ptr<ASMTAssembly> self, std::string& longname)
diff --git a/OndselSolver/ASMTMarker.cpp b/OndselSolver/ASMTMarker.cpp
index a190e96..dd60b31 100644
--- a/OndselSolver/ASMTMarker.cpp
+++ b/OndselSolver/ASMTMarker.cpp
@@ -47,7 +47,7 @@ FMatDsptr MbD::ASMTMarker::aApm()
 	auto& principalMassMarker = static_cast<ASMTPart*>(refItem->owner)->principalMassMarker;
 	auto& aAPcm = principalMassMarker->rotationMatrix;
 	auto aApm = aAPcm->transposeTimesFullMatrix(aAPref->timesFullMatrix(aArefm));
-	return aApm;
+	return toFMDsptr(aApm);
 }
 
 void MbD::ASMTMarker::createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits)
diff --git a/OndselSolver/CADSystem.cpp b/OndselSolver/CADSystem.cpp
index baa9694..900eb83 100644
--- a/OndselSolver/CADSystem.cpp
+++ b/OndselSolver/CADSystem.cpp
@@ -295,7 +295,7 @@ void CADSystem::runOndselDoublePendulum()
 		auto marker2 = CREATE<MarkerFrame>::With("/Assembly1/Part1/Marker2");
 		rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.4, 0.0, 0.05 });
 		marker2->setrpmp(rpmp);
-		aApm = std::make_shared<FullMatrix<double>>(ListListD{
+		aApm = std::make_shared<FullMatrixDouble>(ListListD{
 			{ 1, 0, 0 },
 			{ 0, 1, 0 },
 			{ 0, 0, 1 }
@@ -325,7 +325,7 @@ void CADSystem::runOndselDoublePendulum()
 		auto marker1 = CREATE<MarkerFrame>::With("/Assembly1/Part2/Marker1");
 		rpmp = std::make_shared<FullColumn<double>>(ListD{ -0.65, 0.0, -0.05 });
 		marker1->setrpmp(rpmp);
-		aApm = std::make_shared<FullMatrix<double>>(ListListD{
+		aApm = std::make_shared<FullMatrixDouble>(ListListD{
 			{1.0, 0.0, 0.0},
 			{0.0, 1.0, 0.0},
 			{0.0, 0.0, 1.0}
@@ -336,7 +336,7 @@ void CADSystem::runOndselDoublePendulum()
 		auto marker2 = CREATE<MarkerFrame>::With("/Assembly1/Part2/Marker2");
 		rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.65, 0.0, -0.05 });
 		marker2->setrpmp(rpmp);
-		aApm = std::make_shared<FullMatrix<double>>(ListListD{
+		aApm = std::make_shared<FullMatrixDouble>(ListListD{
 			{1.0, 0.0, 0.0},
 			{0.0, 1.0, 0.0},
 			{0.0, 0.0, 1.0}
@@ -417,7 +417,7 @@ void CADSystem::runOndselPiston()
 		auto marker2 = CREATE<MarkerFrame>::With("/Assembly1/Marker2");
 		rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.0, 0.0, 0.0 });
 		marker2->setrpmp(rpmp);
-		aApm = std::make_shared<FullMatrix<double>>(ListListD{
+		aApm = std::make_shared<FullMatrixDouble>(ListListD{
 			{ 1, 0, 0 },
 			{ 0, 1, 0 },
 			{ 0, 0, 1 }
@@ -428,7 +428,7 @@ void CADSystem::runOndselPiston()
 		auto marker1 = CREATE<MarkerFrame>::With("/Assembly1/Marker1");
 		rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.0, 3.0, 0.0 });
 		marker1->setrpmp(rpmp);
-		aApm = std::make_shared<FullMatrix<double>>(ListListD{
+		aApm = std::make_shared<FullMatrixDouble>(ListListD{
 			{ 1, 0, 0 },
 			{ 0, 0, 1 },
 			{ 0, -1, 0 }
@@ -460,7 +460,7 @@ void CADSystem::runOndselPiston()
 		auto marker1 = CREATE<MarkerFrame>::With("/Assembly1/Part1/Marker1");
 		rpmp = std::make_shared<FullColumn<double>>(ListD{ -0.4, 0.0, 0.05 });
 		marker1->setrpmp(rpmp);
-		aApm = std::make_shared<FullMatrix<double>>(ListListD{
+		aApm = std::make_shared<FullMatrixDouble>(ListListD{
 			{ 1, 0, 0 },
 			{ 0, 1, 0 },
 			{ 0, 0, 1 }
@@ -471,7 +471,7 @@ void CADSystem::runOndselPiston()
 		auto marker2 = CREATE<MarkerFrame>::With("/Assembly1/Part1/Marker2");
 		rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.4, 0.0, 0.05 });
 		marker2->setrpmp(rpmp);
-		aApm = std::make_shared<FullMatrix<double>>(ListListD{
+		aApm = std::make_shared<FullMatrixDouble>(ListListD{
 			{ 1, 0, 0 },
 			{ 0, 1, 0 },
 			{ 0, 0, 1 }
@@ -502,7 +502,7 @@ void CADSystem::runOndselPiston()
 		auto marker1 = CREATE<MarkerFrame>::With("/Assembly1/Part2/Marker1");
 		rpmp = std::make_shared<FullColumn<double>>(ListD{ -0.65, 0.0, -0.05 });
 		marker1->setrpmp(rpmp);
-		aApm = std::make_shared<FullMatrix<double>>(ListListD{
+		aApm = std::make_shared<FullMatrixDouble>(ListListD{
 			{1.0, 0.0, 0.0},
 			{0.0, 1.0, 0.0},
 			{0.0, 0.0, 1.0}
@@ -513,7 +513,7 @@ void CADSystem::runOndselPiston()
 		auto marker2 = CREATE<MarkerFrame>::With("/Assembly1/Part2/Marker2");
 		rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.65, 0.0, -0.05 });
 		marker2->setrpmp(rpmp);
-		aApm = std::make_shared<FullMatrix<double>>(ListListD{
+		aApm = std::make_shared<FullMatrixDouble>(ListListD{
 			{1.0, 0.0, 0.0},
 			{0.0, 1.0, 0.0},
 			{0.0, 0.0, 1.0}
@@ -544,7 +544,7 @@ void CADSystem::runOndselPiston()
 		auto marker1 = CREATE<MarkerFrame>::With("/Assembly1/Part3/Marker1");
 		rpmp = std::make_shared<FullColumn<double>>(ListD{ -0.5, 0.0, 0.0 });
 		marker1->setrpmp(rpmp);
-		aApm = std::make_shared<FullMatrix<double>>(ListListD{
+		aApm = std::make_shared<FullMatrixDouble>(ListListD{
 			{0.0, 1.0, 0.0},
 			{1.0, 0.0, 0.0},
 			{0.0, 0.0, -1.0}
@@ -555,7 +555,7 @@ void CADSystem::runOndselPiston()
 		auto marker2 = CREATE<MarkerFrame>::With("/Assembly1/Part3/Marker2");
 		rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.5, 0.0, 0.0 });
 		marker2->setrpmp(rpmp);
-		aApm = std::make_shared<FullMatrix<double>>(ListListD{
+		aApm = std::make_shared<FullMatrixDouble>(ListListD{
 			{0.0, 0.0, 1.0},
 			{1.0, 0.0, 0.0},
 			{0.0, 1.0, 0.0}
@@ -654,7 +654,7 @@ void CADSystem::runPiston()
 		auto marker2 = CREATE<MarkerFrame>::With("/Assembly1/Marker2");
 		rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.0, 0.0, 0.0 });
 		marker2->setrpmp(rpmp);
-		aApm = std::make_shared<FullMatrix<double>>(ListListD{
+		aApm = std::make_shared<FullMatrixDouble>(ListListD{
 			{ 1, 0, 0 },
 			{ 0, 1, 0 },
 			{ 0, 0, 1 }
@@ -665,7 +665,7 @@ void CADSystem::runPiston()
 		auto marker1 = CREATE<MarkerFrame>::With("/Assembly1/Marker1");
 		rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.0, 2.8817526385684, 0.0 });
 		marker1->setrpmp(rpmp);
-		aApm = std::make_shared<FullMatrix<double>>(ListListD{
+		aApm = std::make_shared<FullMatrixDouble>(ListListD{
 			{ 1, 0, 0 },
 			{ 0, 0, 1 },
 			{ 0, -1, 0 }
@@ -697,7 +697,7 @@ void CADSystem::runPiston()
 		auto marker1 = CREATE<MarkerFrame>::With("/Assembly1/Part1/Marker1");
 		rpmp = std::make_shared<FullColumn<double>>(ListD{ -0.38423368514246, -2.6661567755108e-17, 0.048029210642807 });
 		marker1->setrpmp(rpmp);
-		aApm = std::make_shared<FullMatrix<double>>(ListListD{
+		aApm = std::make_shared<FullMatrixDouble>(ListListD{
 			{ 1, 0, 0 },
 			{ 0, 1, 0 },
 			{ 0, 0, 1 }
@@ -708,7 +708,7 @@ void CADSystem::runPiston()
 		auto marker2 = CREATE<MarkerFrame>::With("/Assembly1/Part1/Marker2");
 		rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.38423368514246, -2.6661567755108e-17, 0.048029210642807 });
 		marker2->setrpmp(rpmp);
-		aApm = std::make_shared<FullMatrix<double>>(ListListD{
+		aApm = std::make_shared<FullMatrixDouble>(ListListD{
 			{ 1, 0, 0 },
 			{ 0, 1, 0 },
 			{ 0, 0, 1 }
@@ -739,7 +739,7 @@ void CADSystem::runPiston()
 		auto marker1 = CREATE<MarkerFrame>::With("/Assembly1/Part2/Marker1");
 		rpmp = std::make_shared<FullColumn<double>>(ListD{ -0.6243797383565, 1.1997705489799e-16, -0.048029210642807 });
 		marker1->setrpmp(rpmp);
-		aApm = std::make_shared<FullMatrix<double>>(ListListD{
+		aApm = std::make_shared<FullMatrixDouble>(ListListD{
 			{1.0, 2.7755575615629e-16, 0.0},
 			{-2.7755575615629e-16, 1.0, 0.0},
 			{0.0, 0.0, 1.0}
@@ -750,7 +750,7 @@ void CADSystem::runPiston()
 		auto marker2 = CREATE<MarkerFrame>::With("/Assembly1/Part2/Marker2");
 		rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.6243797383565, -2.1329254204087e-16, -0.048029210642807 });
 		marker2->setrpmp(rpmp);
-		aApm = std::make_shared<FullMatrix<double>>(ListListD{
+		aApm = std::make_shared<FullMatrixDouble>(ListListD{
 			{1.0, 2.4980018054066e-16, 2.2204460492503e-16},
 			{-2.4980018054066e-16, 1.0, 4.1633363423443e-17},
 			{-2.2204460492503e-16, -4.1633363423443e-17, 1.0}
@@ -781,7 +781,7 @@ void CADSystem::runPiston()
 		auto marker1 = CREATE<MarkerFrame>::With("/Assembly1/Part3/Marker1");
 		rpmp = std::make_shared<FullColumn<double>>(ListD{ -0.48029210642807, 7.6201599718927e-18, -2.816737703896e-17 });
 		marker1->setrpmp(rpmp);
-		aApm = std::make_shared<FullMatrix<double>>(ListListD{
+		aApm = std::make_shared<FullMatrixDouble>(ListListD{
 			{9.2444637330587e-33, 1.0, 2.2204460492503e-16},
 			{1.0, -9.2444637330587e-33, -1.0785207688569e-32},
 			{-1.0785207688569e-32, 2.2204460492503e-16, -1.0}
@@ -792,7 +792,7 @@ void CADSystem::runPiston()
 		auto marker2 = CREATE<MarkerFrame>::With("/Assembly1/Part3/Marker2");
 		rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.48029210642807, 1.7618247880058e-17, 2.5155758471256e-17 });
 		marker2->setrpmp(rpmp);
-		aApm = std::make_shared<FullMatrix<double>>(ListListD{
+		aApm = std::make_shared<FullMatrixDouble>(ListListD{
 			{6.9388939039072e-18, -6.4146353042213e-50, 1.0},
 			{1.0, -6.9388939039072e-18, 6.9388939039072e-18},
 			{-6.9388939039072e-18, 1.0, -7.4837411882581e-50}
diff --git a/OndselSolver/ConstVelConstraintIqcJc.cpp b/OndselSolver/ConstVelConstraintIqcJc.cpp
index 992a85c..bfeaa42 100644
--- a/OndselSolver/ConstVelConstraintIqcJc.cpp
+++ b/OndselSolver/ConstVelConstraintIqcJc.cpp
@@ -97,7 +97,7 @@ void MbD::ConstVelConstraintIqcJc::initialize()
 {
 	ConstVelConstraintIJ::initialize();
 	pGpEI = std::make_shared<FullRow<double>>(4);
-	ppGpEIpEI = std::make_shared<FullMatrix<double>>(4, 4);
+	ppGpEIpEI = std::make_shared<FullMatrixDouble>(4, 4);
 }
 
 void MbD::ConstVelConstraintIqcJc::useEquationNumbers()
diff --git a/OndselSolver/DirectionCosineIeqcJeqc.cpp b/OndselSolver/DirectionCosineIeqcJeqc.cpp
index 52a5535..e01444f 100644
--- a/OndselSolver/DirectionCosineIeqcJeqc.cpp
+++ b/OndselSolver/DirectionCosineIeqcJeqc.cpp
@@ -25,7 +25,7 @@ void DirectionCosineIeqcJeqc::initialize()
 	DirectionCosineIeqcJec::initialize();
 	pAijIeJepEJ = std::make_shared<FullRow<double>>(4);
 	ppAijIeJepEIpEJ = std::make_shared<FullMatrixDouble>(4, 4);
-	ppAijIeJepEJpEJ = std::make_shared<FullMatrix<double>>(4, 4);
+	ppAijIeJepEJpEJ = std::make_shared<FullMatrixDouble>(4, 4);
 }
 
 void DirectionCosineIeqcJeqc::initializeGlobally()
diff --git a/OndselSolver/DispCompIecJecKeqc.cpp b/OndselSolver/DispCompIecJecKeqc.cpp
index efe61f5..b423512 100644
--- a/OndselSolver/DispCompIecJecKeqc.cpp
+++ b/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<FullMatrix<double>>(4, 4);
+    ppriIeJeKepEKpEK = std::make_shared<FullMatrixDouble>(4, 4);
 }
 
 void DispCompIecJecKeqc::initializeGlobally()
diff --git a/OndselSolver/DispCompIeqcJecO.cpp b/OndselSolver/DispCompIeqcJecO.cpp
index 87960a0..bd701fd 100644
--- a/OndselSolver/DispCompIeqcJecO.cpp
+++ b/OndselSolver/DispCompIeqcJecO.cpp
@@ -23,7 +23,7 @@ void DispCompIeqcJecO::initializeGlobally()
 {
 	priIeJeOpXI = std::make_shared<FullRow<double>>(3, 0.0);
 	priIeJeOpXI->at(axis) = -1.0;
-	ppriIeJeOpEIpEI = std::static_pointer_cast<EndFrameqc>(frmI)->ppriOeOpEpE(axis)->negated();
+	ppriIeJeOpEIpEI = toFMDsptr(std::static_pointer_cast<EndFrameqc>(frmI)->ppriOeOpEpE(axis)->negated());
 }
 
 FMatDsptr MbD::DispCompIeqcJecO::ppvaluepEIpEI()
diff --git a/OndselSolver/DispCompIeqcJeqcIe.cpp b/OndselSolver/DispCompIeqcJeqcIe.cpp
index e02461f..9dc5ec1 100644
--- a/OndselSolver/DispCompIeqcJeqcIe.cpp
+++ b/OndselSolver/DispCompIeqcJeqcIe.cpp
@@ -23,7 +23,7 @@ void MbD::DispCompIeqcJeqcIe::calc_ppvaluepEIpEJ()
 {
 	auto frmJeqc = std::static_pointer_cast<EndFrameqc>(frmJ);
 	auto& prIeJeOpEJ = frmJeqc->prOeOpE;
-	ppriIeJeIepEIpEJ = pAjOIepEIT->timesFullMatrix(prIeJeOpEJ);
+	ppriIeJeIepEIpEJ = toFMDsptr(pAjOIepEIT->timesFullMatrix(prIeJeOpEJ));
 }
 
 void MbD::DispCompIeqcJeqcIe::calc_ppvaluepEIpXJ()
diff --git a/OndselSolver/DispCompIeqctJeqcO.cpp b/OndselSolver/DispCompIeqctJeqcO.cpp
index 56b2ec2..90c4d03 100644
--- a/OndselSolver/DispCompIeqctJeqcO.cpp
+++ b/OndselSolver/DispCompIeqctJeqcO.cpp
@@ -39,7 +39,7 @@ void DispCompIeqctJeqcO::calcPostDynCorrectorIteration()
 {
 	//"ppriIeJeOpEIpEI is not a constant now."
 	DispCompIeqcJeqcO::calcPostDynCorrectorIteration();
-	ppriIeJeOpEIpEI = std::static_pointer_cast<EndFrameqct>(frmI)->ppriOeOpEpE(axis)->negated();
+	ppriIeJeOpEIpEI = toFMDsptr(std::static_pointer_cast<EndFrameqct>(frmI)->ppriOeOpEpE(axis)->negated());
 }
 
 void DispCompIeqctJeqcO::preVelIC()
diff --git a/OndselSolver/DistancexyConstraintIqcJc.cpp b/OndselSolver/DistancexyConstraintIqcJc.cpp
index b60d526..1229c74 100644
--- a/OndselSolver/DistancexyConstraintIqcJc.cpp
+++ b/OndselSolver/DistancexyConstraintIqcJc.cpp
@@ -65,18 +65,18 @@ void MbD::DistancexyConstraintIqcJc::calc_pGpXI()
 void MbD::DistancexyConstraintIqcJc::calc_ppGpEIpEI()
 {
 	ppGpEIpEI = (xIeJeIe->pvaluepEI()->transposeTimesFullRow(xIeJeIe->pvaluepEI()));
-	ppGpEIpEI = ppGpEIpEI->plusFullMatrix(xIeJeIe->ppvaluepEIpEI()->times(xIeJeIe->value()));
-	ppGpEIpEI = ppGpEIpEI->plusFullMatrix(yIeJeIe->pvaluepEI()->transposeTimesFullRow(yIeJeIe->pvaluepEI()));
-	ppGpEIpEI = ppGpEIpEI->plusFullMatrix(yIeJeIe->ppvaluepEIpEI()->times(yIeJeIe->value()));
+	ppGpEIpEI = toFMDsptr(ppGpEIpEI->plusFullMatrix(xIeJeIe->ppvaluepEIpEI()->times(xIeJeIe->value())));
+	ppGpEIpEI = toFMDsptr(ppGpEIpEI->plusFullMatrix(yIeJeIe->pvaluepEI()->transposeTimesFullRow(yIeJeIe->pvaluepEI())));
+	ppGpEIpEI = toFMDsptr(ppGpEIpEI->plusFullMatrix(yIeJeIe->ppvaluepEIpEI()->times(yIeJeIe->value())));
 	ppGpEIpEI->magnifySelf(2.0);
 }
 
 void MbD::DistancexyConstraintIqcJc::calc_ppGpXIpEI()
 {
 	ppGpXIpEI = (xIeJeIe->pvaluepXI()->transposeTimesFullRow(xIeJeIe->pvaluepEI()));
-	ppGpXIpEI = ppGpXIpEI->plusFullMatrix(xIeJeIe->ppvaluepXIpEI()->times(xIeJeIe->value()));
-	ppGpXIpEI = ppGpXIpEI->plusFullMatrix(yIeJeIe->pvaluepXI()->transposeTimesFullRow(yIeJeIe->pvaluepEI()));
-	ppGpXIpEI = ppGpXIpEI->plusFullMatrix(yIeJeIe->ppvaluepXIpEI()->times(yIeJeIe->value()));
+	ppGpXIpEI = toFMDsptr(ppGpXIpEI->plusFullMatrix(xIeJeIe->ppvaluepXIpEI()->times(xIeJeIe->value())));
+	ppGpXIpEI = toFMDsptr(ppGpXIpEI->plusFullMatrix(yIeJeIe->pvaluepXI()->transposeTimesFullRow(yIeJeIe->pvaluepEI())));
+	ppGpXIpEI = toFMDsptr(ppGpXIpEI->plusFullMatrix(yIeJeIe->ppvaluepXIpEI()->times(yIeJeIe->value())));
 	ppGpXIpEI->magnifySelf(2.0);
 }
 
@@ -85,7 +85,7 @@ void MbD::DistancexyConstraintIqcJc::calc_ppGpXIpXI()
 	//xIeJeIe ppvaluepXIpXI = 0
 	//yIeJeIe ppvaluepXIpXI = 0
 	ppGpXIpXI = (xIeJeIe->pvaluepXI()->transposeTimesFullRow(xIeJeIe->pvaluepXI()));
-	ppGpXIpXI = ppGpXIpXI->plusFullMatrix(yIeJeIe->pvaluepXI()->transposeTimesFullRow(yIeJeIe->pvaluepXI()));
+	ppGpXIpXI = toFMDsptr(ppGpXIpXI->plusFullMatrix(yIeJeIe->pvaluepXI()->transposeTimesFullRow(yIeJeIe->pvaluepXI())));
 	ppGpXIpXI->magnifySelf(2.0);
 }
 
diff --git a/OndselSolver/DistancexyConstraintIqcJqc.cpp b/OndselSolver/DistancexyConstraintIqcJqc.cpp
index 270c836..2d34f63 100644
--- a/OndselSolver/DistancexyConstraintIqcJqc.cpp
+++ b/OndselSolver/DistancexyConstraintIqcJqc.cpp
@@ -30,16 +30,16 @@ void MbD::DistancexyConstraintIqcJqc::calc_ppGpXIpXJ()
 	//xIeJeIe ppvaluepXIpXJ = 0
 	//yIeJeIe ppvaluepXIpXJ = 0
 	ppGpXIpXJ = (xIeJeIe->pvaluepXI()->transposeTimesFullRow(xIeJeIe->pvaluepXJ()));
-	ppGpXIpXJ = ppGpXIpXJ->plusFullMatrix(yIeJeIe->pvaluepXI()->transposeTimesFullRow(yIeJeIe->pvaluepXJ()));
+	ppGpXIpXJ = toFMDsptr(ppGpXIpXJ->plusFullMatrix(yIeJeIe->pvaluepXI()->transposeTimesFullRow(yIeJeIe->pvaluepXJ())));
 	ppGpXIpXJ->magnifySelf(2.0);
 }
 
 void MbD::DistancexyConstraintIqcJqc::calc_ppGpEIpXJ()
 {
 	ppGpEIpXJ = (xIeJeIe->pvaluepEI()->transposeTimesFullRow(xIeJeIe->pvaluepXJ()));
-	ppGpEIpXJ = ppGpEIpXJ->plusFullMatrix(xIeJeIe->ppvaluepEIpXJ()->times(xIeJeIe->value()));
-	ppGpEIpXJ = ppGpEIpXJ->plusFullMatrix(yIeJeIe->pvaluepEI()->transposeTimesFullRow(yIeJeIe->pvaluepXJ()));
-	ppGpEIpXJ = ppGpEIpXJ->plusFullMatrix(yIeJeIe->ppvaluepEIpXJ()->times(yIeJeIe->value()));
+	ppGpEIpXJ = toFMDsptr(ppGpEIpXJ->plusFullMatrix(xIeJeIe->ppvaluepEIpXJ()->times(xIeJeIe->value())));
+	ppGpEIpXJ = toFMDsptr(ppGpEIpXJ->plusFullMatrix(yIeJeIe->pvaluepEI()->transposeTimesFullRow(yIeJeIe->pvaluepXJ())));
+	ppGpEIpXJ = toFMDsptr(ppGpEIpXJ->plusFullMatrix(yIeJeIe->ppvaluepEIpXJ()->times(yIeJeIe->value())));
 	ppGpEIpXJ->magnifySelf(2.0);
 }
 
@@ -48,7 +48,7 @@ void MbD::DistancexyConstraintIqcJqc::calc_ppGpXJpXJ()
 	//xIeJeIe ppvaluepXJpXJ = 0
 	//yIeJeIe ppvaluepXJpXJ = 0
 	ppGpXJpXJ = (xIeJeIe->pvaluepXJ()->transposeTimesFullRow(xIeJeIe->pvaluepXJ()));
-	ppGpXJpXJ = ppGpXJpXJ->plusFullMatrix(yIeJeIe->pvaluepXJ()->transposeTimesFullRow(yIeJeIe->pvaluepXJ()));
+	ppGpXJpXJ = toFMDsptr(ppGpXJpXJ->plusFullMatrix(yIeJeIe->pvaluepXJ()->transposeTimesFullRow(yIeJeIe->pvaluepXJ())));
 	ppGpXJpXJ->magnifySelf(2.0);
 }
 
@@ -57,16 +57,16 @@ void MbD::DistancexyConstraintIqcJqc::calc_ppGpXIpEJ()
 	//xIeJeIe ppvaluepXIpEJ = 0
 	//yIeJeIe ppvaluepXIpEJ = 0
 	ppGpXIpEJ = (xIeJeIe->pvaluepXI()->transposeTimesFullRow(xIeJeIe->pvaluepEJ()));
-	ppGpXIpEJ = ppGpXIpEJ->plusFullMatrix(yIeJeIe->pvaluepXI()->transposeTimesFullRow(yIeJeIe->pvaluepEJ()));
+	ppGpXIpEJ = toFMDsptr(ppGpXIpEJ->plusFullMatrix(yIeJeIe->pvaluepXI()->transposeTimesFullRow(yIeJeIe->pvaluepEJ())));
 	ppGpXIpEJ->magnifySelf(2.0);
 }
 
 void MbD::DistancexyConstraintIqcJqc::calc_ppGpEIpEJ()
 {
 	ppGpEIpEJ = (xIeJeIe->pvaluepEI()->transposeTimesFullRow(xIeJeIe->pvaluepEJ()));
-	ppGpEIpEJ = ppGpEIpEJ->plusFullMatrix(xIeJeIe->ppvaluepEIpEJ()->times(xIeJeIe->value()));
-	ppGpEIpEJ = ppGpEIpEJ->plusFullMatrix(yIeJeIe->pvaluepEI()->transposeTimesFullRow(yIeJeIe->pvaluepEJ()));
-	ppGpEIpEJ = ppGpEIpEJ->plusFullMatrix(yIeJeIe->ppvaluepEIpEJ()->times(yIeJeIe->value()));
+	ppGpEIpEJ = toFMDsptr(ppGpEIpEJ->plusFullMatrix(xIeJeIe->ppvaluepEIpEJ()->times(xIeJeIe->value())));
+	ppGpEIpEJ = toFMDsptr(ppGpEIpEJ->plusFullMatrix(yIeJeIe->pvaluepEI()->transposeTimesFullRow(yIeJeIe->pvaluepEJ())));
+	ppGpEIpEJ = toFMDsptr(ppGpEIpEJ->plusFullMatrix(yIeJeIe->ppvaluepEIpEJ()->times(yIeJeIe->value())));
 	ppGpEIpEJ->magnifySelf(2.0);
 }
 
@@ -75,16 +75,16 @@ void MbD::DistancexyConstraintIqcJqc::calc_ppGpXJpEJ()
 	//xIeJeIe ppvaluepXJpEJ = 0
 	//yIeJeIe ppvaluepXJpEJ = 0
 	ppGpXJpEJ = (xIeJeIe->pvaluepXJ()->transposeTimesFullRow(xIeJeIe->pvaluepEJ()));
-	ppGpXJpEJ = ppGpXJpEJ->plusFullMatrix(yIeJeIe->pvaluepXJ()->transposeTimesFullRow(yIeJeIe->pvaluepEJ()));
+	ppGpXJpEJ = toFMDsptr(ppGpXJpEJ->plusFullMatrix(yIeJeIe->pvaluepXJ()->transposeTimesFullRow(yIeJeIe->pvaluepEJ())));
 	ppGpXJpEJ->magnifySelf(2.0);
 }
 
 void MbD::DistancexyConstraintIqcJqc::calc_ppGpEJpEJ()
 {
 	ppGpEJpEJ = (xIeJeIe->pvaluepEJ()->transposeTimesFullRow(xIeJeIe->pvaluepEJ()));
-	ppGpEJpEJ = ppGpEJpEJ->plusFullMatrix(xIeJeIe->ppvaluepEJpEJ()->times(xIeJeIe->value()));
-	ppGpEJpEJ = ppGpEJpEJ->plusFullMatrix(yIeJeIe->pvaluepEJ()->transposeTimesFullRow(yIeJeIe->pvaluepEJ()));
-	ppGpEJpEJ = ppGpEJpEJ->plusFullMatrix(yIeJeIe->ppvaluepEJpEJ()->times(yIeJeIe->value()));
+	ppGpEJpEJ = toFMDsptr(ppGpEJpEJ->plusFullMatrix(xIeJeIe->ppvaluepEJpEJ()->times(xIeJeIe->value())));
+	ppGpEJpEJ = toFMDsptr(ppGpEJpEJ->plusFullMatrix(yIeJeIe->pvaluepEJ()->transposeTimesFullRow(yIeJeIe->pvaluepEJ())));
+	ppGpEJpEJ = toFMDsptr(ppGpEJpEJ->plusFullMatrix(yIeJeIe->ppvaluepEJpEJ()->times(yIeJeIe->value())));
 	ppGpEJpEJ->magnifySelf(2.0);
 }
 
diff --git a/OndselSolver/DistxyIeqcJeqc.cpp b/OndselSolver/DistxyIeqcJeqc.cpp
index 92140ff..f422f2d 100644
--- a/OndselSolver/DistxyIeqcJeqc.cpp
+++ b/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<FullMatrix<double>>(3, 3);
-	ppdistxypXIpEJ = std::make_shared<FullMatrix<double>>(3, 4);
-	ppdistxypEIpXJ = std::make_shared<FullMatrix<double>>(4, 3);
-	ppdistxypEIpEJ = std::make_shared<FullMatrix<double>>(4, 4);
-	ppdistxypXJpXJ = std::make_shared<FullMatrix<double>>(3, 3);
-	ppdistxypXJpEJ = std::make_shared<FullMatrix<double>>(3, 4);
-	ppdistxypEJpEJ = std::make_shared<FullMatrix<double>>(4, 4);
+	ppdistxypXIpXJ = std::make_shared<FullMatrixDouble>(3, 3);
+	ppdistxypXIpEJ = std::make_shared<FullMatrixDouble>(3, 4);
+	ppdistxypEIpXJ = std::make_shared<FullMatrixDouble>(4, 3);
+	ppdistxypEIpEJ = std::make_shared<FullMatrixDouble>(4, 4);
+	ppdistxypXJpXJ = std::make_shared<FullMatrixDouble>(3, 3);
+	ppdistxypXJpEJ = std::make_shared<FullMatrixDouble>(3, 4);
+	ppdistxypEJpEJ = std::make_shared<FullMatrixDouble>(4, 4);
 }
 
 FMatDsptr MbD::DistxyIeqcJeqc::ppvaluepEIpEJ()
diff --git a/OndselSolver/EndFramec.cpp b/OndselSolver/EndFramec.cpp
index 84bc5bf..a7c71c9 100644
--- a/OndselSolver/EndFramec.cpp
+++ b/OndselSolver/EndFramec.cpp
@@ -24,7 +24,7 @@ EndFramec::EndFramec(const char* str) : CartesianFrame(str) {
 
 FMatDsptr MbD::EndFramec::aAeO()
 {
-	return aAOe->transpose();
+	return toFMDsptr(aAOe->transpose());
 }
 
 System* EndFramec::root()
diff --git a/OndselSolver/EndFrameqc.cpp b/OndselSolver/EndFrameqc.cpp
index 1837d88..61619f4 100644
--- a/OndselSolver/EndFrameqc.cpp
+++ b/OndselSolver/EndFrameqc.cpp
@@ -28,7 +28,7 @@ void EndFrameqc::initialize()
 	prOeOpE = std::make_shared<FullMatrixDouble>(3, 4);
 	pprOeOpEpE = std::make_shared<FullMatrix<FColDsptr>>(4, 4);
 	pAOepE = std::make_shared<FullColumn<FMatDsptr>>(4);
-	ppAOepEpE = std::make_shared<FullMatrix<FMatDsptr>>(4, 4);
+	ppAOepEpE = std::make_shared<FullMatrixFullMatrixDouble>(4, 4);
 }
 
 void EndFrameqc::initializeGlobally()
diff --git a/OndselSolver/EndFrameqct.cpp b/OndselSolver/EndFrameqct.cpp
index 69020d3..ba8ba1f 100644
--- a/OndselSolver/EndFrameqct.cpp
+++ b/OndselSolver/EndFrameqct.cpp
@@ -32,12 +32,12 @@ void EndFrameqct::initialize()
 	prmempt = std::make_shared<FullColumn<double>>(3);
 	pprmemptpt = std::make_shared<FullColumn<double>>(3);
 	aAme = FullMatrixDouble::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);
+	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<FullMatrix<double>>(3, 3);
+	ppAOeptpt = std::make_shared<FullMatrixDouble>(3, 3);
 }
 
 void EndFrameqct::initializeLocally()
@@ -142,12 +142,12 @@ void EndFrameqct::calcPostDynCorrectorIteration()
 	}
 	auto rpep = markerFrame->rpmp->plusFullColumn(markerFrame->aApm->timesFullColumn(rmem));
 	pprOeOpEpE = EulerParameters<double>::ppApEpEtimesColumn(rpep);
-	aAOe = aAOm->timesFullMatrix(aAme);
+	aAOe = toFMDsptr(aAOm->timesFullMatrix(aAme));
 	for (int i = 0; i < 4; i++)
 	{
-		pAOepE->at(i) = pAOmpE->at(i)->timesFullMatrix(aAme);
+		pAOepE->at(i) = toFMDsptr(pAOmpE->at(i)->timesFullMatrix(aAme));
 	}
-	auto aApe = markerFrame->aApm->timesFullMatrix(aAme);
+	auto aApe = toFMDsptr(markerFrame->aApm->timesFullMatrix(aAme));
 	ppAOepEpE = EulerParameters<double>::ppApEpEtimesMatrix(aApe);
 }
 
@@ -196,7 +196,7 @@ void EndFrameqct::preVelIC()
 	this->evalpAmept();
 	auto& aAOm = markerFrame->aAOm;
 	prOeOpt = aAOm->timesFullColumn(prmempt);
-	pAOept = aAOm->timesFullMatrix(pAmept);
+	pAOept = toFMDsptr(aAOm->timesFullMatrix(pAmept));
 }
 
 void EndFrameqct::postVelIC()
@@ -213,7 +213,7 @@ void EndFrameqct::postVelIC()
 	}
 	for (int i = 0; i < 4; i++)
 	{
-		ppAOepEpt->atiput(i, pAOmpE->at(i)->timesFullMatrix(pAmept));
+		ppAOepEpt->atiput(i, toFMDsptr(pAOmpE->at(i)->timesFullMatrix(pAmept)));
 	}
 }
 
@@ -224,7 +224,7 @@ FColDsptr EndFrameqct::pAjOept(int j)
 
 FMatDsptr EndFrameqct::ppAjOepETpt(int jj)
 {
-	auto answer = std::make_shared<FullMatrix<double>>(4, 3);
+	auto answer = std::make_shared<FullMatrixDouble>(4, 3);
 	for (int i = 0; i < 4; i++)
 	{
 		auto& answeri = answer->at(i);
@@ -326,7 +326,7 @@ void EndFrameqct::evalppAmeptpt()
 		phiThePsi->calc();
 		phiThePsiDot->calc();
 		phiThePsiDDot->calc();
-		ppAmeptpt = phiThePsiDDot->aAddot;
+		ppAmeptpt = toFMDsptr(phiThePsiDDot->aAddot);
 	}
 }
 
@@ -353,11 +353,11 @@ void EndFrameqct::preAccIC()
 	this->evalpAmept();
 	auto& aAOm = markerFrame->aAOm;
 	prOeOpt = aAOm->timesFullColumn(prmempt);
-	pAOept = aAOm->timesFullMatrix(pAmept);
+	pAOept = toFMDsptr(aAOm->timesFullMatrix(pAmept));
 	Item::preAccIC();
 	this->evalpprmemptpt();
 	this->evalppAmeptpt();
 	aAOm = markerFrame->aAOm;
 	pprOeOptpt = aAOm->timesFullColumn(pprmemptpt);
-	ppAOeptpt = aAOm->timesFullMatrix(ppAmeptpt);
+	ppAOeptpt = toFMDsptr(aAOm->timesFullMatrix(ppAmeptpt));
 }
diff --git a/OndselSolver/EulerAngles.h b/OndselSolver/EulerAngles.h
index 4d1ab62..030a493 100644
--- a/OndselSolver/EulerAngles.h
+++ b/OndselSolver/EulerAngles.h
@@ -51,19 +51,19 @@ namespace MbD {
 			auto axis = rotOrder->at(i);
 			auto angle = this->at(i)->getValue();
 			if (axis == 1) {
-				cA->atiput(i, FullMatrixDouble::rotatex(angle));
+				cA->atiput(i, toFMDsptr(FullMatrixDouble::rotatex(angle)));
 			}
 			else if (axis == 2) {
-				cA->atiput(i, FullMatrixDouble::rotatey(angle));
+				cA->atiput(i, toFMDsptr(FullMatrixDouble::rotatey(angle)));
 			}
 			else if (axis == 3) {
-				cA->atiput(i, FullMatrixDouble::rotatez(angle));
+				cA->atiput(i, toFMDsptr(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)));
+		aA = toFMDsptr(cA->at(0)->timesFullMatrix(cA->at(1)->timesFullMatrix(cA->at(2))));
 	}
 	template<>
 	inline void EulerAngles<double>::calc()
@@ -74,19 +74,19 @@ namespace MbD {
 			auto axis = rotOrder->at(i);
 			auto angle = this->at(i);
 			if (axis == 1) {
-				cA->atiput(i, FullMatrixDouble::rotatex(angle));
+				cA->atiput(i, toFMDsptr(FullMatrixDouble::rotatex(angle)));
 			}
 			else if (axis == 2) {
-				cA->atiput(i, FullMatrixDouble::rotatey(angle));
+				cA->atiput(i, toFMDsptr(FullMatrixDouble::rotatey(angle)));
 			}
 			else if (axis == 3) {
-				cA->atiput(i, FullMatrix<double>::rotatez(angle));
+				cA->atiput(i, toFMDsptr(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)));
+		aA = toFMDsptr(cA->at(0)->timesFullMatrix(cA->at(1)->timesFullMatrix(cA->at(2))));
 	}
 	template<typename T>
 	inline void EulerAngles<T>::calc()
diff --git a/OndselSolver/EulerAnglesDDot.h b/OndselSolver/EulerAnglesDDot.h
index 6dc561a..547fc2c 100644
--- a/OndselSolver/EulerAnglesDDot.h
+++ b/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, toFMDsptr(FullMatrixDouble::rotatexrotDotrotDDot(angle, angleDot, angleDDot)));
 			}
 			else if (axis == 2) {
-				cAddot->atiput(i, FullMatrixDouble::rotateyrotDotrotDDot(angle, angleDot, angleDDot));
+				cAddot->atiput(i, toFMDsptr(FullMatrixDouble::rotateyrotDotrotDDot(angle, angleDot, angleDDot)));
 			}
 			else if (axis == 3) {
-				cAddot->atiput(i, FullMatrixDouble::rotatezrotDotrotDDot(angle, angleDot, angleDDot));
+				cAddot->atiput(i, toFMDsptr(FullMatrixDouble::rotatezrotDotrotDDot(angle, angleDot, angleDDot)));
 			}
 			else {
 				throw std::runtime_error("Euler angle rotation order must be any permutation of 1,2,3 without consecutive repeats.");
@@ -79,8 +79,10 @@ namespace MbD {
 		auto term7 = phiA->timesFullMatrix(theAdot->timesFullMatrix(psiAdot));
 		auto term8 = phiA->timesFullMatrix(theA->timesFullMatrix(psiAddot));
 
-		aAddot = term->plusFullMatrix(term1)->plusFullMatrix(term2)->plusFullMatrix(term3)->plusFullMatrix(term4)
-			->plusFullMatrix(term5)->plusFullMatrix(term6)->plusFullMatrix(term7)->plusFullMatrix(term8);
+		aAddot = toFMDsptr(term->plusFullMatrix(term1)->plusFullMatrix(term2)
+                ->plusFullMatrix(term3)->plusFullMatrix(term4)
+			    ->plusFullMatrix(term5)->plusFullMatrix(term6)
+                ->plusFullMatrix(term7)->plusFullMatrix(term8));
 	}
 	template<typename T>
 	inline void EulerAnglesDDot<T>::aEulerAngles(EulerAngles<T>* eulerAngles)
diff --git a/OndselSolver/EulerAnglesDot.h b/OndselSolver/EulerAnglesDot.h
index 27bd936..1200506 100644
--- a/OndselSolver/EulerAnglesDot.h
+++ b/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, toFMDsptr(FullMatrixDouble::rotatexrotDot(angle, angleDot)));
 			}
 			else if (axis == 2) {
-				cAdot->atiput(i, FullMatrixDouble::rotateyrotDot(angle, angleDot));
+				cAdot->atiput(i, toFMDsptr(FullMatrixDouble::rotateyrotDot(angle, angleDot)));
 			}
 			else if (axis == 3) {
-				cAdot->atiput(i, FullMatrixDouble::rotatezrotDot(angle, angleDot));
+				cAdot->atiput(i, toFMDsptr(FullMatrixDouble::rotatezrotDot(angle, angleDot)));
 			}
 			else {
 				throw std::runtime_error("Euler angle rotation order must be any permutation of 1,2,3 without consecutive repeats.");
@@ -78,12 +78,12 @@ namespace MbD {
 		auto theAdot = cAdot->at(1);
 		auto psiAdot = cAdot->at(2);
 
-		aAdot = phiAdot->timesFullMatrix(theA->timesFullMatrix(psiA))
-			->plusFullMatrix(phiA->timesFullMatrix(theAdot->timesFullMatrix(psiA)))
-			->plusFullMatrix(phiA->timesFullMatrix(theA->timesFullMatrix(psiAdot)));
+		aAdot = toFMDsptr(phiAdot->timesFullMatrix(theA->timesFullMatrix(psiA))
+                ->plusFullMatrix(phiA->timesFullMatrix(theAdot->timesFullMatrix(psiA)))
+                ->plusFullMatrix(phiA->timesFullMatrix(theA->timesFullMatrix(psiAdot))));
 		omeF = (phiA->column(0)->times(phidot)
-			->plusFullColumn(phiA->timesFullMatrix(theA)->column(1)->times(thedot))
-			->plusFullColumn(aEulerAngles->aA->column(2)->times(psidot)));
+                ->plusFullColumn(phiA->timesFullMatrix(theA)->column(1)->times(thedot))
+                ->plusFullColumn(aEulerAngles->aA->column(2)->times(psidot)));
 		omef = aEulerAngles->aA->transposeTimesFullColumn(omeF);
 	}
 }
diff --git a/OndselSolver/EulerAngleszxz.h b/OndselSolver/EulerAngleszxz.h
index 55dc48f..01404f5 100644
--- a/OndselSolver/EulerAngleszxz.h
+++ b/OndselSolver/EulerAngleszxz.h
@@ -29,9 +29,9 @@ namespace MbD {
 	template<typename T>
 	inline void EulerAngleszxz<T>::initialize()
 	{
-		phiA = FullMatrix<T>::identitysptr(3);
-		theA = FullMatrix<T>::identitysptr(3);
-		psiA = FullMatrix<T>::identitysptr(3);
+		phiA = FullMatrixDouble::identitysptr(3);
+		theA = FullMatrixDouble::identitysptr(3);
+		psiA = FullMatrixDouble::identitysptr(3);
 	}
 	template<typename T>
 	inline void EulerAngleszxz<T>::calc()
@@ -67,7 +67,7 @@ namespace MbD {
 		psiAi = psiA->at(1);
 		psiAi->at(0) = spsi;
 		psiAi->at(1) = cpsi;
-		aA = phiA->timesFullMatrix(theA->timesFullMatrix(psiA));
+		aA = toFMDsptr(phiA->timesFullMatrix(theA->timesFullMatrix(psiA)));
 	}
 }
 
diff --git a/OndselSolver/EulerAngleszxzDDot.h b/OndselSolver/EulerAngleszxzDDot.h
index 69bc344..70c4b3d 100644
--- a/OndselSolver/EulerAngleszxzDDot.h
+++ b/OndselSolver/EulerAngleszxzDDot.h
@@ -8,6 +8,7 @@
  
 #pragma once
 
+#include "FullMatrix.h"
 #include "EulerArray.h"
 #include "EulerAngleszxzDot.h"
 
@@ -23,16 +24,17 @@ namespace MbD {
         void calc() override;
 
         std::shared_ptr<EulerAngleszxzDot<double>> phiThePsiDot;
-        FMatDsptr phiAddot, theAddot, psiAddot, aAddot;
+        FMatDsptr phiAddot, theAddot, psiAddot;
+        std::shared_ptr<FullMatrix<double>> aAddot;
     };
     template<typename T>
     inline void EulerAngleszxzDDot<T>::initialize()
     {
-        phiAddot = std::make_shared<FullMatrix<double>>(3, 3);
+        phiAddot = std::make_shared<FullMatrixDouble>(3, 3);
         phiAddot->zeroSelf();
-        theAddot = std::make_shared<FullMatrix<double>>(3, 3);
+        theAddot = std::make_shared<FullMatrixDouble>(3, 3);
         theAddot->zeroSelf();
-        psiAddot = std::make_shared<FullMatrix<double>>(3, 3);
+        psiAddot = std::make_shared<FullMatrixDouble>(3, 3);
         psiAddot->zeroSelf();
     }
     template<typename T>
diff --git a/OndselSolver/EulerAngleszxzDot.h b/OndselSolver/EulerAngleszxzDot.h
index 03f56b7..f7c333f 100644
--- a/OndselSolver/EulerAngleszxzDot.h
+++ b/OndselSolver/EulerAngleszxzDot.h
@@ -74,7 +74,7 @@ namespace MbD {
 		auto term1 = phiAdot->timesFullMatrix(theA->timesFullMatrix(psiA));
 		auto term2 = phiA->timesFullMatrix(theAdot->timesFullMatrix(psiA));
 		auto term3 = phiA->timesFullMatrix(theA->timesFullMatrix(psiAdot));
-		aAdot = (term1->plusFullMatrix(term2))->plusFullMatrix(term3);
+		aAdot = toFMDsptr((term1->plusFullMatrix(term2))->plusFullMatrix(term3));
 	}
 }
 
diff --git a/OndselSolver/EulerParameters.cpp b/OndselSolver/EulerParameters.cpp
index 7590fc1..27ac4f1 100644
--- a/OndselSolver/EulerParameters.cpp
+++ b/OndselSolver/EulerParameters.cpp
@@ -128,14 +128,14 @@ inline FMatFMatDsptr EulerParameters<double>::ppApEpEtimesMatrix(FMatDsptr mat)
     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<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 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<FullMatrix<FMatDsptr>>(4, 4);
     auto& row0 = answer->at(0);
     row0->at(0) = mat00;
@@ -157,19 +157,19 @@ inline FMatFMatDsptr EulerParameters<double>::ppApEpEtimesMatrix(FMatDsptr mat)
     row3->at(1) = mat13;
     row3->at(2) = mat23;
     row3->at(3) = mat33;
-    return answer;
+    return toFMFMDsptr(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);
+    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<FullMatrix<double>>(3, 3);
+        pApE->at(i) = std::make_shared<FullMatrixDouble>(3, 3);
     }
 }
 template<typename T>
diff --git a/OndselSolver/EulerParameters.h b/OndselSolver/EulerParameters.h
index 887ed5d..1a320ec 100644
--- a/OndselSolver/EulerParameters.h
+++ b/OndselSolver/EulerParameters.h
@@ -44,7 +44,7 @@ namespace MbD {
 		static std::shared_ptr<FullMatrix<FColsptr<T>>> ppApEpEtimesColumn(FColDsptr col);
 		static FMatDsptr pCpEtimesColumn(FColDsptr col);
 		static FMatDsptr pCTpEtimesColumn(FColDsptr col);
-		static std::shared_ptr<FullMatrix<FMatsptr<T>>> ppApEpEtimesMatrix(FMatDsptr mat);
+		static std::shared_ptr<FullMatrixFullMatrixDouble> ppApEpEtimesMatrix(FMatDsptr mat);
 
 
 		void initialize() override;
diff --git a/OndselSolver/EulerParametersDot.h b/OndselSolver/EulerParametersDot.h
index 46d980f..2423cc4 100644
--- a/OndselSolver/EulerParametersDot.h
+++ b/OndselSolver/EulerParametersDot.h
@@ -55,13 +55,13 @@ namespace MbD {
 	template<typename T>
 	inline void EulerParametersDot<T>::initialize()
 	{
-		aAdot = std::make_shared<FullMatrix<double>>(3, 3);
-		aBdot = std::make_shared<FullMatrix<double>>(3, 4);
-		aCdot = std::make_shared<FullMatrix<double>>(3, 4);
+		aAdot = std::make_shared<FullMatrixDouble>(3, 3);
+		aBdot = std::make_shared<FullMatrixDouble>(3, 4);
+		aCdot = std::make_shared<FullMatrixDouble>(3, 4);
 		pAdotpE = std::make_shared<FullColumn<FMatDsptr>>(4);
 		for (int i = 0; i < 4; i++)
 		{
-			pAdotpE->at(i) = std::make_shared<FullMatrix<double>>(3, 3);
+			pAdotpE->at(i) = std::make_shared<FullMatrixDouble>(3, 3);
 		}
 	}
 
diff --git a/OndselSolver/FullMatrix.cpp b/OndselSolver/FullMatrix.cpp
index e69de29..1e07729 100644
--- a/OndselSolver/FullMatrix.cpp
+++ b/OndselSolver/FullMatrix.cpp
@@ -0,0 +1,698 @@
+/***************************************************************************
+ *   Copyright (c) 2023 Ondsel, Inc.                                       *
+ *                                                                         *
+ *   This file is part of OndselSolver.                                    *
+ *                                                                         *
+ *   See LICENSE file for details about copyright.                         *
+ ***************************************************************************/
+
+#include "FullMatrix.h"
+#include "FullColumn.h"
+#include "FullRow.h"
+#include "DiagonalMatrix.h"
+#include "EulerParameters.h"
+
+using namespace MbD;
+
+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;
+}
+inline std::shared_ptr<MbD::FullMatrixDouble> FullMatrixDouble::identitysptr(int n)
+{
+    auto mat = std::make_shared<FullMatrixDouble>(n, n);
+    mat->identity();
+    return mat;
+}
+inline std::shared_ptr<MbD::FullMatrixFullMatrixDouble> FullMatrixFullMatrixDouble::identitysptr(int n)
+{
+    auto mat = std::make_shared<FullMatrixFullMatrixDouble>(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<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;
+}
+template<>
+inline void FullMatrix<double>::zeroSelf()
+{
+    for (int i = 0; i < this->size(); i++) {
+        this->at(i)->zeroSelf();
+    }
+}
+template<typename T>
+inline void FullMatrix<T>::identity() {
+    assert(false);
+}
+inline void FullMatrixDouble::identity() {
+    this->zeroSelf();
+    for (int i = 0; i < this->size(); i++) {
+        this->at(i)->at(i) = 1.0;
+    }
+}
+inline void FullMatrixFullMatrixDouble::identity() {
+    assert(false);
+//    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)
+{
+    assert(false);
+//    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;
+}
+inline 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;
+}
+inline 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;
+}
+inline 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;
+}
+inline std::shared_ptr<FullMatrixFullMatrixDouble> FullMatrixFullMatrixDouble::times(double a)
+{
+    assert(false);
+}
+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;
+}
+inline double FullMatrixDouble::sumOfSquares()
+{
+    double sum = 0.0;
+    for (int i = 0; i < this->size(); i++)
+    {
+        sum += this->at(i)->sumOfSquares();
+    }
+    return sum;
+}
+inline double FullMatrixFullMatrixDouble::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 = M_PI / 2.0;
+            the2z = 0.0;
+        }
+        else {
+            the0x = std::atan2(this->at(2)->at(1), this->at(2)->at(0));
+            the1y = 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;
+}
diff --git a/OndselSolver/FullMatrix.h b/OndselSolver/FullMatrix.h
index 1ae4f85..ad94b0b 100644
--- a/OndselSolver/FullMatrix.h
+++ b/OndselSolver/FullMatrix.h
@@ -17,6 +17,7 @@
 #include "DiagonalMatrix.ref.h"
 #include "EulerParameters.ref.h"
 #include "RowTypeMatrix.h"
+#include "FullRow.h" // exception to normal include pattern
 
 namespace MbD {
     template<typename T>
@@ -29,8 +30,8 @@ namespace MbD {
 	class FullMatrix : public RowTypeMatrix<FRowsptr<T>>
 	{
 	public:
-		FullMatrix() {}
-		FullMatrix(int m) : RowTypeMatrix<FRowsptr<T>>(m)
+		FullMatrix() {};
+		explicit FullMatrix(int m) : RowTypeMatrix<FRowsptr<T>>(m)
 		{
 		}
 		FullMatrix(int m, int n) {
@@ -63,13 +64,16 @@ namespace MbD {
 		static FMatsptr<T> rotatezrotDotrotDDot(T angle, T angleDot, T angleDDot);
 		static FMatsptr<T> identitysptr(int n);
 		static FMatsptr<T> tildeMatrix(FColDsptr col);
-		void identity();
+
+        virtual 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);
+
+        virtual 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);
@@ -100,12 +104,54 @@ namespace MbD {
 		std::ostream& printOn(std::ostream& s) const override;
 	};
 
-    class FullMatrixDouble : public FullMatrix<double> {};
-    using FMatDsptr = std::shared_ptr<MbD::FullMatrixDouble>;
+    //
+    // FULL MATRIX DOUBLE instantiation
+    //
+    class FullMatrixDouble : public FullMatrix<double> {
+    public:
+        FullMatrixDouble() : FullMatrix<double>() {};
+        explicit FullMatrixDouble(int m) : FullMatrix<double>(m) {};
+        FullMatrixDouble(int m, int n) : FullMatrix<double>(m, n) {};
+        FullMatrixDouble(std::initializer_list<std::initializer_list<double>> list2D) : FullMatrix<double>(list2D) {}
+        FullMatrixDouble(std::initializer_list<FRowsptr<double>> listOfRows) : FullMatrix<double>(listOfRows) {};
 
-    using FMatFColDsptr = std::shared_ptr<FullMatrix<FColDsptr>>;
-    using FMatFMatDsptr = std::shared_ptr<FullMatrix<FMatDsptr>>;
+        std::shared_ptr<FullMatrixDouble> times(double a);
+        std::shared_ptr<FullMatrixDouble> timesTransposeFullMatrix(std::shared_ptr<FullMatrixDouble> fullMat);
+        double sumOfSquares() override;
+        void identity() override;
+        static std::shared_ptr<MbD::FullMatrixDouble> identitysptr(int n);
+    };
+    using FMatDsptr = std::shared_ptr<MbD::FullMatrixDouble>;
+    std::shared_ptr<FullMatrixDouble> toFMDsptr(FMatsptr<double> s) {
+        return std::static_pointer_cast<FullMatrixDouble>(s);
+    }
+
+    //
+    // FULL MATRIX FULL MATRIX DOUBLE instantiation
+    //
+    class FullMatrixFullMatrixDouble : public FullMatrix<FMatDsptr> {
+    public:
+        FullMatrixFullMatrixDouble() : FullMatrix<FMatDsptr>() {};
+        explicit FullMatrixFullMatrixDouble(int m) : FullMatrix<FMatDsptr>(m) {};
+        FullMatrixFullMatrixDouble(int m, int n) : FullMatrix<FMatDsptr>(m, n) {};
+
+        std::shared_ptr<FullMatrixFullMatrixDouble> times(double a);
+        std::shared_ptr<FullMatrixFullMatrixDouble> timesTransposeFullMatrix(std::shared_ptr<FullMatrixFullMatrixDouble> fullMat);
+        double sumOfSquares() override;
+        void identity() override;
+        static std::shared_ptr<MbD::FullMatrixFullMatrixDouble> identitysptr(int n);
+
+
+
+    };
+    using FMatFMatDsptr = std::shared_ptr<FullMatrixFullMatrixDouble>;
+    std::shared_ptr<FullMatrixFullMatrixDouble> toFMFMDsptr(std::shared_ptr<FullMatrix<FMatDsptr>> s) {
+        return std::static_pointer_cast<FullMatrixFullMatrixDouble>(s);
+    }
 
     using FColFMatDsptr = std::shared_ptr<FullColumn<FMatDsptr>>;
+    using FMatFColDsptr = std::shared_ptr<FullMatrix<FColDsptr>>;
+
+
 }
 
diff --git a/OndselSolver/FullMatrix.ref.h b/OndselSolver/FullMatrix.ref.h
index 1cb29fa..f250568 100644
--- a/OndselSolver/FullMatrix.ref.h
+++ b/OndselSolver/FullMatrix.ref.h
@@ -4,6 +4,7 @@
 
 namespace MbD {
     class FullMatrixDouble;
+    class FullMatrixFullMatrixDouble;
 
 //    using FMatDsptr = std::shared_ptr<MbD::FullMatrixDouble>;
 
diff --git a/OndselSolver/FullRow.cpp b/OndselSolver/FullRow.cpp
index d87387b..6648076 100644
--- a/OndselSolver/FullRow.cpp
+++ b/OndselSolver/FullRow.cpp
@@ -12,7 +12,7 @@
 using namespace MbD;
 
 template<typename T>
-std::shared_ptr<FullMatrixDouble> FullRow<T>::transposeTimesFullRow(std::shared_ptr<FullMatrixDouble> fullRow)
+std::shared_ptr<FullMatrixDouble> FullRow<T>::transposeTimesFullRow(FRowsptr<T> fullRow)
 {
     //"a*b = a(i)b(j)"
     auto nrow = (int)this->size();
@@ -36,6 +36,18 @@ inline FRowsptr<T> FullRow<T>::timesTransposeFullMatrix(std::shared_ptr<FullMatr
     return answer;
 }
 
+template<typename T>
+inline FRowsptr<T> FullRow<T>::timesTransposeFullMatrixForFMFMDsptr(std::shared_ptr<FullMatrixFullMatrixDouble> fullMat)
+{
+    //"a*bT = a(1,j)b(k,j)"
+    int ncol = fullMat->nrow();
+    auto answer = std::make_shared<FullRow<T>>(ncol);
+    for (int k = 0; k < ncol; k++) {
+        answer->at(k) = this->dot(fullMat->at(k));
+    }
+    return answer;
+}
+
 template<typename T>
 inline FRowsptr<T> FullRow<T>::timesFullMatrix(std::shared_ptr<FullMatrixDouble> fullMat)
 {
@@ -45,5 +57,15 @@ inline FRowsptr<T> FullRow<T>::timesFullMatrix(std::shared_ptr<FullMatrixDouble>
         answer->equalSelfPlusFullRowTimes(fullMat->at(j), this->at(j));
     }
     return answer;
-    //return FRowsptr<T>();
+}
+
+template<typename T>
+inline FRowsptr<T> FullRow<T>::timesFullMatrixForFMFMDsptr(std::shared_ptr<FullMatrixFullMatrixDouble> 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;
 }
diff --git a/OndselSolver/FullRow.h b/OndselSolver/FullRow.h
index 78beb8d..75d1935 100644
--- a/OndselSolver/FullRow.h
+++ b/OndselSolver/FullRow.h
@@ -30,17 +30,21 @@ namespace MbD {
 		FRowsptr<T> minusFullRow(FRowsptr<T> fullRow);
 		T timesFullColumn(FColsptr<T> fullCol);
 		T timesFullColumn(FullColumn<T>* fullCol);
-		FRowsptr<T> timesFullMatrix(std::shared_ptr<FullMatrixDouble> fullMat);
-		FRowsptr<T> timesTransposeFullMatrix(std::shared_ptr<FullMatrixDouble> fullMat);
 		void equalSelfPlusFullRowTimes(FRowsptr<T> fullRow, double factor);
 		void equalFullRow(FRowsptr<T> fullRow);
 		FColsptr<T> transpose();
 		FRowsptr<T> copy();
 		void atiplusFullRow(int j, FRowsptr<T> fullRow);
-        std::shared_ptr<FullMatrixDouble> transposeTimesFullRow(std::shared_ptr<FullMatrixDouble> fullRow);
 		std::ostream& printOn(std::ostream& s) const override;
 
-	};
+        FRowsptr<T> timesFullMatrix(std::shared_ptr<FullMatrixDouble> fullMat);
+        FRowsptr<T> timesTransposeFullMatrix(std::shared_ptr<FullMatrixDouble> fullMat);
+        std::shared_ptr<FullMatrixDouble> transposeTimesFullRow(FRowsptr<T> fullRow);
+
+        FRowsptr<T> timesFullMatrixForFMFMDsptr(std::shared_ptr<FullMatrixFullMatrixDouble> fullMat);
+        FRowsptr<T> timesTransposeFullMatrixForFMFMDsptr(std::shared_ptr<FullMatrixFullMatrixDouble> fullMat);
+        // std::shared_ptr<FullMatrixFullMatrixDouble> transposeTimesFullRow(FRowsptr<T> fullRow);
+    };
 
 	template<>
 	inline FRowDsptr FullRow<double>::times(double a)
diff --git a/OndselSolver/GearConstraintIqcJc.cpp b/OndselSolver/GearConstraintIqcJc.cpp
index cd348ff..a5cff8c 100644
--- a/OndselSolver/GearConstraintIqcJc.cpp
+++ b/OndselSolver/GearConstraintIqcJc.cpp
@@ -52,17 +52,18 @@ void MbD::GearConstraintIqcJc::calc_pGpXI()
 
 void MbD::GearConstraintIqcJc::calc_ppGpEIpEI()
 {
-	ppGpEIpEI = orbitJeIe->ppvaluepEJpEJ()->plusFullMatrix(orbitIeJe->ppvaluepEIpEI()->times(this->ratio()));
+	ppGpEIpEI = toFMDsptr(orbitJeIe->ppvaluepEJpEJ()->plusFullMatrix(orbitIeJe->ppvaluepEIpEI()->times(this->ratio())));
 }
 
 void MbD::GearConstraintIqcJc::calc_ppGpXIpEI()
 {
-	ppGpXIpEI = orbitJeIe->ppvaluepXJpEJ()->plusFullMatrix(orbitIeJe->ppvaluepXIpEI()->times(this->ratio()));
+	ppGpXIpEI = toFMDsptr(orbitJeIe->ppvaluepXJpEJ()->plusFullMatrix(orbitIeJe->ppvaluepXIpEI()->times(this->ratio())));
 }
 
 void MbD::GearConstraintIqcJc::calc_ppGpXIpXI()
 {
-	ppGpXIpXI = orbitJeIe->ppvaluepXJpXJ()->plusFullMatrix(orbitIeJe->ppvaluepXIpXI()->times(this->ratio()));
+	ppGpXIpXI = toFMDsptr(orbitJeIe->ppvaluepXJpXJ()
+            ->plusFullMatrix(orbitIeJe->ppvaluepXIpXI()->times(this->ratio())));
 }
 
 void MbD::GearConstraintIqcJc::calcPostDynCorrectorIteration()
diff --git a/OndselSolver/GearConstraintIqcJqc.cpp b/OndselSolver/GearConstraintIqcJqc.cpp
index 717c815..418f198 100644
--- a/OndselSolver/GearConstraintIqcJqc.cpp
+++ b/OndselSolver/GearConstraintIqcJqc.cpp
@@ -29,37 +29,37 @@ void MbD::GearConstraintIqcJqc::calc_pGpXJ()
 
 void MbD::GearConstraintIqcJqc::calc_ppGpEIpEJ()
 {
-	ppGpEIpEJ = orbitJeIe->ppvaluepEIpEJ()->transpose()->plusFullMatrix(orbitIeJe->ppvaluepEIpEJ()->times(this->ratio()));
+	ppGpEIpEJ = toFMDsptr(orbitJeIe->ppvaluepEIpEJ()->transpose()->plusFullMatrix(orbitIeJe->ppvaluepEIpEJ()->times(this->ratio())));
 }
 
 void MbD::GearConstraintIqcJqc::calc_ppGpEIpXJ()
 {
-	ppGpEIpXJ = orbitJeIe->ppvaluepXIpEJ()->transpose()->plusFullMatrix(orbitIeJe->ppvaluepEIpXJ()->times(this->ratio()));
+	ppGpEIpXJ = toFMDsptr(orbitJeIe->ppvaluepXIpEJ()->transpose()->plusFullMatrix(orbitIeJe->ppvaluepEIpXJ()->times(this->ratio())));
 }
 
 void MbD::GearConstraintIqcJqc::calc_ppGpEJpEJ()
 {
-	ppGpEJpEJ = orbitJeIe->ppvaluepEIpEI()->plusFullMatrix(orbitIeJe->ppvaluepEJpEJ()->times(this->ratio()));
+	ppGpEJpEJ = toFMDsptr(orbitJeIe->ppvaluepEIpEI()->plusFullMatrix(orbitIeJe->ppvaluepEJpEJ()->times(this->ratio())));
 }
 
 void MbD::GearConstraintIqcJqc::calc_ppGpXIpEJ()
 {
-	ppGpXIpEJ = orbitJeIe->ppvaluepEIpXJ()->transpose()->plusFullMatrix(orbitIeJe->ppvaluepXIpEJ()->times(this->ratio()));
+	ppGpXIpEJ = toFMDsptr(orbitJeIe->ppvaluepEIpXJ()->transpose()->plusFullMatrix(orbitIeJe->ppvaluepXIpEJ()->times(this->ratio())));
 }
 
 void MbD::GearConstraintIqcJqc::calc_ppGpXIpXJ()
 {
-	ppGpXIpXJ = orbitJeIe->ppvaluepXIpXJ()->transpose()->plusFullMatrix(orbitIeJe->ppvaluepXIpXJ()->times(this->ratio()));
+	ppGpXIpXJ = toFMDsptr(orbitJeIe->ppvaluepXIpXJ()->transpose()->plusFullMatrix(orbitIeJe->ppvaluepXIpXJ()->times(this->ratio())));
 }
 
 void MbD::GearConstraintIqcJqc::calc_ppGpXJpEJ()
 {
-	ppGpXJpEJ = orbitJeIe->ppvaluepXIpEI()->plusFullMatrix(orbitIeJe->ppvaluepXJpEJ()->times(this->ratio()));
+	ppGpXJpEJ = toFMDsptr(orbitJeIe->ppvaluepXIpEI()->plusFullMatrix(orbitIeJe->ppvaluepXJpEJ()->times(this->ratio())));
 }
 
 void MbD::GearConstraintIqcJqc::calc_ppGpXJpXJ()
 {
-	ppGpXJpXJ = orbitJeIe->ppvaluepXIpXI()->plusFullMatrix(orbitIeJe->ppvaluepXJpXJ()->times(this->ratio()));
+	ppGpXJpXJ = toFMDsptr(orbitJeIe->ppvaluepXIpXI()->plusFullMatrix(orbitIeJe->ppvaluepXJpXJ()->times(this->ratio())));
 }
 
 void MbD::GearConstraintIqcJqc::calcPostDynCorrectorIteration()
diff --git a/OndselSolver/Item.h b/OndselSolver/Item.h
index 79a5690..dd1a6c1 100644
--- a/OndselSolver/Item.h
+++ b/OndselSolver/Item.h
@@ -12,7 +12,7 @@
 #include <vector>
 
 #include "FullColumn.h"
-#include "FullMatrix.h"
+// #include "FullMatrix.h"
 #include "DiagonalMatrix.h"
 #include "SparseMatrix.h"
 #include "enum.h"
diff --git a/OndselSolver/LDUFullMat.cpp b/OndselSolver/LDUFullMat.cpp
index 010b21e..b3d0498 100644
--- a/OndselSolver/LDUFullMat.cpp
+++ b/OndselSolver/LDUFullMat.cpp
@@ -63,7 +63,7 @@ void LDUFullMat::postSolve()
 void LDUFullMat::preSolvesaveOriginal(FMatDsptr fullMat, bool saveOriginal)
 {
 	if (saveOriginal) {
-		matrixA = fullMat->copy();
+		matrixA = toFMDsptr(fullMat->copy());
 	}
 	else {
 		matrixA = fullMat;
diff --git a/OndselSolver/MBDynItem.cpp b/OndselSolver/MBDynItem.cpp
index 78272b0..31a328d 100644
--- a/OndselSolver/MBDynItem.cpp
+++ b/OndselSolver/MBDynItem.cpp
@@ -164,7 +164,7 @@ FMatDsptr MbD::MBDynItem::readOrientation(std::vector<std::string>& args)
 		auto ref = mbdynReferences()->at(refName);
 		auto aAFf = readBasicOrientation(args);
 		auto aAOF = ref->aAOf;
-		aAOf = aAOF->timesFullMatrix(aAFf);
+		aAOf = toFMDsptr(aAOF->timesFullMatrix(aAFf));
 	}
 	else if (str.find("hinge") != std::string::npos) {
 		args.erase(args.begin());
@@ -300,13 +300,13 @@ FMatDsptr MbD::MBDynItem::readBasicOrientation(std::vector<std::string>& args)
 		else {
 			assert(false);
 		}
-		auto aAFf = FullMatrix<double>::identitysptr(3);
+		auto aAFf = FullMatrixDouble::identitysptr(3);
 		aAFf->atijputFullColumn(0, 0, vecX);
 		aAFf->atijputFullColumn(0, 1, vecY);
 		aAFf->atijputFullColumn(0, 2, vecZ);
 		return aAFf;
 	}
-	auto aAFf = FullMatrix<double>::identitysptr(3);
+	auto aAFf = FullMatrixDouble::identitysptr(3);
 	for (int i = 0; i < 3; i++)
 	{
 		auto rowi = aAFf->at(i);
diff --git a/OndselSolver/MBDynMarker.cpp b/OndselSolver/MBDynMarker.cpp
index 37fd175..9c789a3 100644
--- a/OndselSolver/MBDynMarker.cpp
+++ b/OndselSolver/MBDynMarker.cpp
@@ -21,7 +21,7 @@ void MbD::MBDynMarker::parseMBDyn(std::vector<std::string>& args)
 		auto rOmO = readPosition(args);
 		auto aAOm = readOrientation(args);
 		rPmP = aAOP->transposeTimesFullColumn(rOmO->minusFullColumn(rOPO));
-		aAPm = aAOP->transposeTimesFullMatrix(aAOm);
+		aAPm = toFMDsptr(aAOP->transposeTimesFullMatrix(aAOm));
 	}
 	else if (str.find("offset") != std::string::npos) {
 		rPmP = readPosition(args);
diff --git a/OndselSolver/MarkerFrame.cpp b/OndselSolver/MarkerFrame.cpp
index 72075d0..81c115e 100644
--- a/OndselSolver/MarkerFrame.cpp
+++ b/OndselSolver/MarkerFrame.cpp
@@ -70,13 +70,13 @@ void MarkerFrame::calcPostDynCorrectorIteration()
 	auto rOpO = partFrame->rOpO();
 	auto aAOp = partFrame->aAOp();
 	rOmO = rOpO->plusFullColumn(aAOp->timesFullColumn(rpmp));
-	aAOm = aAOp->timesFullMatrix(aApm);
+	aAOm = toFMDsptr(aAOp->timesFullMatrix(aApm));
 	auto pAOppE = partFrame->pAOppE();
 	for (int i = 0; i < 4; i++)
 	{
 		auto& pAOppEi = pAOppE->at(i);
 		prOmOpE->atijputFullColumn(0, i, pAOppEi->timesFullColumn(rpmp));
-		pAOmpE->at(i) = pAOppEi->timesFullMatrix(aApm);
+		pAOmpE->at(i) = toFMDsptr(pAOppEi->timesFullMatrix(aApm));
 	}
 }
 
diff --git a/OndselSolver/MarkerFrame.h b/OndselSolver/MarkerFrame.h
index 7c67474..ac49416 100644
--- a/OndselSolver/MarkerFrame.h
+++ b/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 = FullMatrix<double>::identitysptr(3);
+		FMatDsptr aApm = FullMatrixDouble::identitysptr(3);
 		FColDsptr rOmO = std::make_shared<FullColumn<double>>(3);
-		FMatDsptr aAOm = FullMatrix<double>::identitysptr(3);
+		FMatDsptr aAOm = FullMatrixDouble::identitysptr(3);
 		FMatDsptr prOmOpE;
 		FColFMatDsptr pAOmpE;
 		FMatFColDsptr pprOmOpEpE;
diff --git a/OndselSolver/MomentOfInertiaSolver.cpp b/OndselSolver/MomentOfInertiaSolver.cpp
index 84fcc13..04240a9 100644
--- a/OndselSolver/MomentOfInertiaSolver.cpp
+++ b/OndselSolver/MomentOfInertiaSolver.cpp
@@ -32,7 +32,7 @@ void MbD::MomentOfInertiaSolver::example1()
 	solver->setJPP(aJPP);
 	auto rPoP = aApP->transposeTimesFullColumn(rpPp->negated());
 	solver->setrPoP(rPoP);
-	auto aAPo = aApP->transpose();
+	auto aAPo = toFMDsptr(aApP->transpose());
 	solver->setAPo(aAPo);
 	solver->setrPcmP(rPoP);
 	solver->calc();
@@ -188,7 +188,7 @@ void MbD::MomentOfInertiaSolver::calcJoo()
 	auto term22 = rPoPtilde->timesFullMatrix(rocmPtilde);
 	auto term23 = term22->transpose();
 	auto term2 = term21->plusFullMatrix(term22)->plusFullMatrix(term23)->times(m);
-	aJoo = aAPo->transposeTimesFullMatrix(term1->plusFullMatrix(term2))->timesFullMatrix(aAPo);
+	aJoo = toFMDsptr(aAPo->transposeTimesFullMatrix(term1->plusFullMatrix(term2))->timesFullMatrix(aAPo));
 	aJoo->symLowerWithUpper();
 	aJoo->conditionSelfWithTol(aJoo->maxMagnitude() * 1.0e-6);
 }
@@ -197,8 +197,8 @@ void MbD::MomentOfInertiaSolver::calcJpp()
 {
 	//"aJcmP = aJPP + mass*(rPcmPTilde*rPcmPTilde)"
 
-	auto rPcmPtilde = FullMatrix<double>::tildeMatrix(rPcmP);
-	aJcmP = aJPP->plusFullMatrix(rPcmPtilde->timesFullMatrix(rPcmPtilde)->times(m));
+	auto rPcmPtilde = FullMatrixDouble::tildeMatrix(rPcmP);
+	aJcmP = toFMDsptr(aJPP->plusFullMatrix(rPcmPtilde->timesFullMatrix(rPcmPtilde)->times(m)));
 	aJcmP->symLowerWithUpper();
 	aJcmP->conditionSelfWithTol(aJcmP->maxMagnitude() * 1.0e-6);
 	if (aJcmP->isDiagonal()) {
@@ -217,7 +217,7 @@ void MbD::MomentOfInertiaSolver::calcAPp()
 	auto lam2 = aJpp->at(2);
 	if (lam0 == lam1) {
 		if (lam1 == lam2) {
-			aAPp = FullMatrix<double>::identitysptr(3);
+			aAPp = FullMatrixDouble::identitysptr(3);
 		}
 		else {
 			eigenvector1 = eigenvectorFor(lam1);
@@ -238,7 +238,7 @@ void MbD::MomentOfInertiaSolver::calcAPp()
 		if (eigenvector1->at(1) < 0.0) eigenvector1->negateSelf();
 		eigenvector2 = eigenvector0->cross(eigenvector1);
 	}
-	aAPp = std::make_shared<FullMatrix<double>>(3, 3);
+	aAPp = std::make_shared<FullMatrixDouble>(3, 3);
 	aAPp->atijputFullColumn(0, 0, eigenvector0);
 	aAPp->atijputFullColumn(0, 1, eigenvector1);
 	aAPp->atijputFullColumn(0, 2, eigenvector2);
@@ -249,7 +249,7 @@ FColDsptr MbD::MomentOfInertiaSolver::eigenvectorFor(double lam)
 	//"[aJcmP] - lam[I]."
 
 	double e0, e1, e2;
-	aJcmPcopy = aJcmP->copy();
+	aJcmPcopy = toFMDsptr(aJcmP->copy());
 	colOrder = std::make_shared<FullRow<int>>(3);
 	auto eigenvector = std::make_shared<FullColumn<double>>(3);
 	for (int i = 0; i < 3; i++)
diff --git a/OndselSolver/OrbitAnglezIeqcJeqc.cpp b/OndselSolver/OrbitAnglezIeqcJeqc.cpp
index 35f96ca..245fba7 100644
--- a/OndselSolver/OrbitAnglezIeqcJeqc.cpp
+++ b/OndselSolver/OrbitAnglezIeqcJeqc.cpp
@@ -243,7 +243,7 @@ void MbD::OrbitAngleZIeqcJeqc::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);
+	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);
diff --git a/OndselSolver/Part.cpp b/OndselSolver/Part.cpp
index 701874c..44e1ad0 100644
--- a/OndselSolver/Part.cpp
+++ b/OndselSolver/Part.cpp
@@ -462,7 +462,7 @@ void Part::calcmEdot()
 	auto a4J = aJ->times(4.0);
 	auto term1 = aC->transposeTimesFullMatrix(a4J->timesFullMatrix(aCdot));
 	auto term2 = term1->transpose();
-	mEdot = term1->plusFullMatrix(term2);
+	mEdot = toFMDsptr(term1->plusFullMatrix(term2));
 }
 
 void Part::calcpTpE()
@@ -479,7 +479,7 @@ void Part::calcppTpEpE()
 	auto& qEdot = partFrame->qEdot;
 	auto pCpEtimesqEdot = EulerParameters<double>::pCpEtimesColumn(qEdot);
 	auto a4J = aJ->times(4.0);
-	ppTpEpE = pCpEtimesqEdot->transposeTimesFullMatrix(a4J->timesFullMatrix(pCpEtimesqEdot));
+	ppTpEpE = toFMDsptr(pCpEtimesqEdot->transposeTimesFullMatrix(a4J->timesFullMatrix(pCpEtimesqEdot)));
 }
 
 void Part::calcppTpEpEdot()
@@ -491,13 +491,13 @@ void Part::calcppTpEpEdot()
 	auto term1 = EulerParameters<double>::pCTpEtimesColumn(a4J->timesFullColumn(aC->timesFullColumn(qEdot)));
 	auto pCpEtimesqEdot = EulerParameters<double>::pCpEtimesColumn(qEdot);
 	auto term2 = aC->transposeTimesFullMatrix(a4J->timesFullMatrix(pCpEtimesqEdot));
-	ppTpEpEdot = term1->plusFullMatrix(term2)->transpose();
+	ppTpEpEdot = toFMDsptr(term1->plusFullMatrix(term2)->transpose());
 }
 
 void Part::calcmE()
 {
 	auto aC = partFrame->aC();
-	mE = aC->transposeTimesFullMatrix(aJ->timesFullMatrix(aC))->times(4.0);
+	mE = toFMDsptr(aC->transposeTimesFullMatrix(aJ->timesFullMatrix(aC))->times(4.0));
 }
 
 void Part::fillAccICIterError(FColDsptr col)
diff --git a/OndselSolver/RackPinConstraintIqcJc.cpp b/OndselSolver/RackPinConstraintIqcJc.cpp
index d79a3d8..3b4cebc 100644
--- a/OndselSolver/RackPinConstraintIqcJc.cpp
+++ b/OndselSolver/RackPinConstraintIqcJc.cpp
@@ -53,12 +53,14 @@ void MbD::RackPinConstraintIqcJc::calc_pGpXI()
 
 void MbD::RackPinConstraintIqcJc::calc_ppGpEIpEI()
 {
-	ppGpEIpEI = xIeJeIe->ppvaluepEIpEI()->plusFullMatrix(thezIeJe->ppvaluepEIpEI()->times(pitchRadius));
+	ppGpEIpEI = toFMDsptr(xIeJeIe->ppvaluepEIpEI()
+            ->plusFullMatrix(thezIeJe->ppvaluepEIpEI()->times(pitchRadius)));
 }
 
 void MbD::RackPinConstraintIqcJc::calc_ppGpXIpEI()
 {
-	ppGpXIpEI = xIeJeIe->ppvaluepXIpEI()->plusFullMatrix(thezIeJe->ppvaluepXIpEI()->times(pitchRadius));
+	ppGpXIpEI = toFMDsptr(xIeJeIe->ppvaluepXIpEI()
+            ->plusFullMatrix(thezIeJe->ppvaluepXIpEI()->times(pitchRadius)));
 }
 
 void MbD::RackPinConstraintIqcJc::calcPostDynCorrectorIteration()
diff --git a/OndselSolver/RackPinConstraintIqcJqc.cpp b/OndselSolver/RackPinConstraintIqcJqc.cpp
index 8c25136..144b81b 100644
--- a/OndselSolver/RackPinConstraintIqcJqc.cpp
+++ b/OndselSolver/RackPinConstraintIqcJqc.cpp
@@ -30,17 +30,20 @@ void MbD::RackPinConstraintIqcJqc::calc_pGpXJ()
 
 void MbD::RackPinConstraintIqcJqc::calc_ppGpEIpEJ()
 {
-	ppGpEIpEJ = xIeJeIe->ppvaluepEIpEJ()->plusFullMatrix(thezIeJe->ppvaluepEIpEJ()->times(pitchRadius));
+	ppGpEIpEJ = toFMDsptr(xIeJeIe->ppvaluepEIpEJ()
+            ->plusFullMatrix(thezIeJe->ppvaluepEIpEJ()->times(pitchRadius)));
 }
 
 void MbD::RackPinConstraintIqcJqc::calc_ppGpEIpXJ()
 {
-	ppGpEIpXJ = xIeJeIe->ppvaluepEIpXJ()->plusFullMatrix(thezIeJe->ppvaluepEIpXJ()->times(pitchRadius));
+	ppGpEIpXJ = toFMDsptr(xIeJeIe->ppvaluepEIpXJ()
+            ->plusFullMatrix(thezIeJe->ppvaluepEIpXJ()->times(pitchRadius)));
 }
 
 void MbD::RackPinConstraintIqcJqc::calc_ppGpEJpEJ()
 {
-	ppGpEJpEJ = xIeJeIe->ppvaluepEJpEJ()->plusFullMatrix(thezIeJe->ppvaluepEJpEJ()->times(pitchRadius));
+	ppGpEJpEJ = toFMDsptr(xIeJeIe->ppvaluepEJpEJ()
+            ->plusFullMatrix(thezIeJe->ppvaluepEJpEJ()->times(pitchRadius)));
 }
 
 void MbD::RackPinConstraintIqcJqc::calcPostDynCorrectorIteration()
diff --git a/OndselSolver/ScrewConstraintIqcJc.cpp b/OndselSolver/ScrewConstraintIqcJc.cpp
index 463954f..9249c97 100644
--- a/OndselSolver/ScrewConstraintIqcJc.cpp
+++ b/OndselSolver/ScrewConstraintIqcJc.cpp
@@ -55,12 +55,14 @@ void MbD::ScrewConstraintIqcJc::calc_pGpXI()
 
 void MbD::ScrewConstraintIqcJc::calc_ppGpEIpEI()
 {
-	ppGpEIpEI = zIeJeIe->ppvaluepEIpEI()->times(2.0 * M_PI)->minusFullMatrix(thezIeJe->ppvaluepEIpEI()->times(pitch));
+	ppGpEIpEI = toFMDsptr(zIeJeIe->ppvaluepEIpEI()->times(2.0 * M_PI)
+            ->minusFullMatrix(thezIeJe->ppvaluepEIpEI()->times(pitch)));
 }
 
 void MbD::ScrewConstraintIqcJc::calc_ppGpXIpEI()
 {
-	ppGpXIpEI = zIeJeIe->ppvaluepXIpEI()->times(2.0 * M_PI)->minusFullMatrix(thezIeJe->ppvaluepXIpEI()->times(pitch));
+	ppGpXIpEI = toFMDsptr(zIeJeIe->ppvaluepXIpEI()->times(2.0 * M_PI)
+            ->minusFullMatrix(thezIeJe->ppvaluepXIpEI()->times(pitch)));
 }
 
 void MbD::ScrewConstraintIqcJc::calcPostDynCorrectorIteration()
diff --git a/OndselSolver/ScrewConstraintIqcJqc.cpp b/OndselSolver/ScrewConstraintIqcJqc.cpp
index e02d7fe..18c9ce2 100644
--- a/OndselSolver/ScrewConstraintIqcJqc.cpp
+++ b/OndselSolver/ScrewConstraintIqcJqc.cpp
@@ -32,17 +32,20 @@ void MbD::ScrewConstraintIqcJqc::calc_pGpXJ()
 
 void MbD::ScrewConstraintIqcJqc::calc_ppGpEIpEJ()
 {
-	ppGpEIpEJ = zIeJeIe->ppvaluepEIpEJ()->times(2.0 * M_PI)->minusFullMatrix(thezIeJe->ppvaluepEIpEJ()->times(pitch));
+	ppGpEIpEJ = toFMDsptr(zIeJeIe->ppvaluepEIpEJ()->times(2.0 * M_PI)
+            ->minusFullMatrix(thezIeJe->ppvaluepEIpEJ()->times(pitch)));
 }
 
 void MbD::ScrewConstraintIqcJqc::calc_ppGpEIpXJ()
 {
-	ppGpEIpXJ = zIeJeIe->ppvaluepEIpXJ()->times(2.0 * M_PI)->minusFullMatrix(thezIeJe->ppvaluepEIpXJ()->times(pitch));
+	ppGpEIpXJ = toFMDsptr(zIeJeIe->ppvaluepEIpXJ()->times(2.0 * M_PI)
+            ->minusFullMatrix(thezIeJe->ppvaluepEIpXJ()->times(pitch)));
 }
 
 void MbD::ScrewConstraintIqcJqc::calc_ppGpEJpEJ()
 {
-	ppGpEJpEJ = zIeJeIe->ppvaluepEJpEJ()->times(2.0 * M_PI)->minusFullMatrix(thezIeJe->ppvaluepEJpEJ()->times(pitch));
+	ppGpEJpEJ = toFMDsptr(zIeJeIe->ppvaluepEJpEJ()->times(2.0 * M_PI)
+            ->minusFullMatrix(thezIeJe->ppvaluepEJpEJ()->times(pitch)));
 }
 
 void MbD::ScrewConstraintIqcJqc::calcPostDynCorrectorIteration()
diff --git a/OndselSolver/TranslationConstraintIqcJc.cpp b/OndselSolver/TranslationConstraintIqcJc.cpp
index 264ebce..db2ce00 100644
--- a/OndselSolver/TranslationConstraintIqcJc.cpp
+++ b/OndselSolver/TranslationConstraintIqcJc.cpp
@@ -30,8 +30,10 @@ void TranslationConstraintIqcJc::calcPostDynCorrectorIteration()
 	pGpXI = riIeqJeIeq->pvaluepXI();
 	pGpEI = (riIeqJeIeq->pvaluepEI())->plusFullRow(riIeqJeIeq->pvaluepEK());
 	ppGpXIpEI = riIeqJeIeq->ppvaluepXIpEK();
-	ppGpEIpEI = riIeqJeIeq->ppvaluepEIpEI()->plusFullMatrix(riIeqJeIeq->ppvaluepEIpEK())
-		->plusFullMatrix((riIeqJeIeq->ppvaluepEIpEK()->transpose()->plusFullMatrix(riIeqJeIeq->ppvaluepEKpEK())));
+	ppGpEIpEI = toFMDsptr(riIeqJeIeq->ppvaluepEIpEI()
+            ->plusFullMatrix(riIeqJeIeq->ppvaluepEIpEK())
+            ->plusFullMatrix((riIeqJeIeq->ppvaluepEIpEK()->
+                transpose()->plusFullMatrix(riIeqJeIeq->ppvaluepEKpEK()))));
 }
 
 void TranslationConstraintIqcJc::useEquationNumbers()
diff --git a/OndselSolver/TranslationConstraintIqcJqc.cpp b/OndselSolver/TranslationConstraintIqcJqc.cpp
index 5659e02..02377a4 100644
--- a/OndselSolver/TranslationConstraintIqcJqc.cpp
+++ b/OndselSolver/TranslationConstraintIqcJqc.cpp
@@ -28,8 +28,8 @@ void TranslationConstraintIqcJqc::calcPostDynCorrectorIteration()
 	TranslationConstraintIqcJc::calcPostDynCorrectorIteration();
 	pGpXJ = riIeJeIe->pvaluepXJ();
 	pGpEJ = riIeJeIe->pvaluepEJ();
-	ppGpEIpXJ = riIeJeIe->ppvaluepXJpEK()->transpose();
-	ppGpEIpEJ = riIeJeIe->ppvaluepEJpEK()->transpose();
+	ppGpEIpXJ = toFMDsptr(riIeJeIe->ppvaluepXJpEK()->transpose());
+	ppGpEIpEJ = toFMDsptr(riIeJeIe->ppvaluepEJpEK()->transpose());
 	ppGpEJpEJ = riIeJeIe->ppvaluepEJpEJ();
 }