98051ba0c9
- Move existing OndselSolver, GNN ML layer, and tooling into GNN/ directory for integration in later phases - Add Create addon scaffold: package.xml, Init.py - Add expression DAG with eval, symbolic diff, simplification - Add parameter table with fixed/free variable tracking - Add quaternion rotation as polynomial Expr trees - Add RigidBody entity (7 DOF: position + unit quaternion) - Add constraint classes: Coincident, DistancePointPoint, Fixed - Add Newton-Raphson solver with symbolic Jacobian + numpy lstsq - Add pre-solve passes: substitution + single-equation - Add DOF counting via Jacobian SVD rank - Add KindredSolver IKCSolver bridge for kcsolve integration - Add 82 unit tests covering all modules Registers as 'kindred' solver via kcsolve.register_solver() when loaded by Create's addon_loader.
101 lines
2.8 KiB
C++
101 lines
2.8 KiB
C++
/***************************************************************************
|
|
* Copyright (c) 2023 Ondsel, Inc. *
|
|
* *
|
|
* This file is part of OndselSolver. *
|
|
* *
|
|
* See LICENSE file for details about copyright. *
|
|
***************************************************************************/
|
|
|
|
#include "DispCompIeqcJeqcKeqc.h"
|
|
#include "EndFrameqc.h"
|
|
|
|
using namespace MbD;
|
|
|
|
DispCompIeqcJeqcKeqc::DispCompIeqcJeqcKeqc()
|
|
{
|
|
}
|
|
|
|
DispCompIeqcJeqcKeqc::DispCompIeqcJeqcKeqc(EndFrmsptr frmi, EndFrmsptr frmj, EndFrmsptr frmk, size_t axisk) : DispCompIeqcJecKeqc(frmi, frmj, frmk, axisk)
|
|
{
|
|
}
|
|
|
|
void DispCompIeqcJeqcKeqc::initialize()
|
|
{
|
|
DispCompIeqcJecKeqc::initialize();
|
|
priIeJeKepXJ = std::make_shared<FullRow<double>>(3);
|
|
priIeJeKepEJ = std::make_shared<FullRow<double>>(4);
|
|
ppriIeJeKepEJpEJ = std::make_shared<FullMatrix<double>>(4, 4);
|
|
ppriIeJeKepXJpEK = std::make_shared<FullMatrix<double>>(3, 4);
|
|
ppriIeJeKepEJpEK = std::make_shared<FullMatrix<double>>(4, 4);
|
|
}
|
|
|
|
void DispCompIeqcJeqcKeqc::calcPostDynCorrectorIteration()
|
|
{
|
|
DispCompIeqcJecKeqc::calcPostDynCorrectorIteration();
|
|
auto frmJqc = std::static_pointer_cast<EndFrameqc>(frmJ);
|
|
auto prIeJeOpEJT = frmJqc->prOeOpE->transpose();
|
|
auto& pprIeJeOpEJpEJ = frmJqc->pprOeOpEpE;
|
|
for (size_t i = 0; i < 3; i++)
|
|
{
|
|
priIeJeKepXJ->atiput(i, aAjOKe->at(i));
|
|
}
|
|
for (size_t i = 0; i < 4; i++)
|
|
{
|
|
priIeJeKepEJ->atiput(i, aAjOKe->dot(prIeJeOpEJT->at(i)));
|
|
}
|
|
for (size_t i = 0; i < 3; i++)
|
|
{
|
|
auto& ppriIeJeKepXJipEK = ppriIeJeKepXJpEK->at(i);
|
|
for (size_t j = 0; j < 4; j++)
|
|
{
|
|
ppriIeJeKepXJipEK->atiput(j, pAjOKepEKT->at(j)->at(i));
|
|
}
|
|
}
|
|
for (size_t i = 0; i < 4; i++)
|
|
{
|
|
auto& pprIeJeOpEJipEJ = pprIeJeOpEJpEJ->at(i);
|
|
auto& ppriIeJeKepEJipEJ = ppriIeJeKepEJpEJ->at(i);
|
|
ppriIeJeKepEJipEJ->atiput(i, aAjOKe->dot(pprIeJeOpEJipEJ->at(i)));
|
|
for (size_t j = 0; j < 4; j++)
|
|
{
|
|
auto ppriIeJeKepEJipEJj = (aAjOKe->dot(pprIeJeOpEJipEJ->at(j)));
|
|
ppriIeJeKepEJipEJ->atiput(j, ppriIeJeKepEJipEJj);
|
|
ppriIeJeKepEJpEJ->atijput(j, i, ppriIeJeKepEJipEJj);
|
|
}
|
|
}
|
|
for (size_t i = 0; i < 4; i++)
|
|
{
|
|
auto& prIeJeOpEJTi = prIeJeOpEJT->at(i);
|
|
auto& ppriIeJeKepEJipEK = ppriIeJeKepEJpEK->at(i);
|
|
for (size_t j = 0; j < 4; j++)
|
|
{
|
|
ppriIeJeKepEJipEK->atiput(j, pAjOKepEKT->at(j)->dot(prIeJeOpEJTi));
|
|
}
|
|
}
|
|
}
|
|
|
|
FRowDsptr DispCompIeqcJeqcKeqc::pvaluepXJ()
|
|
{
|
|
return priIeJeKepXJ;
|
|
}
|
|
|
|
FRowDsptr DispCompIeqcJeqcKeqc::pvaluepEJ()
|
|
{
|
|
return priIeJeKepEJ;
|
|
}
|
|
|
|
FMatDsptr DispCompIeqcJeqcKeqc::ppvaluepXJpEK()
|
|
{
|
|
return ppriIeJeKepXJpEK;
|
|
}
|
|
|
|
FMatDsptr DispCompIeqcJeqcKeqc::ppvaluepEJpEK()
|
|
{
|
|
return ppriIeJeKepEJpEK;
|
|
}
|
|
|
|
FMatDsptr DispCompIeqcJeqcKeqc::ppvaluepEJpEJ()
|
|
{
|
|
return ppriIeJeKepEJpEJ;
|
|
}
|