add DistanceConstraintIJ and AngleConstraintIJ

MbDCode/DistIeqcJeqc.cpp

@@ -0,0 +1,169 @@
+#include "DistIeqcJeqc.h"
+#include "EndFrameqc.h"
+
+using namespace MbD;
+
+MbD::DistIeqcJeqc::DistIeqcJeqc()
+{
+}
+
+MbD::DistIeqcJeqc::DistIeqcJeqc(EndFrmcptr frmi, EndFrmcptr frmj) : DistIeqcJec(frmi, frmj)
+{
+}
+
+void MbD::DistIeqcJeqc::calcPrivate()
+{
+	DistIeqcJec::calcPrivate();
+	if (rIeJe == 0.0) return;
+	auto frmJeqc = std::static_pointer_cast<EndFrameqc>(frmJ);
+	auto& prIeJeOpEJ = frmJeqc->prOeOpE;
+	auto prIeJeOpEJT = prIeJeOpEJ->transpose();
+	auto& pprIeJeOpEJpEJ = frmJeqc->pprOeOpEpE;
+	auto uIeJeOT = uIeJeO->transpose();
+	prIeJepXJ = uIeJeOT;
+	prIeJepEJ = uIeJeOT->timesFullMatrix(prIeJeOpEJ);
+	for (int i = 0; i < 3; i++)
+	{
+		auto& pprIeJepXIipXJ = pprIeJepXIpXJ->at(i);
+		auto& prIeJepXIi = prIeJepXI->at(i);
+		for (int j = 0; j < 3; j++)
+		{
+			auto element = (i == j) ? -1.0 : 0.0;
+			element -= prIeJepXIi * prIeJepXJ->at(j);
+			pprIeJepXIipXJ->atiput(j, element / rIeJe);
+		}
+	}
+
+	for (int i = 0; i < 4; i++)
+	{
+		auto& pprIeJepEIipXJ = pprIeJepEIpXJ->at(i);
+		auto& prIeJepEIi = prIeJepEI->at(i);
+		auto& mprIeJeOpEIiT = mprIeJeOpEIT->at(i);
+		for (int j = 0; j < 3; j++)
+		{
+			auto element = 0.0 - mprIeJeOpEIiT->at(j) - prIeJepEIi * prIeJepXJ->at(j);
+			pprIeJepEIipXJ->atiput(j, element / rIeJe);
+		}
+	}
+
+	for (int i = 0; i < 3; i++)
+	{
+		auto& pprIeJepXJipXJ = pprIeJepXJpXJ->at(i);
+		auto& prIeJepXJi = prIeJepXJ->at(i);
+		for (int j = 0; j < 3; j++)
+		{
+			auto element = (i == j) ? 1.0 : 0.0;
+			element -= prIeJepXJi * prIeJepXJ->at(j);
+			pprIeJepXJipXJ->atiput(j, element / rIeJe);
+		}
+	}
+
+	for (int i = 0; i < 3; i++)
+	{
+		auto& pprIeJepXIipEJ = pprIeJepXIpEJ->at(i);
+		auto& prIeJepXIi = prIeJepXI->at(i);
+		auto& prIeJeOipEJ = prIeJeOpEJ->at(i);
+		for (int j = 0; j < 4; j++)
+		{
+			auto element = 0.0 - prIeJeOipEJ->at(j) - prIeJepXIi * prIeJepEJ->at(j);
+			pprIeJepXIipEJ->atiput(j, element / rIeJe);
+		}
+	}
+
+	for (int i = 0; i < 4; i++)
+	{
+		auto& pprIeJepEIipEJ = pprIeJepEIpEJ->at(i);
+		auto& prIeJepEIi = prIeJepEI->at(i);
+		auto& mprIeJeOpEIiT = mprIeJeOpEIT->at(i);
+		for (int j = 0; j < 4; j++)
+		{
+			auto element = 0.0 - mprIeJeOpEIiT->dot(prIeJeOpEJT->at(j)) - prIeJepEIi * prIeJepEJ->at(j);
+			pprIeJepEIipEJ->atiput(j, element / rIeJe);
+		}
+	}
+
+	for (int i = 0; i < 3; i++)
+	{
+		auto& pprIeJepXJipEJ = pprIeJepXJpEJ->at(i);
+		auto& prIeJepXJi = prIeJepXJ->at(i);
+		auto& prIeJeOipEJ = prIeJeOpEJ->at(i);
+		for (int j = 0; j < 4; j++)
+		{
+			auto element = prIeJeOipEJ->at(j) - prIeJepXJi * prIeJepEJ->at(j);
+			pprIeJepXJipEJ->atiput(j, element / rIeJe);
+		}
+	}
+
+	for (int i = 0; i < 4; i++)
+	{
+		auto& pprIeJepEJipEJ = pprIeJepEJpEJ->at(i);
+		auto& prIeJepEJi = prIeJepEJ->at(i);
+		auto& pprIeJeOpEJipEJ = pprIeJeOpEJpEJ->at(i);
+		auto& prIeJeOpEJiT = prIeJeOpEJT->at(i);
+		for (int j = 0; j < 4; j++)
+		{
+			auto element = prIeJeOpEJiT->dot(prIeJeOpEJT->at(j))
+				+ pprIeJeOpEJipEJ->at(j)->dot(rIeJeO) - prIeJepEJi * prIeJepEJ->at(j);
+			pprIeJepEJipEJ->atiput(j, element / rIeJe);
+		}
+	}
+}
+
+void MbD::DistIeqcJeqc::initialize()
+{
+	DistIeqcJec::initialize();
+	prIeJepXJ = std::make_shared<FullRow<double>>(3);
+	prIeJepEJ = std::make_shared<FullRow<double>>(4);
+	pprIeJepXIpXJ = std::make_shared<FullMatrix<double>>(3, 3);
+	pprIeJepEIpXJ = std::make_shared<FullMatrix<double>>(4, 3);
+	pprIeJepXJpXJ = std::make_shared<FullMatrix<double>>(3, 3);
+	pprIeJepXIpEJ = std::make_shared<FullMatrix<double>>(3, 4);
+	pprIeJepEIpEJ = std::make_shared<FullMatrix<double>>(4, 4);
+	pprIeJepXJpEJ = std::make_shared<FullMatrix<double>>(3, 4);
+	pprIeJepEJpEJ = std::make_shared<FullMatrix<double>>(4, 4);
+}
+
+FMatDsptr MbD::DistIeqcJeqc::ppvaluepEIpEJ()
+{
+	return pprIeJepEIpEJ;
+}
+
+FMatDsptr MbD::DistIeqcJeqc::ppvaluepEIpXJ()
+{
+	return pprIeJepEIpXJ;
+}
+
+FMatDsptr MbD::DistIeqcJeqc::ppvaluepEJpEJ()
+{
+	return pprIeJepEJpEJ;
+}
+
+FMatDsptr MbD::DistIeqcJeqc::ppvaluepXIpEJ()
+{
+	return pprIeJepXIpEJ;
+}
+
+FMatDsptr MbD::DistIeqcJeqc::ppvaluepXIpXJ()
+{
+	return pprIeJepXIpXJ;
+}
+
+FMatDsptr MbD::DistIeqcJeqc::ppvaluepXJpEJ()
+{
+	return pprIeJepXJpEJ;
+}
+
+FMatDsptr MbD::DistIeqcJeqc::ppvaluepXJpXJ()
+{
+	return pprIeJepXJpXJ;
+}
+
+FRowDsptr MbD::DistIeqcJeqc::pvaluepEJ()
+{
+	return prIeJepEJ;
+}
+
+FRowDsptr MbD::DistIeqcJeqc::pvaluepXJ()
+{
+	return prIeJepXJ;
+}