File indexing completed on 2024-04-28 07:39:44
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