Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00016 #include "lcp.h"
00017 #include <limits>
00018 #include <list>
00019
00020 int LCP::Solve(fVec& _g, fVec& _a)
00021 {
00022 fVec g_init(n_vars);
00023 g_init.zero();
00024 return SolveEx(_g, _a, g_init);
00025 }
00026
00027 int LCP::SolveEx(fVec& _g, fVec& _a, const fVec& _g_init, double _max_error, int _max_iteration, double _speed, int* n_iteration)
00028 {
00029 _g.resize(n_vars);
00030 _a.resize(n_vars);
00031 _g.set(_g_init);
00032 int failed = true, count = 0;;
00033 while(_max_iteration < 0 || count < _max_iteration)
00034 {
00035 for(int i=0; i<n_vars; i++)
00036 {
00037 double a = 0.0;
00038 for(int j=0; j<n_vars; j++)
00039 {
00040 a += N(i, j) * _g(j);
00041 }
00042 double z = _g(i) - _speed*(r(i)+a)/N(i, i);
00043 _g(i) = (z >= 0.0 ? z : 0.0);
00044 }
00045 _a.mul(N, _g);
00046 _a += r;
00047 if(_g.min_value() > -_max_error && _a.min_value() > -_max_error)
00048 {
00049 failed = false;
00050 break;
00051 }
00052 count++;
00053 }
00054 _a.mul(N, _g);
00055 _a += r;
00056 if(n_iteration) *n_iteration = count;
00057 return failed;
00058 }
00059