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.
44 lines
1.6 KiB
C++
44 lines
1.6 KiB
C++
/***************************************************************************
|
|
* Copyright (c) 2023 Ondsel, Inc. *
|
|
* *
|
|
* This file is part of OndselSolver. *
|
|
* *
|
|
* See LICENSE file for details about copyright. *
|
|
***************************************************************************/
|
|
|
|
#include "DispCompIeqctJeqcKeqct.h"
|
|
#include "EndFrameqct.h"
|
|
|
|
using namespace MbD;
|
|
|
|
DispCompIeqctJeqcKeqct::DispCompIeqctJeqcKeqct()
|
|
{
|
|
}
|
|
|
|
DispCompIeqctJeqcKeqct::DispCompIeqctJeqcKeqct(EndFrmsptr frmi, EndFrmsptr frmj, EndFrmsptr frmk, size_t axisk) : DispCompIeqcJeqcKeqct(frmi, frmj, frmk, axisk)
|
|
{
|
|
}
|
|
|
|
void DispCompIeqctJeqcKeqct::preVelIC()
|
|
{
|
|
DispCompIeqcJeqcKeqct::preVelIC();
|
|
auto& mprIeJeOpt = std::static_pointer_cast<EndFrameqct>(frmI)->prOeOpt;
|
|
priIeJeKept -= aAjOKe->dot(mprIeJeOpt);
|
|
}
|
|
|
|
void DispCompIeqctJeqcKeqct::preAccIC()
|
|
{
|
|
DispCompIeqcJeqcKeqct::preAccIC();
|
|
auto pAjOKept = std::static_pointer_cast<EndFrameqct>(efrmK)->pAjOept(axisK);
|
|
auto efrmIqct = std::static_pointer_cast<EndFrameqct>(frmI);
|
|
auto& mprIeJeOpt = efrmIqct->prOeOpt;
|
|
auto mpprIeJeOpEITpt = efrmIqct->pprOeOpEpt->transpose();
|
|
auto& mpprIeJeOptpt = efrmIqct->pprOeOptpt;
|
|
for (size_t i = 0; i < 4; i++)
|
|
{
|
|
ppriIeJeKepEIpt->atiminusNumber(i, aAjOKe->dot(mpprIeJeOpEITpt->at(i)));
|
|
ppriIeJeKepEKpt->atiminusNumber(i, pAjOKepEKT->at(i)->dot(mprIeJeOpt));
|
|
}
|
|
ppriIeJeKeptpt += -(2.0 * pAjOKept->dot(mprIeJeOpt)) - aAjOKe->dot(mpprIeJeOptpt);
|
|
}
|