1 /* This file is part of StepCore library.
2 Copyright (C) 2007 Vladimir Kuznetsov <ks.vladimir@gmail.com>
3
4 StepCore library is free software; you can redistribute it and/or modify
5 it under the terms of the GNU General Public License as published by
6 the Free Software Foundation; either version 2 of the License, or
7 (at your option) any later version.
8
9 StepCore library is distributed in the hope that it will be useful,
10 but WITHOUT ANY WARRANTY; without even the implied warranty of
11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 GNU General Public License for more details.
13
14 You should have received a copy of the GNU General Public License
15 along with StepCore; if not, write to the Free Software
16 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
17 */
18
19 #include "constraintsolver.h"
20 #include "rigidbody.h"
21 #include "particle.h"
22 #include "types.h"
23 #include <iostream>
24 #include <unsupported/Eigen/IterativeSolvers>
25 #include <cmath>
26
27 using namespace Eigen;
28
29 namespace StepCore {
30
31 STEPCORE_META_OBJECT(ConstraintSolver, QT_TRANSLATE_NOOP("ObjectClass", "ConstraintSolver"), "ConstraintSolver", MetaObject::ABSTRACT, STEPCORE_SUPER_CLASS(Object),)
32 //STEPCORE_PROPERTY_RW(double, toleranceAbs, QT_TRANSLATE_NOOP("PropertyName", "toleranceAbs"), STEPCORE_UNITS_1, "Allowed absolute tolerance", toleranceAbs, setToleranceAbs)
33 //STEPCORE_PROPERTY_R_D(double, localError, QT_TRANSLATE_NOOP("PropertyName", "localError"), STEPCORE_UNITS_1, "Maximal local error during last step", localError))
34
35 STEPCORE_META_OBJECT(CGConstraintSolver, QT_TRANSLATE_NOOP("ObjectClass", "CGConstraintSolver"), "CGConstraintSolver", 0,
36 STEPCORE_SUPER_CLASS(ConstraintSolver),)
37
38
solve(ConstraintsInfo * info)39 int CGConstraintSolver::solve(ConstraintsInfo* info)
40 {
41 int nc = info->constraintsCount + info->contactsCount;
42
43 // XXX: make this matrixes permanent to avoid memory allocations
44 SparseRowMatrix a(nc, nc);
45 VectorXd b(nc);
46 VectorXd x(nc);
47 x.setZero();
48
49 a = info->jacobian * (info->inverseMass.asDiagonal() * info->jacobian.transpose());
50
51 b = info->jacobian * info->acceleration;
52 b += info->jacobianDerivative * info->velocity;
53 b = - (b + info->value + info->derivative);
54
55 IterationController iter(2.0E-5); // XXX
56
57 // print debug info
58 /*std::cout << "ConstraintSolver:" << endl
59 << "J=" << info->jacobian << endl
60 << "J'=" << info->jacobianDerivative << endl
61 << "C=" << info->value << endl
62 << "C'=" << info->derivative << endl
63 << "invM=" << info->inverseMass << endl
64 << "pos=" << info->position << endl
65 << "vel=" << info->velocity << endl
66 << "acc=" << info->acceleration << endl
67 << "a=" << a << endl
68 << "b=" << b << endl
69 << "l=" << l << endl
70 << "force=" << info->force << endl;*/
71
72 // constrained_cg ?
73 // XXX: limit iterations count
74
75 // XXX: Use sparce vectors for fmin and fmax
76 int fminCount = 0;
77 int fmaxCount = 0;
78 for(int i=0; i<nc; ++i) {
79 if(std::isfinite(info->forceMin[i])) ++fminCount;
80 if(std::isfinite(info->forceMax[i])) ++fmaxCount;
81 }
82
83 DynSparseRowMatrix c(fminCount + fmaxCount, nc);
84 VectorXd f(fminCount + fmaxCount);
85
86 int fminIndex = 0;
87 int fmaxIndex = fminCount;
88 for(int i=0; i<nc; ++i) {
89 if(std::isfinite(info->forceMin[i])) {
90 c.coeffRef(fminIndex,i) = -1;
91 f[fminIndex] = -info->forceMin[i];
92 ++fminIndex;
93 }
94 if(std::isfinite(info->forceMax[i])) {
95 c.coeffRef(fmaxIndex, i) = 1;
96 f[fmaxIndex] = info->forceMax[i];
97 ++fmaxIndex;
98 }
99 }
100 internal::constrained_cg(a, c, x, b, f, iter);
101
102 info->force = info->jacobian.transpose() * x;
103
104 // print debug info
105 /*std::cout << "Solved:" << endl
106 << "J=" << info->jacobian << endl
107 << "J'=" << info->jacobianDerivative << endl
108 << "C=" << info->value << endl
109 << "C'=" << info->derivative << endl
110 << "invM=" << info->inverseMass << endl
111 << "pos=" << info->position << endl
112 << "vel=" << info->velocity << endl
113 << "acc=" << info->acceleration << endl
114 << "a=" << a << endl
115 << "b=" << b << endl
116 << "l=" << l << endl
117 << "force=" << info->force << endl << endl << endl;*/
118 return 0;
119 }
120
121 } // namespace StepCore
122
123