step/stepcore/constraintsolver.cc

0001 /*
0002     SPDX-FileCopyrightText: 2007 Vladimir Kuznetsov <ks.vladimir@gmail.com>
0003
0004     SPDX-License-Identifier: GPL-2.0-or-later
0005 */
0006
0007 #include "constraintsolver.h"
0008 #include "rigidbody.h"
0009 #include "particle.h"
0010 #include "types.h"
0011 #include <iostream>
0012 #include <unsupported/Eigen/IterativeSolvers>
0013 #include <cmath>
0014
0015 using namespace Eigen;
0016
0017 namespace StepCore {
0018
0019 STEPCORE_META_OBJECT(ConstraintSolver, QT_TRANSLATE_NOOP("ObjectClass", "ConstraintSolver"), "ConstraintSolver", MetaObject::ABSTRACT, STEPCORE_SUPER_CLASS(Object),)
0020     //STEPCORE_PROPERTY_RW(double, toleranceAbs, QT_TRANSLATE_NOOP("PropertyName", "toleranceAbs"), STEPCORE_UNITS_1, "Allowed absolute tolerance", toleranceAbs, setToleranceAbs)
0021     //STEPCORE_PROPERTY_R_D(double, localError, QT_TRANSLATE_NOOP("PropertyName", "localError"), STEPCORE_UNITS_1, "Maximal local error during last step", localError))
0022
0023 STEPCORE_META_OBJECT(CGConstraintSolver, QT_TRANSLATE_NOOP("ObjectClass", "CGConstraintSolver"), "CGConstraintSolver", 0,
0024                         STEPCORE_SUPER_CLASS(ConstraintSolver),)
0025
0026
0027 int CGConstraintSolver::solve(ConstraintsInfo* info)
0028 {
0029     int nc = info->constraintsCount + info->contactsCount;
0030
0031     // XXX: make this matrixes permanent to avoid memory allocations
0032     SparseRowMatrix a(nc, nc);
0033     VectorXd b(nc);
0034     VectorXd x(nc);
0035     x.setZero();
0036
0037     a = info->jacobian * (info->inverseMass.asDiagonal() * info->jacobian.transpose());
0038
0039     b = info->jacobian * info->acceleration;
0040     b += info->jacobianDerivative * info->velocity;
0041     b = - (b + info->value + info->derivative);
0042
0043     IterationController iter(2.0E-5); // XXX
0044
0045     // print debug info
0046     /*std::cout << "ConstraintSolver:" << endl
0047               << "J=" << info->jacobian << endl
0048               << "J'=" << info->jacobianDerivative << endl
0049               << "C=" << info->value << endl
0050               << "C'=" << info->derivative << endl
0051               << "invM=" << info->inverseMass << endl
0052               << "pos=" << info->position << endl
0053               << "vel=" << info->velocity << endl
0054               << "acc=" << info->acceleration << endl
0055               << "a=" << a << endl
0056               << "b=" << b << endl
0057               << "l=" << l << endl
0058               << "force=" << info->force << endl;*/
0059
0060     // constrained_cg ?
0061     // XXX: limit iterations count
0062
0063     // XXX: Use sparce vectors for fmin and fmax
0064     int fminCount = 0;
0065     int fmaxCount = 0;
0066     for(int i=0; i<nc; ++i) {
0067         if(std::isfinite(info->forceMin[i])) ++fminCount;
0068         if(std::isfinite(info->forceMax[i])) ++fmaxCount;
0069     }
0070
0071     DynSparseRowMatrix c(fminCount + fmaxCount, nc);
0072     VectorXd f(fminCount + fmaxCount);
0073
0074     int fminIndex = 0;
0075     int fmaxIndex = fminCount;
0076     for(int i=0; i<nc; ++i) {
0077         if(std::isfinite(info->forceMin[i])) {
0078             c.coeffRef(fminIndex,i) = -1;
0079             f[fminIndex] = -info->forceMin[i];
0080             ++fminIndex;
0081         }
0082         if(std::isfinite(info->forceMax[i])) {
0083             c.coeffRef(fmaxIndex, i) = 1;
0084             f[fmaxIndex] = info->forceMax[i];
0085             ++fmaxIndex;
0086         }
0087     }
0088     internal::constrained_cg(a, c, x, b, f, iter);
0089
0090     info->force = info->jacobian.transpose() * x;
0091
0092     // print debug info
0093     /*std::cout << "Solved:" << endl
0094               << "J=" << info->jacobian << endl
0095               << "J'=" << info->jacobianDerivative << endl
0096               << "C=" << info->value << endl
0097               << "C'=" << info->derivative << endl
0098               << "invM=" << info->inverseMass << endl
0099               << "pos=" << info->position << endl
0100               << "vel=" << info->velocity << endl
0101               << "acc=" << info->acceleration << endl
0102               << "a=" << a << endl
0103               << "b=" << b << endl
0104               << "l=" << l << endl
0105               << "force=" << info->force << endl << endl << endl;*/
0106     return 0;
0107 }
0108
0109 } // namespace StepCore
0110