add DistanceConstraintIJ and AngleConstraintIJ

MbDCode/DistIeqcJec.cpp

@@ -0,0 +1,97 @@
+#include "DistIeqcJec.h"
+#include "EndFrameqc.h"
+
+using namespace MbD;
+
+MbD::DistIeqcJec::DistIeqcJec()
+{
+}
+
+MbD::DistIeqcJec::DistIeqcJec(EndFrmcptr frmi, EndFrmcptr frmj) : DistIecJec(frmi, frmj)
+{
+}
+
+void MbD::DistIeqcJec::calcPrivate()
+{
+	DistIecJec::calcPrivate();
+	if (rIeJe == 0.0) return;
+	auto frmIeqc = std::static_pointer_cast<EndFrameqc>(frmI);
+	auto& mprIeJeOpEI = frmIeqc->prOeOpE;
+	auto mprIeJeOpEIT = mprIeJeOpEI->transpose();
+	auto& mpprIeJeOpEIpEI = frmIeqc->pprOeOpEpE;
+	auto muIeJeOT = muIeJeO->transpose();
+	prIeJepXI = muIeJeOT;
+	prIeJepEI = muIeJeOT->timesFullMatrix(mprIeJeOpEI);
+	for (int i = 0; i < 3; i++)
+	{
+		auto& pprIeJepXIipXI = pprIeJepXIpXI->at(i);
+		auto& prIeJepXIi = prIeJepXI->at(i);
+		for (int j = 0; j < 3; j++)
+		{
+			auto element = (i == j) ? 1.0 : 0.0;
+			element -= prIeJepXIi * prIeJepXI->at(j);
+			pprIeJepXIipXI->atiput(j, element / rIeJe);
+		}
+	}
+	pprIeJepXIpEI = std::make_shared<FullMatrix<double>>(3, 4);
+	for (int i = 0; i < 3; i++)
+	{
+		auto& pprIeJepXIipEI = pprIeJepXIpEI->at(i);
+		auto& prIeJepXIi = prIeJepXI->at(i);
+		auto& mprIeJeOipEI = mprIeJeOpEI->at(i);
+		for (int j = 0; j < 4; j++)
+		{
+			auto element = mprIeJeOipEI->at(j) - prIeJepXIi * prIeJepEI->at(j);
+			pprIeJepXIipEI->atiput(j, element / rIeJe);
+		}
+	}
+	pprIeJepEIpEI = std::make_shared<FullMatrix<double>>(4, 4);
+	for (int i = 0; i < 4; i++)
+	{
+		auto& pprIeJepEIipEI = pprIeJepEIpEI->at(i);
+		auto& prIeJepEIi = prIeJepEI->at(i);
+		auto& mpprIeJeOpEIipEI = mpprIeJeOpEIpEI->at(i);
+		auto& mprIeJeOpEIiT = mprIeJeOpEIT->at(i);
+		for (int j = 0; j < 4; j++)
+		{
+			auto element = mprIeJeOpEIiT->dot(mprIeJeOpEIT->at(j))
+				- mpprIeJeOpEIipEI->at(j)->dot(rIeJeO) - prIeJepEIi * prIeJepEI->at(j);
+			pprIeJepEIipEI->atiput(j, element / rIeJe);
+		}
+	}
+}
+
+void MbD::DistIeqcJec::initialize()
+{
+	DistIecJec::initialize();
+	prIeJepXI = std::make_shared<FullRow<double>>(3);
+	prIeJepEI = std::make_shared<FullRow<double>>(4);
+	pprIeJepXIpXI = std::make_shared<FullMatrix<double>>(3, 3);
+	pprIeJepXIpEI = std::make_shared<FullMatrix<double>>(3, 4);
+	pprIeJepEIpEI = std::make_shared<FullMatrix<double>>(4, 4);
+}
+
+FMatDsptr MbD::DistIeqcJec::ppvaluepEIpEI()
+{
+	return pprIeJepEIpEI;
+}
+
+FMatDsptr MbD::DistIeqcJec::ppvaluepXIpEI()
+{
+	return pprIeJepXIpEI;
+}
+
+FMatDsptr MbD::DistIeqcJec::ppvaluepXIpXI()
+{
+	return pprIeJepXIpXI;
+}
+
+FRowDsptr MbD::DistIeqcJec::pvaluepEI()
+{
+	return prIeJepEI;
+}
+
+FRowDsptr MbD::DistIeqcJec::pvaluepXI()
+{
+	return prIeJepXI;
+}