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.
79 lines
2.4 KiB
C++
79 lines
2.4 KiB
C++
/***************************************************************************
|
|
* Copyright (c) 2023 Ondsel, Inc. *
|
|
* *
|
|
* This file is part of OndselSolver. *
|
|
* *
|
|
* See LICENSE file for details about copyright. *
|
|
***************************************************************************/
|
|
|
|
#include <algorithm>
|
|
#include "FunctionXY.h"
|
|
#include "Sum.h"
|
|
#include "Constant.h"
|
|
|
|
using namespace MbD;
|
|
|
|
MbD::FunctionXY::FunctionXY()
|
|
{
|
|
}
|
|
|
|
MbD::FunctionXY::FunctionXY(Symsptr base, Symsptr exp) : x(base), y(exp)
|
|
{
|
|
}
|
|
|
|
Symsptr MbD::FunctionXY::copyWith(Symsptr argx, Symsptr argy)
|
|
{
|
|
return Symsptr();
|
|
}
|
|
|
|
void MbD::FunctionXY::arguments(Symsptr args)
|
|
{
|
|
//args is a Sum with "terms" containing the actual arguments
|
|
auto sum = std::static_pointer_cast<Sum>(args);
|
|
assert(sum->terms->size() == 2);
|
|
x = sum->terms->at(0);
|
|
y = sum->terms->at(1);
|
|
}
|
|
|
|
Symsptr MbD::FunctionXY::expandUntil(Symsptr sptr, std::shared_ptr<std::unordered_set<Symsptr>> set)
|
|
{
|
|
auto itr = std::find_if(set->begin(), set->end(), [sptr](Symsptr sym) {return sptr.get() == sym.get(); });
|
|
if (itr != set->end()) return sptr;
|
|
auto newx = x->expandUntil(x, set);
|
|
auto newy = y->expandUntil(y, set);
|
|
auto copy = copyWith(newx, newy);
|
|
return copy;
|
|
}
|
|
|
|
Symsptr MbD::FunctionXY::simplifyUntil(Symsptr sptr, std::shared_ptr<std::unordered_set<Symsptr>> set)
|
|
{
|
|
auto itr = std::find_if(set->begin(), set->end(), [sptr](Symsptr sym) {return sptr.get() == sym.get(); });
|
|
if (itr != set->end()) return sptr;
|
|
auto newx = x->simplifyUntil(x, set);
|
|
auto newy = y->simplifyUntil(y, set);
|
|
auto copy = copyWith(newx, newy);
|
|
return copy;
|
|
}
|
|
|
|
void MbD::FunctionXY::createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits)
|
|
{
|
|
x->createMbD(mbdSys, mbdUnits);
|
|
y->createMbD(mbdSys, mbdUnits);
|
|
}
|
|
|
|
bool MbD::FunctionXY::isConstant()
|
|
{
|
|
return x->isConstant() && y->isConstant();
|
|
}
|
|
|
|
Symsptr MbD::FunctionXY::differentiateWRT(Symsptr var)
|
|
{
|
|
if (this == var.get()) return sptrConstant(1.0);
|
|
auto dfdx = differentiateWRTx();
|
|
auto dfdy = differentiateWRTy();
|
|
auto dxdvar = x->differentiateWRT(var);
|
|
auto dydvar = y->differentiateWRT(var);
|
|
return Symbolic::sum(Symbolic::times(dfdx, dxdvar), Symbolic::times(dfdy, dydvar));
|
|
}
|
|
|