00001
00002
00003
00004
00005
00006
00007
00008
00016 #include "lcp.h"
00017 #include <limits>
00018 #include <dp.h>
00019 #include <list>
00020
00021
00022
00023 void swap_index(std::vector<int>& idx1, std::vector<int>& idx2, std::vector<int>& w2a, std::vector<int>& w2g, std::vector<int>& z2a, std::vector<int>& z2g)
00024 {
00025 int size = idx1.size();
00026 for(int i=0; i<size; i++)
00027 {
00028 int m = idx1[i], n = idx2[i];
00029 #ifdef VERBOSE
00030 cerr << "swap: w[" << m << "] <-> z[" << n << "]" << endl;
00031 cerr << "w[" << m << "]: a/g = " << w2a[m] << "/" << w2g[m] << endl;
00032 cerr << "z[" << n << "]: a/g = " << z2a[n] << "/" << z2g[n] << endl;
00033 #endif
00034 int w2a_org = w2a[m], z2g_org = z2g[n];
00035 w2a[m] = z2a[n];
00036 z2g[n] = w2g[m];
00037 z2a[n] = w2a_org;
00038 w2g[m] = z2g_org;
00039 }
00040 }
00041
00042 void swap_index(int idx1, int idx2, std::vector<int>& w2a, std::vector<int>& w2g, std::vector<int>& z2a, std::vector<int>& z2g)
00043 {
00044 int w2a_org = w2a[idx1], z2g_org = z2g[idx2];
00045 w2a[idx1] = z2a[idx2];
00046 z2g[idx2] = w2g[idx1];
00047 z2a[idx2] = w2a_org;
00048 w2g[idx1] = z2g_org;
00049 }
00050
00051 class LCPNode
00052 : public dpNode
00053 {
00054 public:
00055 LCPNode(LCP* _lcp,
00056 LCPNode* parent_lcp,
00057 const fVec& _q_old,
00058 double _max_error,
00059 const vector<int>& _w2a, const vector<int>& _w2g,
00060 const vector<int>& _z2a, const vector<int>& _z2g,
00061 int _n_steps,
00062 int _js, int _jr, double _cost) : dpNode() {
00063 terminate = false;
00064 n_vars = _w2a.size();
00065 q_old.resize(n_vars);
00066 q_old.set(_q_old);
00067 max_error = _max_error;
00068 lcp = _lcp;
00069 js = _js;
00070 jr = _jr;
00071 w2a = _w2a;
00072 w2g = _w2g;
00073 z2a = _z2a;
00074 z2g = _z2g;
00075 n_steps = _n_steps;
00076
00077
00078 cost = exp(_cost);
00079
00080
00081 if(js >= 0)
00082 {
00083 ys2a = w2a[js];
00084 ys2g = w2g[js];
00085 }
00086 else
00087 {
00088 ys2a = -1;
00089 ys2g = -1;
00090 }
00091 if(jr >= 0)
00092 {
00093 yr2a = z2a[jr];
00094 yr2g = z2g[jr];
00095 }
00096 else
00097 {
00098 yr2a = -1;
00099 yr2g = -1;
00100 }
00101 #ifdef VERBOSE
00102 cerr << "new node: step = " << n_steps << ", cost = " << cost << ", js = " << js << ", jr = " << jr << endl;
00103 #endif
00104 if(ys2g == n_vars)
00105 {
00106 #ifdef VERBOSE
00107 cerr << "another pivot to terminate" << endl;
00108 #endif
00109 static fMat m_jr_dummy;
00110 m_jr_dummy.resize(n_vars, 1);
00111 swap_index(js, jr, w2a, w2g, z2a, z2g);
00112 #ifdef INVERSE_FROM_SCRATCH
00113 LCP::Pivot(lcp->Mref(), lcp->Qref(), w2a, w2g, z2a, z2g, 0, m_jr_dummy, q_old);
00114 #else
00115 LCP::Pivot(lcp->Mref(), lcp->Qref(), parent_lcp->Maa_inv, parent_lcp->w2a, parent_lcp->w2g, parent_lcp->z2a, parent_lcp->z2g, ys2a, ys2g, yr2a, yr2g, w2a, w2g, z2a, z2g, 0, Maa_inv, m_jr_dummy, q_old);
00116 #endif
00117 terminate = true;
00118 }
00119 else if(js >= 0)
00120 {
00121 swap_index(js, jr, w2a, w2g, z2a, z2g);
00122 }
00123 for(int i=0; i<n_vars; i++)
00124 {
00125 if(w2g[i] >= 0)
00126 {
00127 g_in_w.push_back(w2g[i]);
00128 }
00129 }
00130 for(int i=0; i<=n_vars; i++)
00131 {
00132 if(z2a[i] >= 0)
00133 {
00134 a_in_z.push_back(z2a[i]);
00135 }
00136 }
00137 g_in_w.sort();
00138 a_in_z.sort();
00139 #ifdef VERBOSE
00140 #if 1
00141 cerr << "g_in_w = [" << flush;
00142 for(std::list<int>::iterator i=g_in_w.begin(); i!=g_in_w.end(); i++)
00143 {
00144 cerr << *i << " " << flush;
00145 }
00146 cerr << "]" << endl;
00147 cerr << "a_in_z = [" << flush;
00148 for(std::list<int>::iterator i=a_in_z.begin(); i!=a_in_z.end(); i++)
00149 {
00150 cerr << *i << " " << flush;
00151 }
00152 cerr << "]" << endl;
00153 #endif
00154 #endif
00155 new_jr = -1;
00156 if(js < 0)
00157 {
00158 new_jr = jr;
00159 }
00160 else
00161 {
00162
00163 int new_yr2a=0, new_yr2g=0;
00164 if(ys2a >= 0)
00165 {
00166 new_yr2a = -1;
00167 new_yr2g = ys2a;
00168 }
00169 else if(ys2g >= 0)
00170 {
00171 new_yr2a = ys2g;
00172 new_yr2g = -1;
00173 }
00174
00175 if(new_yr2a >= 0)
00176 {
00177 for(int i=0; i<=n_vars; i++)
00178 {
00179 if(new_yr2a == z2a[i])
00180 {
00181 new_jr = i;
00182 break;
00183 }
00184 }
00185 }
00186 else if(new_yr2g >= 0)
00187 {
00188 for(int i=0; i<=n_vars; i++)
00189 {
00190 if(new_yr2g == z2g[i])
00191 {
00192 new_jr = i;
00193 break;
00194 }
00195 }
00196 }
00197 #ifdef VERBOSE
00198 if(new_jr < 0)
00199 {
00200 cerr << "new_jr = " << new_jr << endl;
00201 cerr << "ys2a = " << ys2a << endl;
00202 cerr << "ys2g = " << ys2g << endl;
00203 cerr << "new_yr2a = " << new_yr2a << endl;
00204 cerr << "new_yr2g = " << new_yr2g << endl;
00205 }
00206 #endif
00207 assert(terminate || new_jr >= 0);
00208 }
00209 #ifdef VERBOSE
00210 cerr << "new_jr = " << new_jr << endl;
00211 #endif
00212 }
00213
00214 ~LCPNode() {
00215 }
00216
00217 int NumStep() {
00218 return n_steps;
00219 }
00220
00221 #ifndef INVERSE_FROM_SCRATCH
00222 fMat Maa_inv;
00223 #endif
00224 fVec q_old;
00225 vector<int> w2a, w2g, z2a, z2g;
00226
00227
00228 static int num_loops;
00229 static int num_errors;
00230 protected:
00231 std::list<int> g_in_w, a_in_z;
00232 int new_jr;
00233
00234 int create_child_nodes(dpNode**& nodes) {
00235 #ifdef VERBOSE
00236 cerr << "=== create_child_nodes (step = " << n_steps << ", cost = " << cost << ", total cost = " << total_cost << ", total_astar_cost = " << total_astar_cost << ", js = " << js << ", jr = " << jr << ")" << endl;
00237 #endif
00238
00239 static fVec q;
00240 q.resize(n_vars);
00241 static fMat m_jr;
00242 m_jr.resize(n_vars, 1);
00243 if(js < 0)
00244 {
00245 q.set(q_old);
00246 m_jr.get_submat(0, jr, lcp->Mref());
00247 #ifdef VERBOSE
00248 cerr << "initial node" << endl;
00249 #endif
00250 }
00251 else
00252 {
00253 #if 0
00254
00255 dpNode* n;
00256 for(n=Parent(); n; n=n->Parent())
00257 {
00258 LCPNode* ln = (LCPNode*)n;
00259
00260 if(ln->g_in_w == g_in_w && ln->a_in_z == a_in_z)
00261 {
00262 num_loops++;
00263 #ifdef VERBOSE
00264 cerr << "loop; don't open" << endl;
00265 #endif
00266 q_old.resize(0);
00267 return 0;
00268 }
00269 }
00270 #endif
00271 #ifdef INVERSE_FROM_SCRATCH // solve linear equation every time
00272 LCP::Pivot(lcp->Mref(), lcp->Qref(), w2a, w2g, z2a, z2g, new_jr, m_jr, q);
00273 #else // update Maa_inv
00274 LCPNode* lcpnode = (LCPNode*)parent;
00275 LCP::Pivot(lcp->Mref(), lcp->Qref(), lcpnode->Maa_inv, lcpnode->w2a, lcpnode->w2g, lcpnode->z2a, lcpnode->z2g, ys2a, ys2g, yr2a, yr2g, w2a, w2g, z2a, z2g, new_jr, Maa_inv, m_jr, q);
00276 #endif
00277 double error = lcp->CheckPivotResult(q, w2a, w2g);
00278 if(error >= max_error)
00279 {
00280 num_errors++;
00281 #ifdef VERBOSE
00282 cerr << "error is too large (" << error << "); don't open" << endl;
00283 #endif
00284 return 0;
00285 }
00286 }
00287 q_old.resize(0);
00288
00289 #ifdef PIVOT_MINIMUM_ONLY
00290 int min_q_m_index = -1;
00291 double min_q_m = 0.0;
00292 #else
00293 std::vector<int> js_cand;
00294 std::vector<double> js_cost;
00295 std::vector<double> js_g0;
00296 #endif
00297
00298
00299 int current_g0_index = -1;
00300 for(int j=0; j<n_vars; j++)
00301 {
00302 if(w2g[j] == n_vars)
00303 {
00304 current_g0_index = j;
00305 break;
00306 }
00307 }
00308
00309 int g0_index = -1;
00310 for(int j=0; j<n_vars; j++)
00311 {
00312 if(js < 0 || m_jr(j,0) < 0.0)
00313 {
00314 double new_q_g0 = 0.0;
00315
00316 double q_m = min_new_q(q, m_jr, j, max_error, current_g0_index, &new_q_g0);
00317 if(w2g[j] == n_vars && q_m >= -max_error)
00318 {
00319 g0_index = j;
00320 break;
00321 }
00322 #ifdef VERBOSE
00323
00324
00325 #endif
00326 #ifdef PIVOT_MINIMUM_ONLY
00327 if(q_m >= -max_error && (min_q_m_index < 0 || q_m > min_q_m))
00328 {
00329 min_q_m_index = j;
00330 min_q_m = q_m;
00331 }
00332 #else
00333 if(q_m >= -max_error)
00334 {
00335 js_cand.push_back(j);
00336 js_cost.push_back(q_m);
00337 js_g0.push_back(new_q_g0);
00338 #ifdef VERBOSE
00339 cerr << j << ": q_m = " << q_m << ", q_g0 = " << new_q_g0 << endl;
00340 #endif
00341 }
00342 #endif
00343 }
00344 }
00345 #ifdef VERBOSE
00346 if(js_cand.size() == 0)
00347 {
00348 cerr << "no candidate" << endl;
00349 }
00350 #endif
00351
00352 if(g0_index >= 0)
00353 {
00354 #ifdef VERBOSE
00355 cerr << "z0 can be pivoted" << endl;
00356 #endif
00357 nodes = new dpNode* [1];
00358
00359 LCPNode* node = new LCPNode(lcp, this, q, max_error, w2a, w2g, z2a, z2g, n_steps+1, g0_index, new_jr, -1.0);
00360 nodes[0] = (dpNode*)node;
00361 return 1;
00362 }
00363 #ifdef PIVOT_MINIMUM_ONLY
00364 if(min_q_m_index >= 0)
00365 {
00366 nodes = new dpNode* [1];
00367 LCPNode* node = new LCPNode(lcp, this, q, max_error, w2a, w2g, z2a, z2g, n_steps+1, min_q_m_index, new_jr, -min_q_m);
00368 nodes[0] = (dpNode*)node;
00369 return 1;
00370 }
00371 return 0;
00372 #else // #ifdef PIVOT_MINIMUM_ONLY
00373 #if 0
00374
00375 if(js_cand.size() > 1)
00376 {
00377 for(int i=0; i<n_vars; i++)
00378 {
00379 static fMat beta0;
00380 beta0.resize(n_vars, 1);
00381
00382 int a2z = -1;
00383 for(int j=0; j<=n_vars; j++)
00384 {
00385 if(z2a[j] == i)
00386 {
00387 a2z = j;
00388 break;
00389 }
00390 }
00391 if(a2z >= 0)
00392 {
00393 static fVec q0;
00394 q0.resize(n_vars);
00395 LCP::Pivot(lcp->Mref(), lcp->Qref(), w2a, w2g, z2a, z2g, a2z, beta0, q0);
00396 beta0 *= -1.0;
00397 }
00398 else
00399 {
00400
00401 int a2w = -1;
00402 for(int j=0; j<n_vars; j++)
00403 {
00404 if(w2a[j] == i)
00405 {
00406 a2w = j;
00407 break;
00408 }
00409 }
00410 beta0.zero();
00411 beta0(a2w, 0) = 1.0;
00412 }
00413 int minimum_defined = false;
00414 double min_beta = std::numeric_limits<double>::max();
00415 double min_diff = 1e-3;
00416 #ifdef VERBOSE
00417 cerr << "column " << i << ": beta = " << flush;
00418 #endif
00419 for(int j=0; j<js_cand.size(); j++)
00420 {
00421 if(j == 0)
00422 {
00423 min_beta = beta0(js_cand[j],0);
00424 }
00425 else if(fabs(beta0(js_cand[j],0)-min_beta) < min_diff)
00426 {
00427 minimum_defined = false;
00428 }
00429 else if(beta0(js_cand[j],0) < min_beta - min_diff)
00430 {
00431 minimum_defined = true;
00432 min_beta = beta0(js_cand[j],0);
00433 }
00434 #ifdef VERBOSE
00435 cerr << beta0(js_cand[j],0) << " " << flush;
00436 #endif
00437 }
00438 #ifdef VERBOSE
00439 cerr << endl;
00440 #endif
00441 if(minimum_defined)
00442 {
00443 #ifdef VERBOSE
00444 cerr << "minimum found" << endl;
00445 #endif
00446 for(int j=0; j<js_cand.size(); j++)
00447 {
00448 js_cost[j] = -beta0(js_cand[j],0);
00449 }
00450 break;
00451 }
00452 }
00453 }
00454
00455 #endif
00456
00457 nodes = new dpNode* [js_cand.size()];
00458 for(unsigned int j=0; j<js_cand.size(); j++)
00459 {
00460 LCPNode* node = new LCPNode(lcp, this, q, max_error, w2a, w2g, z2a, z2g, n_steps+1, js_cand[j], new_jr, -js_cost[j]);
00461 nodes[j] = (dpNode*)node;
00462 }
00463 return js_cand.size();
00464 #endif
00465 }
00466
00467 double calc_cost() {
00468 return cost;
00469 }
00470
00471 double calc_astar_cost(dpNode* potential_parent) {
00472
00473 return -n_steps;
00474 }
00475
00476 int same_state(dpNode* refnode) {
00477 #ifndef ACTIVATE_SAME_STATE
00478 return false;
00479 #else
00480 if(terminate) return false;
00481 LCPNode* lcp_refnode = (LCPNode*)refnode;
00482 int ret = (lcp_refnode->z2a[lcp_refnode->new_jr] == z2a[new_jr] && lcp_refnode->z2g[lcp_refnode->new_jr] == z2g[new_jr] &&
00483 lcp_refnode->g_in_w == g_in_w && lcp_refnode->a_in_z == a_in_z);
00484 #ifdef VERBOSE
00485 if(ret)
00486 {
00487 cerr << "same_state = " << ret << " (step = " << lcp_refnode->n_steps << ", js = " << lcp_refnode->js << ", jr = " << lcp_refnode->jr << ", z2a[" << new_jr << "] = " << z2a[new_jr] << ", z2g[" << new_jr << "] = " << z2g[new_jr] << ", cost diff = " << lcp_refnode->total_cost - total_cost << ")" << endl;
00488 }
00489 #endif
00490 return ret;
00491 #endif
00492 }
00493
00494 int is_goal() {
00495 return terminate;
00496 }
00497
00498 void calc_new_q(const fVec& q, const fMat& m_jr, int piv, fVec& q_new) {
00499 double q_m = - q(piv) / m_jr(piv, 0);
00500 q_new.set(q);
00501 for(int i=0; i<n_vars; i++)
00502 {
00503 q_new(i) += m_jr(i,0)*q_m;
00504 }
00505 q_new(piv) = q_m;
00506 }
00507
00508 double min_new_q(const fVec& q, const fMat& m_jr, int piv, double max_error, int z0_index = -1, double* new_q_z0 = 0) {
00509 double q_m = - q(piv) / m_jr(piv, 0);
00510
00511 double min_q = q_m;
00512
00513
00514 for(int i=0; i<n_vars; i++)
00515 {
00516 double qi = q(i) + m_jr(i,0)*q_m;
00517 if(qi < -max_error)
00518 {
00519 return qi;
00520 }
00521
00522 if(qi < min_q)
00523 {
00524 min_q = qi;
00525 }
00526 }
00527
00528 if(z0_index >= 0 && new_q_z0)
00529 {
00530 if(z0_index == piv)
00531 {
00532 *new_q_z0 = q_m;
00533 }
00534 else
00535 {
00536 *new_q_z0 = q(z0_index) + m_jr(z0_index,0)*q_m;
00537 }
00538 }
00539 return min_q;
00540 }
00541
00542 double max_error;
00543 int js, jr;
00544 int ys2a, ys2g, yr2a, yr2g;
00545 int n_vars;
00546 LCP* lcp;
00547 int terminate;
00548 int n_steps;
00549 };
00550
00551 int LCPNode::num_loops = 0;
00552 int LCPNode::num_errors = 0;
00553
00554
00555 int LCP::NumLoops() {
00556 return LCPNode::num_loops;
00557 }
00558 int LCP::NumErrors() {
00559 return LCPNode::num_errors;
00560 }
00561
00562 int LCP::SolvePivot2(fVec& g, fVec& a, double _max_error, int _max_nodes, int* n_nodes, std::vector<int>& _g2w)
00563 {
00564 LCPNode::num_loops = 0;
00565 LCPNode::num_errors = 0;
00566 if(n_nodes) *n_nodes = 0;
00567
00568 std::vector<int> z2g, w2g, z2a, w2a;
00569 z2g.resize(n_vars+1);
00570 z2a.resize(n_vars+1);
00571 w2g.resize(n_vars);
00572 w2a.resize(n_vars);
00573 for(int i=0; i<n_vars; i++)
00574 {
00575 z2g[i] = i;
00576 z2a[i] = -1;
00577 w2g[i] = -1;
00578 w2a[i] = i;
00579 }
00580 z2g[n_vars] = n_vars;
00581 z2a[n_vars] = -1;
00582
00583
00584 fMat c(n_vars, 1);
00585 c = 1.0;
00586 M.resize(n_vars, n_vars+1);
00587 q.resize(n_vars);
00588 M.set_submat(0, 0, N);
00589 M.set_submat(0, n_vars, c);
00590 q.set(r);
00591 int max_nodes = _max_nodes;
00592 double max_error = _max_error;
00593 int terminate = true;
00594 #ifdef VERBOSE
00595 cerr << "LCP::SolvePivot2(" << n_vars << ")" << endl;
00596
00597
00598
00599 #endif
00600
00601
00602
00603 #ifdef VERBOSE
00604 cerr << "--- step 0 ---" << endl;
00605 #endif
00606 for(int j=0; j<n_vars; j++)
00607 {
00608 if(q(j) < 0.0)
00609 {
00610 terminate = false;
00611 break;
00612 }
00613 }
00614 if(terminate)
00615 {
00616 #ifdef VERBOSE
00617 cerr << "q >= 0: trivial solution" << endl;
00618 #endif
00619 g.resize(n_vars);
00620 a.resize(n_vars);
00621 g.zero();
00622 a.set(r);
00623 return 0;
00624 }
00625 dpMain* dp = new dpMain;
00626 LCPNode* snode = new LCPNode(this, NULL, q, max_error, w2a, w2g, z2a, z2g, 0, -1, n_vars, 0.0);
00627 dp->SetStartNode((dpNode*)snode);
00628 dp->Search(max_nodes, 1);
00629 dpNode* goal = dp->BestGoal();
00630 if(n_nodes) *n_nodes = dp->NumNodes();
00631
00632
00633
00634 g.resize(n_vars);
00635 a.resize(n_vars);
00636 g.zero();
00637 a.zero();
00638 if(!goal)
00639 {
00640 cerr << "[LCP] solution not found (number of nodes = " << dp->NumNodes() << ", number of variables = " << n_vars << ")" << endl;
00641 #ifdef VERBOSE
00642 cerr << "M = " << M << endl;
00643 cerr << "q = " << tran(q) << endl;
00644 #endif
00645 delete dp;
00646 return -1;
00647 }
00648 LCPNode* lgoal = (LCPNode*)goal;
00649 #ifdef VERBOSE
00650 cerr << "goal found (" << dp->NumNodes() << "/" << lgoal->NumStep() << ")" << endl;
00651 cerr << "num_loops = " << LCPNode::num_loops << endl;
00652 cerr << "w =" << flush;
00653 #endif
00654 for(int j=0; j<n_vars; j++)
00655 {
00656 if(lgoal->w2a[j] >= 0)
00657 {
00658 #ifdef VERBOSE
00659 cerr << "\ta[" << lgoal->w2a[j] << "]" << flush;
00660 #endif
00661 a(lgoal->w2a[j]) = lgoal->q_old(j);
00662 }
00663 else if(lgoal->w2g[j] >= 0 && lgoal->w2g[j] != n_vars)
00664 {
00665 #ifdef VERBOSE
00666 cerr << "\tg[" << lgoal->w2g[j] << "]" << flush;
00667 #endif
00668 g(lgoal->w2g[j]) = lgoal->q_old(j);
00669 }
00670 else
00671 {
00672 assert(0);
00673 }
00674 }
00675 #ifdef VERBOSE
00676 cerr << endl;
00677 cerr << "a = " << tran(a) << endl;
00678 cerr << "g = " << tran(g) << endl;
00679 #endif
00680 if((int)_g2w.size() == n_vars)
00681 {
00682 for(int i=0; i<n_vars; i++)
00683 {
00684 _g2w[i] = -1;
00685 }
00686 for(int i=0; i<n_vars; i++)
00687 {
00688 if(lgoal->w2g[i] >= 0 && lgoal->w2g[i] < n_vars)
00689 {
00690 _g2w[lgoal->w2g[i]] = i;
00691 }
00692 }
00693 }
00694 delete dp;
00695 return 0;
00696 }
00697
00698 int LCP::SolvePivot(fVec& g, fVec& a, double _max_error, int _max_iteration, int* n_iteration, std::vector<int>& _g2w)
00699 {
00700 if(n_iteration) *n_iteration = 0;
00701
00702 fMat M(n_vars, n_vars+1);
00703 fMat c(n_vars, 1);
00704 fVec q(n_vars);
00705 c = 1.0;
00706 M.set_submat(0, 0, N);
00707 M.set_submat(0, n_vars, c);
00708 q.set(r);
00709
00710
00711 std::vector<int> z2g, w2g, z2a, w2a;
00712 int yr2g = -1, yr2a = -1, ys2g = -1, ys2a = -1;
00713 z2g.resize(n_vars+1);
00714 z2a.resize(n_vars+1);
00715 w2g.resize(n_vars);
00716 w2a.resize(n_vars);
00717 for(int i=0; i<n_vars; i++)
00718 {
00719 z2g[i] = i;
00720 z2a[i] = -1;
00721 w2g[i] = -1;
00722 w2a[i] = i;
00723 }
00724 z2g[n_vars] = n_vars;
00725 z2a[n_vars] = -1;
00726
00727 int max_iteration = _max_iteration;
00728 double max_error = _max_error;
00729 int terminate = true;
00730 #ifdef VERBOSE
00731 cerr << "LCP::SolvePivot" << endl;
00732 cerr << "--- inputs ---" << endl;
00733 cerr << "M = " << M << endl;
00734 cerr << "q = " << tran(q) << endl;
00735 #endif
00736
00737
00738
00739 #ifdef VERBOSE
00740 cerr << "--- step 0 ---" << endl;
00741 #endif
00742 for(int j=0; j<n_vars; j++)
00743 {
00744 if(q(j) < 0.0)
00745 {
00746 terminate = false;
00747 break;
00748 }
00749 }
00750 if(terminate)
00751 {
00752 #ifdef VERBOSE
00753 cerr << "q >= 0: trivial solution" << endl;
00754 #endif
00755 g.resize(n_vars);
00756 a.resize(n_vars);
00757 g.zero();
00758 a.set(r);
00759 return 0;
00760 }
00761
00762 int jr = -1;
00763 double min_q_c = 0.0;
00764 for(int j=0; j<n_vars; j++)
00765 {
00766 double q_c = q(j) / M(j, n_vars);
00767 if(jr < 0 || q_c < min_q_c)
00768 {
00769 jr = j;
00770 min_q_c = q_c;
00771 }
00772 }
00773
00774 fMat M_new(n_vars, n_vars+1);
00775 fVec q_new(n_vars);
00776 std::vector<int> idx1, idx2;
00777 idx1.resize(1);
00778 idx2.resize(1);
00779 idx1[0] = jr;
00780 idx2[0] = n_vars;
00781 swap_index(idx1, idx2, w2a, w2g, z2a, z2g);
00782 Pivot(idx1, idx2, M, q, M_new, q_new);
00783 yr2g = z2g[jr];
00784
00785
00786
00787 int n_iter;
00788 for(n_iter=0; n_iter<max_iteration; n_iter++)
00789 {
00790 #if 0
00791 for(int i=0; i<n_vars; i++)
00792 {
00793 cerr << "w[" << i << "]: a/g = " << w2a[i] << "/" << w2g[i] << endl;
00794 }
00795 for(int i=0; i<n_vars+1; i++)
00796 {
00797 cerr << "z[" << i << "]: a/g = " << z2a[i] << "/" << z2g[i] << endl;
00798 }
00799 #endif
00800
00801
00802
00803 #ifdef VERBOSE
00804 cerr << "--- step 1 [iteration " << n_iter << "] ---" << endl;
00805 cerr << "jr = " << jr << endl;
00806 #endif
00807 fMat m_jr(n_vars, 1);
00808 m_jr.get_submat(0, jr, M_new);
00809
00810 double min_q_m = 0.0;
00811 int js = -1;
00812 std::vector<int> js_cand;
00813 for(int j=0; j<n_vars; j++)
00814 {
00815 if(q_new(j) >= 0.0 && m_jr(j,0) < 0.0)
00816 {
00817 double q_m = - q_new(j) / m_jr(j,0);
00818 #ifdef VERBOSE
00819 cerr << "[" << j << "]: z = " << w2g[j] << ", q_new/m_jr = " << q_new(j) << "/" << m_jr(j,0) << " = " << q_m << endl;
00820 #endif
00821 if(js_cand.empty())
00822 {
00823
00824 js_cand.push_back(j);
00825 min_q_m = q_m;
00826 }
00827
00828 else if(fabs(q_m-min_q_m) < max_error)
00829 {
00830
00831 min_q_m = (min_q_m*js_cand.size() + q_m)/(js_cand.size()+1);
00832 js_cand.push_back(j);
00833 }
00834 else if(q_m < min_q_m)
00835 {
00836
00837 js_cand.clear();
00838 js_cand.push_back(j);
00839 min_q_m = q_m;
00840 }
00841 }
00842 }
00843 if(js_cand.empty())
00844 {
00845
00846 cerr << "[LCP] m_jr >= 0: no solution " << endl;
00847
00848 terminate = false;
00849 break;
00850 }
00851 int n_js_cand = js_cand.size();
00852 double min_q_negative = 0.0;
00853 #ifdef VERBOSE
00854 cerr << "n_js_cand = " << n_js_cand << endl;
00855 #endif
00856 for(int j=0; j<n_js_cand; j++)
00857 {
00858 idx1[0] = js_cand[j];
00859 idx2[0] = jr;
00860 swap_index(idx1, idx2, w2a, w2g, z2a, z2g);
00861 Pivot(w2a, w2g, z2a, z2g, M, q, M_new, q_new);
00862 swap_index(idx1, idx2, w2a, w2g, z2a, z2g);
00863 q_new *= -1.0;
00864 double q_negative = q_new.max_value();
00865 #ifdef VERBOSE
00866 cerr << "js_cand[" << j << "] = " << js_cand[j] << endl;
00867 cerr << "q_negative = " << q_negative << endl;
00868 #endif
00869 if(w2g[js_cand[j]] == n_vars && q_negative <= max_error)
00870 {
00871 js = js_cand[j];
00872 break;
00873 }
00874 if(js < 0 || q_negative < min_q_negative)
00875 {
00876 js = js_cand[j];
00877 min_q_negative = q_negative;
00878 }
00879 }
00880
00881 if(js < 0)
00882 {
00883 cerr << "[LCP] no pivot found" << endl;
00884 terminate = false;
00885 break;
00886 }
00887 #ifdef VERBOSE
00888 cerr << "js = " << js << endl;
00889 #endif
00890 if(w2a[js] >= 0)
00891 {
00892 ys2a = w2a[js];
00893 ys2g = -1;
00894 #ifdef VERBOSE
00895 cerr << "ys is a[" << ys2a << "]" << endl;
00896 #endif
00897 }
00898 else if(w2g[js] >= 0)
00899 {
00900 ys2g = w2g[js];
00901 ys2a = -1;
00902 #ifdef VERBOSE
00903 cerr << "ys is g[" << ys2g << "]" << endl;
00904 #endif
00905 }
00906 else
00907 {
00908 assert(0);
00909 }
00910
00911
00912
00913 #ifdef VERBOSE
00914 cerr << "--- step 2 [iteration " << n_iter << "] ---" << endl;
00915 #endif
00916
00917 idx1[0] = js;
00918 idx2[0] = jr;
00919 swap_index(idx1, idx2, w2a, w2g, z2a, z2g);
00920 Pivot(w2a, w2g, z2a, z2g, M, q, M_new, q_new);
00921
00922
00923 #ifdef VERBOSE
00924 cerr << "q_new = " << tran(q_new) << endl;
00925 #endif
00926 #if 1
00927 double error = CheckPivotResult(q_new, w2a, w2g);
00928 if(error > 1e-2)
00929 {
00930
00931 terminate = false;
00932 break;
00933 }
00934 #endif
00935 if(ys2g == n_vars)
00936 {
00937 #ifdef VERBOSE
00938 cerr << "success" << endl;
00939 #endif
00940 terminate = true;
00941 break;
00942 }
00943 if(ys2g >= 0)
00944 {
00945 yr2a = ys2g;
00946 yr2g = -1;
00947 #ifdef VERBOSE
00948 cerr << "yr is a[" << yr2a << "]" << endl;
00949 #endif
00950 jr = -1;
00951 for(int j=0; j<=n_vars; j++)
00952 {
00953 if(z2a[j] == yr2a)
00954 {
00955 jr = j;
00956 break;
00957 }
00958 }
00959 assert(jr >= 0);
00960 }
00961 else if(ys2a >= 0)
00962 {
00963 yr2g = ys2a;
00964 yr2a = -1;
00965 #ifdef VERBOSE
00966 cerr << "yr is g[" << yr2g << "]" << endl;
00967 #endif
00968 jr = -1;
00969 for(int j=0; j<=n_vars; j++)
00970 {
00971 if(z2g[j] == yr2g)
00972 {
00973 jr = j;
00974 break;
00975 }
00976 }
00977 assert(jr >= 0);
00978 }
00979 else
00980 {
00981 assert(0);
00982 }
00983 #ifdef VERBOSE
00984 cerr << "g2w = " << endl;
00985 int* __g2w = new int [n_vars];
00986 for(int i=0; i<n_vars; i++)
00987 {
00988 __g2w[i] = 0;
00989 }
00990 for(int i=0; i<n_vars; i++)
00991 {
00992 if(w2g[i] >= 0 && w2g[i] < n_vars)
00993 {
00994 __g2w[w2g[i]] = 1;
00995 }
00996 }
00997 for(int i=0; i<n_vars; i++)
00998 {
00999 cerr << " " << __g2w[i] << flush;
01000 }
01001 cerr << endl;
01002 delete[] __g2w;
01003 #endif
01004 }
01005 if(n_iter == max_iteration)
01006 {
01007 cerr << "[LCP] did not converge" << endl;
01008 }
01009 g.resize(n_vars);
01010 a.resize(n_vars);
01011 g.zero();
01012 a.zero();
01013 if(terminate)
01014 {
01015 #ifdef VERBOSE
01016 cerr << "q_new = " << tran(q_new) << endl;
01017 #endif
01018 for(int j=0; j<n_vars; j++)
01019 {
01020 if(w2a[j] >= 0)
01021 {
01022 #ifdef VERBOSE
01023 cerr << "w2a[" << j << "] = " << w2a[j] << endl;
01024 #endif
01025 a(w2a[j]) = q_new(j);
01026 }
01027 else if(w2g[j] >= 0 && w2g[j] != n_vars)
01028 {
01029 #ifdef VERBOSE
01030 cerr << "w2g[" << j << "] = " << w2g[j] << endl;
01031 #endif
01032 g(w2g[j]) = q_new(j);
01033 }
01034 else
01035 {
01036 assert(0);
01037 }
01038 }
01039 #ifdef VERBOSE
01040 cerr << "a = " << tran(a) << endl;
01041 cerr << "g = " << tran(g) << endl;
01042 #endif
01043 }
01044 if(n_iteration) *n_iteration = n_iter;
01045 if((int)_g2w.size() == n_vars)
01046 {
01047 for(int i=0; i<n_vars; i++)
01048 {
01049 _g2w[i] = -1;
01050 }
01051 for(int i=0; i<n_vars; i++)
01052 {
01053 if(w2g[i] >= 0 && w2g[i] < n_vars)
01054 {
01055 _g2w[w2g[i]] = i;
01056 }
01057 }
01058 }
01059 return !terminate;
01060 }
01061
01062
01063 void LCP::Pivot(std::vector<int>& w2a, std::vector<int>& w2g,
01064 std::vector<int>& z2a, std::vector<int>& z2g,
01065 const fMat& M, const fVec& q,
01066 fMat& M_new, fVec& q_new)
01067 {
01068 int n_row = M.row(), n_col = M.col();
01069 std::vector<int> a_piv, a_bar, g_piv, g_bar;
01070 M_new.resize(n_row, n_col);
01071 q_new.resize(n_row);
01072
01073 for(int i=0; i<n_row; i++)
01074 {
01075
01076 if(w2g[i] >= 0)
01077 {
01078 g_piv.push_back(w2g[i]);
01079 }
01080 else
01081 {
01082 a_bar.push_back(w2a[i]);
01083 }
01084 }
01085 for(int i=0; i<n_col; i++)
01086 {
01087
01088 if(z2a[i] >= 0) a_piv.push_back(z2a[i]);
01089 else g_bar.push_back(z2g[i]);
01090 }
01091 int n_pivot = g_piv.size();
01092 #ifdef VERBOSE
01093 cerr << "n_pivot = " << n_pivot << endl;
01094 #endif
01095 if(n_pivot == 0)
01096 {
01097 M_new.set(M);
01098 q_new.set(q);
01099 return;
01100 }
01101 int n_row_bar = n_row - n_pivot, n_col_bar = n_col - n_pivot;
01102 fMat M12bar(n_row-n_pivot, n_col-n_pivot);
01103 fMat M1(n_pivot, n_col-n_pivot), M2(n_row-n_pivot, n_pivot);
01104 fMat M12(n_pivot, n_pivot);
01105 fVec q1(n_pivot), q1bar(n_row-n_pivot);
01106 for(int i=0; i<n_pivot; i++)
01107 {
01108 q1(i) = q(a_piv[i]);
01109 for(int j=0; j<n_pivot; j++)
01110 {
01111 M12(i, j) = M(a_piv[i], g_piv[j]);
01112 }
01113 for(int j=0; j<n_col_bar; j++)
01114 {
01115 M1(i, j) = M(a_piv[i], g_bar[j]);
01116 }
01117 }
01118 for(int i=0; i<n_row_bar; i++)
01119 {
01120 q1bar(i) = q(a_bar[i]);
01121 for(int j=0; j<n_pivot; j++)
01122 {
01123 M2(i, j) = M(a_bar[i], g_piv[j]);
01124 }
01125 for(int j=0; j<n_row_bar; j++)
01126 {
01127 M12bar(i, j) = M(a_bar[i], g_bar[j]);
01128 }
01129 }
01130 #if 0
01131 cerr << "M12 = " << M12 << endl;
01132 cerr << "M1 = " << M1 << endl;
01133 cerr << "M2 = " << M2 << endl;
01134 cerr << "M12bar = " << M12bar << endl;
01135 cerr << "q1 = " << tran(q1) << endl;
01136 cerr << "q1bar = " << tran(q1bar) << endl;
01137 #endif
01138 fMat Md12(n_pivot, n_pivot);
01139 fMat Md1(n_pivot, n_col-n_pivot);
01140 fMat Md2(n_row-n_pivot, n_pivot);
01141 fMat Md12bar(n_row-n_pivot, n_col-n_pivot);
01142 fVec qd1(n_pivot), qd1bar(n_row-n_pivot);
01143
01144 pivot_body(M12, M1, M2, M12bar, q1, q1bar, Md12, Md1, Md2, Md12bar, qd1, qd1bar);
01145
01146
01147 int i_piv = 0, i_bar = 0;
01148 for(int i=0; i<n_row; i++)
01149 {
01150 if(w2g[i] >= 0)
01151 {
01152 q_new(i) = qd1(i_piv);
01153 int j_piv = 0, j_bar = 0;
01154 for(int j=0; j<n_col; j++)
01155 {
01156 if(z2a[j] >= 0)
01157 {
01158 M_new(i, j) = Md12(i_piv, j_piv);
01159 j_piv++;
01160 }
01161 else
01162 {
01163 M_new(i, j) = Md1(i_piv, j_bar);
01164 j_bar++;
01165 }
01166 }
01167 i_piv++;
01168 }
01169 else
01170 {
01171 q_new(i) = qd1bar(i_bar);
01172 int j_piv = 0, j_bar = 0;
01173 for(int j=0; j<n_col; j++)
01174 {
01175 if(z2a[j] >= 0)
01176 {
01177 M_new(i, j) = Md2(i_bar, j_piv);
01178 j_piv++;
01179 }
01180 else
01181 {
01182 M_new(i, j) = Md12bar(i_bar, j_bar);
01183 j_bar++;
01184 }
01185 }
01186 i_bar++;
01187 }
01188 }
01189 #if 0
01190
01191
01192 #endif
01193 }
01194
01195
01196 void LCP::Pivot(std::vector<int>& idx1, std::vector<int>& idx2,
01197 const fMat& M, const fVec& q,
01198 fMat& M_new, fVec& q_new)
01199 {
01200 assert(idx1.size() == idx2.size());
01201
01202 int n_pivot = idx1.size();
01203 int n_row = M.row(), n_col = M.col();
01204 std::vector<int> row_pivot_index;
01205 std::vector<int> col_pivot_index;
01206 row_pivot_index.resize(n_row);
01207 col_pivot_index.resize(n_col);
01208 for(int i=0; i<n_row; i++) row_pivot_index[i] = -1;
01209 for(int i=0; i<n_col; i++) col_pivot_index[i] = -1;
01210 for(int i=0; i<n_pivot; i++) row_pivot_index[idx1[i]] = i;
01211 for(int i=0; i<n_pivot; i++) col_pivot_index[idx2[i]] = i;
01212
01213 fMat M12bar(n_row-n_pivot, n_col-n_pivot);
01214 fMat M1(n_pivot, n_col-n_pivot), M2(n_row-n_pivot, n_pivot);
01215 fMat M12(n_pivot, n_pivot);
01216 fVec q1(n_pivot), q1bar(n_row-n_pivot);
01217 int i_bar = 0;
01218 for(int i=0; i<n_row; i++)
01219 {
01220 int i_piv = row_pivot_index[i];
01221 if(i_piv >= 0)
01222 {
01223 q1(i_piv) = q(i);
01224 int j_bar = 0;
01225 for(int j=0; j<n_col; j++)
01226 {
01227 int j_piv = col_pivot_index[j];
01228 if(j_piv >= 0)
01229 {
01230 M12(i_piv, j_piv) = M(i, j);
01231 }
01232 else
01233 {
01234 M1(i_piv, j_bar) = M(i, j);
01235 j_bar++;
01236 }
01237 }
01238 }
01239 else
01240 {
01241 q1bar(i_bar) = q(i);
01242 int j_bar = 0;
01243 for(int j=0; j<n_col; j++)
01244 {
01245 int j_piv = col_pivot_index[j];
01246 if(j_piv >= 0)
01247 {
01248 M2(i_bar, j_piv) = M(i, j);
01249 }
01250 else
01251 {
01252 M12bar(i_bar, j_bar) = M(i, j);
01253 j_bar++;
01254 }
01255 }
01256 i_bar++;
01257 }
01258 }
01259
01260
01261
01262
01263
01264 fMat Md12(n_pivot, n_pivot);
01265 fMat Md1(n_pivot, n_col-n_pivot);
01266 fMat Md2(n_row-n_pivot, n_pivot);
01267 fMat Md12bar(n_row-n_pivot, n_col-n_pivot);
01268 fVec qd1(n_pivot), qd1bar(n_row-n_pivot);
01269
01270 pivot_body(M12, M1, M2, M12bar, q1, q1bar, Md12, Md1, Md2, Md12bar, qd1, qd1bar);
01271
01272
01273 M_new.resize(n_row, n_col);
01274 q_new.resize(n_row);
01275 i_bar = 0;
01276 for(int i=0; i<n_row; i++)
01277 {
01278 int i_piv = row_pivot_index[i];
01279 if(i_piv >= 0)
01280 {
01281 q_new(i) = qd1(i_piv);
01282 int j_bar = 0;
01283 for(int j=0; j<n_col; j++)
01284 {
01285 int j_piv = col_pivot_index[j];
01286 if(j_piv >= 0)
01287 {
01288 M_new(i, j) = Md12(i_piv, j_piv);
01289 }
01290 else
01291 {
01292 M_new(i, j) = Md1(i_piv, j_bar);
01293 j_bar++;
01294 }
01295 }
01296 }
01297 else
01298 {
01299 q_new(i) = qd1bar(i_bar);
01300 int j_bar = 0;
01301 for(int j=0; j<n_col; j++)
01302 {
01303 int j_piv = col_pivot_index[j];
01304 if(j_piv >= 0)
01305 {
01306 M_new(i, j) = Md2(i_bar, j_piv);
01307 }
01308 else
01309 {
01310 M_new(i, j) = Md12bar(i_bar, j_bar);
01311 j_bar++;
01312 }
01313 }
01314 i_bar++;
01315 }
01316 }
01317
01318
01319 }
01320
01321 void LCP::Pivot(int idx1, int idx2,
01322 const fMat& M, const fVec& q,
01323 fMat& M_new, fVec& q_new)
01324 {
01325 int n_row = M.row(), n_col = M.col();
01326 static fMat M12bar, M1, M2, M12;
01327 static fVec q1, q1bar;
01328 int i_bar = 0;
01329 M12bar.resize(n_row-1, n_col-1);
01330 M1.resize(1, n_col-1);
01331 M2.resize(n_row-1, 1);
01332 M12.resize(1, 1);
01333 q1.resize(1);
01334 q1bar.resize(n_row-1);
01335 for(int i=0; i<n_row; i++)
01336 {
01337 if(i == idx1)
01338 {
01339 q1(0) = q(i);
01340 int j_bar = 0;
01341 for(int j=0; j<n_col; j++)
01342 {
01343 if(j == idx2)
01344 {
01345 M12(0, 0) = M(i, j);
01346 }
01347 else
01348 {
01349 M1(0, j_bar) = M(i, j);
01350 j_bar++;
01351 }
01352 }
01353 }
01354 else
01355 {
01356 q1bar(i_bar) = q(i);
01357 int j_bar = 0;
01358 for(int j=0; j<n_col; j++)
01359 {
01360 if(j == idx2)
01361 {
01362 M2(i_bar, 0) = M(i, j);
01363 }
01364 else
01365 {
01366 M12bar(i_bar, j_bar) = M(i, j);
01367 j_bar++;
01368 }
01369 }
01370 i_bar++;
01371 }
01372 }
01373
01374 static fMat Md12;
01375 static fMat Md1;
01376 static fMat Md2;
01377 static fMat Md12bar;
01378 static fVec qd1, qd1bar;
01379 Md12.resize(1, 1);
01380 Md1.resize(1, n_col-1);
01381 Md2.resize(n_row-1, 1);
01382 Md12bar.resize(n_row-1, n_col-1);
01383 qd1.resize(1);
01384 qd1bar.resize(n_row-1);
01385 pivot_body(M12, M1, M2, M12bar, q1, q1bar, Md12, Md1, Md2, Md12bar, qd1, qd1bar);
01386
01387
01388 M_new.resize(n_row, n_col);
01389 q_new.resize(n_row);
01390 i_bar = 0;
01391 for(int i=0; i<n_row; i++)
01392 {
01393 if(i == idx1)
01394 {
01395 q_new(i) = qd1(0);
01396 int j_bar = 0;
01397 for(int j=0; j<n_col; j++)
01398 {
01399 if(j == idx2)
01400 {
01401 M_new(i, j) = Md12(0, 0);
01402 }
01403 else
01404 {
01405 M_new(i, j) = Md1(0, j_bar);
01406 j_bar++;
01407 }
01408 }
01409 }
01410 else
01411 {
01412 q_new(i) = qd1bar(i_bar);
01413 int j_bar = 0;
01414 for(int j=0; j<n_col; j++)
01415 {
01416 if(j == idx2)
01417 {
01418 M_new(i, j) = Md2(i_bar, 0);
01419 }
01420 else
01421 {
01422 M_new(i, j) = Md12bar(i_bar, j_bar);
01423 j_bar++;
01424 }
01425 }
01426 i_bar++;
01427 }
01428 }
01429
01430
01431 }
01432
01433 void LCP::pivot_body(const fMat& M12, const fMat& M1, const fMat& M2, const fMat& M12bar, const fVec& q1, const fVec& q1bar,
01434 fMat& Md12, fMat& Md1, fMat& Md2, fMat& Md12bar, fVec& qd1, fVec& qd1bar)
01435 {
01436 int n_pivot = M12.row();
01437 int n_row_bar = M2.row();
01438 int n_col_bar = M1.col();
01439 static fMat M2Md12, M2Md12M1;
01440 M2Md12.resize(n_row_bar, n_pivot);
01441 M2Md12M1.resize(n_row_bar, n_col_bar);
01442
01443 Md12.inv_svd(M12);
01444 Md1.mul(Md12, M1);
01445 Md1 *= -1.0;
01446 Md2.mul(M2, Md12);
01447 Md12bar.set(M12bar);
01448
01449
01450
01451 M2Md12M1.mul(M2, Md1);
01452 Md12bar += M2Md12M1;
01453
01454 #ifdef VERBOSE
01455
01456 cerr << "M12*Md12 = " << M12*Md12 << endl;
01457
01458
01459
01460 #endif
01461
01462 static fVec qd1bar_temp;
01463 qd1bar_temp.resize(n_row_bar);
01464 qd1.mul(Md12, q1);
01465 qd1 *= -1.0;
01466 qd1bar.set(q1bar);
01467
01468
01469 qd1bar_temp.mul(M2, qd1);
01470 qd1bar += qd1bar_temp;
01471 #ifdef VERBOSE
01472
01473
01474
01475
01476
01477
01478 #endif
01479 }
01480
01481 void LCP::Pivot(const fMat& M, const fVec& q,
01482 const fMat& oldMinv,
01483 std::vector<int>& old_w2a, std::vector<int>& old_w2g,
01484 std::vector<int>& old_z2a, std::vector<int>& old_z2g,
01485 int ys2a, int ys2g, int yr2a, int yr2g,
01486 std::vector<int>& w2a, std::vector<int>& w2g,
01487 std::vector<int>& z2a, std::vector<int>& z2g,
01488 int jr,
01489 fMat& newMinv, fMat& m_jr, fVec& q_new)
01490 {
01491 int n_vars = w2a.size();
01492 std::vector<int> piv2g, piv2a, piv2w, piv2z;
01493 std::vector<int> non2g, non2a, non2w, non2z;
01494 std::vector<int> old_piv2g, old_piv2a;
01495 std::vector<int>::iterator piv2g_iter, piv2w_iter;
01496 std::vector<int>::iterator non2w_iter, non2a_iter;
01497 int i;
01498
01499 for(i=0; i<n_vars; i++)
01500 {
01501
01502 if(old_w2g[i] >= 0) old_piv2g.push_back(old_w2g[i]);
01503 int wgi = w2g[i];
01504 if(wgi >= 0)
01505 {
01506 piv2w.push_back(i);
01507 piv2g.push_back(wgi);
01508 continue;
01509 }
01510 int wai = w2a[i];
01511 if(wai >= 0)
01512 {
01513 non2w.push_back(i);
01514 non2a.push_back(wai);
01515 }
01516 }
01517 int jr2piv = -1, jr2non = -1;
01518
01519 for(i=0; i<=n_vars; i++)
01520 {
01521
01522 if(old_z2a[i] >= 0) old_piv2a.push_back(old_z2a[i]);
01523 int zai = z2a[i];
01524 if(zai >= 0)
01525 {
01526 piv2z.push_back(i);
01527 piv2a.push_back(zai);
01528 if(i == jr) jr2piv = piv2a.size()-1;
01529 continue;
01530 }
01531 int zgi = z2g[i];
01532 if(zgi >= 0)
01533 {
01534 non2z.push_back(i);
01535 non2g.push_back(zgi);
01536 if(i == jr) jr2non = non2g.size()-1;
01537 }
01538 }
01539 assert(piv2g.size() == piv2a.size());
01540 int n_pivot = piv2g.size(), n_none = n_vars-n_pivot;
01541 int jr2a = z2a[jr], jr2g = z2g[jr];
01542 static fMat Maa, b, x;
01543 Maa.resize(n_pivot, n_pivot);
01544 b.resize(n_pivot, 2);
01545 x.resize(n_pivot, 2);
01546 b.zero();
01547
01548 if(jr2a >= 0)
01549 {
01550 assert(jr2piv >= 0);
01551 b(jr2piv, 0) = 1.0;
01552 }
01553 else
01554 {
01555 assert(jr2g >= 0);
01556 assert(jr2non >= 0);
01557 for(i=0; i<n_pivot; i++)
01558 {
01559 b(i, 0) = M(piv2a[i], jr2g);
01560 }
01561 }
01562 for(i=0; i<n_pivot; i++)
01563 {
01564 b(i, 1) = q(piv2a[i]);
01565 }
01566 for(i=0; i<n_pivot; i++)
01567 {
01568 int pai = piv2a[i];
01569 for(int j=0; j<n_pivot; j++)
01570 {
01571 Maa(i, j) = M(pai, piv2g[j]);
01572 }
01573 }
01574
01575 newMinv.resize(n_pivot, n_pivot);
01576 if(ys2a >=0)
01577 {
01578 if(yr2a >= 0)
01579 {
01580 #ifdef VERBOSE
01581 cerr << "row_replaced" << endl;
01582 #endif
01583 static fMat P, q, m2d, X, y;
01584 P.resize(n_pivot, n_pivot-1);
01585 q.resize(n_pivot, 1);
01586 m2d.resize(1, n_pivot);
01587 int count = 0;
01588 for(i=0; i<n_pivot; i++)
01589 {
01590 if(piv2a[i] == ys2a)
01591 {
01592 for(int j=0; j<n_pivot; j++)
01593 {
01594 q(j, 0) = oldMinv(j, i);
01595 }
01596 }
01597 else
01598 {
01599 for(int j=0; j<n_pivot; j++)
01600 {
01601 P(j, count) = oldMinv(j, i);
01602 }
01603 count++;
01604 }
01605 }
01606 for(int j=0; j<n_pivot; j++)
01607 {
01608 m2d(0, j) = M(ys2a, piv2g[j]);
01609 }
01610 inv_row_replaced(P, q, m2d, X, y);
01611 for(i=0; i<n_pivot; i++)
01612 {
01613 count = 0;
01614 for(int j=0; j<n_pivot; j++)
01615 {
01616 if(piv2a[j] == ys2a)
01617 {
01618 newMinv(i,j) = y(i, 0);
01619 }
01620 else
01621 {
01622 newMinv(i,j) = X(i, count);
01623 count++;
01624 }
01625 }
01626 }
01627 }
01628 else if(yr2g >= 0)
01629 {
01630 #ifdef VERBOSE
01631 cerr << "enlarge" << endl;
01632 #endif
01633 static fMat m12, m21, m22, X, y, z, w;
01634 m12.resize(n_pivot-1, 1);
01635 m21.resize(1, n_pivot-1);
01636 m22.resize(1, 1);
01637 int count1 = 0, count2 = 0;
01638 for(i=0; i<n_pivot; i++)
01639 {
01640 if(piv2a[i] != ys2a) m12(count1++, 0) = M(piv2a[i], yr2g);
01641 if(piv2g[i] != yr2g) m21(0, count2++) = M(ys2a, piv2g[i]);
01642 }
01643 m22(0, 0) = M(ys2a, yr2g);
01644 inv_enlarge(m12, m21, m22, oldMinv, X, y, z, w);
01645 count1 = 0;
01646 for(i=0; i<n_pivot; i++)
01647 {
01648 if(piv2g[i] == yr2g)
01649 {
01650 int count2 = 0;
01651 for(int j=0; j<n_pivot; j++)
01652 {
01653 if(piv2a[j] == ys2a)
01654 {
01655 newMinv(i,j) = w(0,0);
01656 }
01657 else
01658 {
01659 newMinv(i,j) = z(0, count2);
01660 count2++;
01661 }
01662 }
01663 }
01664 else
01665 {
01666 int count2 = 0;
01667 for(int j=0; j<n_pivot; j++)
01668 {
01669 if(piv2a[j] == ys2a)
01670 {
01671 newMinv(i,j) = y(count1, 0);
01672 }
01673 else
01674 {
01675 newMinv(i,j) = X(count1, count2);
01676 count2++;
01677 }
01678 }
01679 count1++;
01680 }
01681 }
01682 #ifdef VERBOSE
01683
01684
01685
01686
01687
01688 #endif
01689 }
01690 else
01691 {
01692 assert(0);
01693 }
01694 }
01695 else if(ys2g >= 0)
01696 {
01697 if(yr2a >= 0)
01698 {
01699 #ifdef VERBOSE
01700 cerr << "shrink" << endl;
01701 #endif
01702 static fMat P, q, r, s;
01703 P.resize(n_pivot, n_pivot);
01704 q.resize(n_pivot, 1);
01705 r.resize(1, n_pivot);
01706 s.resize(1, 1);
01707 int count1 = 0;
01708 for(i=0; i<n_pivot+1; i++)
01709 {
01710 if(old_piv2g[i] == ys2g)
01711 {
01712 int count2 = 0;
01713 for(int j=0; j<n_pivot+1; j++)
01714 {
01715 if(old_piv2a[j] == yr2a)
01716 {
01717 s(0,0) = oldMinv(i,j);
01718 }
01719 else
01720 {
01721 r(0, count2) = oldMinv(i,j);
01722 count2++;
01723 }
01724 }
01725 }
01726 else
01727 {
01728 int count2 = 0;
01729 for(int j=0; j<n_pivot+1; j++)
01730 {
01731 if(old_piv2a[j] == yr2a)
01732 {
01733 q(count1,0) = oldMinv(i,j);
01734 }
01735 else
01736 {
01737 P(count1, count2) = oldMinv(i,j);
01738 count2++;
01739 }
01740 }
01741 count1++;
01742 }
01743 }
01744 inv_shrink(P, q, r, s, newMinv);
01745 }
01746 else if(yr2g >= 0)
01747 {
01748 #ifdef VERBOSE
01749 cerr << "col_replaced" << endl;
01750 #endif
01751 static fMat P, q, m2d, X, y;
01752 P.resize(n_pivot-1, n_pivot);
01753 q.resize(1, n_pivot);
01754 m2d.resize(n_pivot, 1);
01755 int count = 0;
01756 for(i=0; i<n_pivot; i++)
01757 {
01758 if(piv2g[i] == yr2g)
01759 {
01760 for(int j=0; j<n_pivot; j++)
01761 {
01762 q(0, j) = oldMinv(i, j);
01763 }
01764 }
01765 else
01766 {
01767 for(int j=0; j<n_pivot; j++)
01768 {
01769 P(count, j) = oldMinv(i, j);
01770 }
01771 count++;
01772 }
01773 }
01774 for(int j=0; j<n_pivot; j++)
01775 {
01776 m2d(j, 0) = M(piv2a[j], yr2g);
01777 }
01778 inv_col_replaced(P, q, m2d, X, y);
01779 count = 0;
01780 for(i=0; i<n_pivot; i++)
01781 {
01782 if(piv2g[i] == yr2g)
01783 {
01784 for(int j=0; j<n_pivot; j++)
01785 {
01786 newMinv(i,j) = y(0, j);
01787 }
01788 }
01789 else
01790 {
01791 for(int j=0; j<n_pivot; j++)
01792 {
01793 newMinv(i,j) = X(count, j);
01794 }
01795 count++;
01796 }
01797 }
01798 }
01799 else
01800 {
01801 assert(0);
01802 }
01803 }
01804 else
01805 {
01806 assert(0);
01807 }
01808 x.mul(newMinv, b);
01809 #ifdef VERBOSE
01810 cerr << "newMinv * Maa = " << newMinv*Maa << endl;
01811 #endif
01812 if(jr2a >= 0)
01813 {
01814 for(i=0; i<n_pivot; i++)
01815 {
01816 m_jr(piv2w[i], 0) = x(i, 0);
01817 }
01818 for(i=0, non2w_iter=non2w.begin(), non2a_iter=non2a.begin(); i<n_none; i++, non2w_iter++, non2a_iter++)
01819 {
01820 int nwi = *non2w_iter, nai = *non2a_iter;
01821 m_jr(nwi, 0) = 0.0;
01822 int j;
01823 for(j=0, piv2g_iter=piv2g.begin(), piv2w_iter=piv2w.begin(); j<n_pivot; j++, piv2g_iter++, piv2w_iter++)
01824 {
01825 m_jr(nwi, 0) += M(nai, *piv2g_iter) * m_jr(*piv2w_iter, 0);
01826 }
01827 }
01828 }
01829 else
01830 {
01831 for(i=0; i<n_pivot; i++)
01832 {
01833 m_jr(piv2w[i], 0) = -x(i, 0);
01834 }
01835 for(i=0, non2w_iter=non2w.begin(), non2a_iter=non2a.begin(); i<n_none; i++, non2w_iter++, non2a_iter++)
01836 {
01837 int nwi = *non2w_iter, nai = *non2a_iter;
01838 m_jr(nwi, 0) = M(nai, jr2g);
01839 int j;
01840 for(j=0, piv2g_iter=piv2g.begin(), piv2w_iter=piv2w.begin(); j<n_pivot; j++, piv2g_iter++, piv2w_iter++)
01841 {
01842 m_jr(nwi, 0) += M(nai, *piv2g_iter) * m_jr(*piv2w_iter, 0);
01843 }
01844 }
01845 }
01846
01847 q_new.resize(n_vars);
01848 q_new.zero();
01849 for(i=0; i<n_pivot; i++)
01850 {
01851 q_new(piv2w[i]) = -x(i, 1);
01852 }
01853
01854 for(i=0, non2w_iter=non2w.begin(), non2a_iter=non2a.begin(); i<n_none; i++, non2w_iter++, non2a_iter++)
01855 {
01856 int nwi = *non2w_iter, nai = *non2a_iter;
01857 q_new(nwi) = q(nai);
01858 int j;
01859 for(j=0, piv2g_iter=piv2g.begin(), piv2w_iter=piv2w.begin(); j<n_pivot; j++, piv2g_iter++, piv2w_iter++)
01860 {
01861 q_new(nwi) += M(nai, *piv2g_iter) * q_new(*piv2w_iter);
01862 }
01863 }
01864 }
01865
01866 void LCP::Pivot(const fMat& M, const fVec& q,
01867 std::vector<int>& w2a, std::vector<int>& w2g,
01868 std::vector<int>& z2a, std::vector<int>& z2g,
01869 int jr,
01870 fMat& m_jr, fVec& q_new)
01871 {
01872 int n_vars = w2a.size();
01873 std::vector<int> piv2g, piv2a, piv2w, piv2z;
01874 std::vector<int> non2g, non2a, non2w, non2z;
01875 std::vector<int>::iterator piv2g_iter, piv2w_iter;
01876 std::vector<int>::iterator non2w_iter, non2a_iter;
01877 int i;
01878
01879 for(i=0; i<n_vars; i++)
01880 {
01881
01882 int wgi = w2g[i];
01883 if(wgi >= 0)
01884 {
01885 piv2w.push_back(i);
01886 piv2g.push_back(wgi);
01887 continue;
01888 }
01889 int wai = w2a[i];
01890 if(wai >= 0)
01891 {
01892 non2w.push_back(i);
01893 non2a.push_back(wai);
01894 }
01895 }
01896 int jr2piv = -1, jr2non = -1;
01897
01898 for(i=0; i<=n_vars; i++)
01899 {
01900
01901 int zai = z2a[i];
01902 if(zai >= 0)
01903 {
01904 piv2z.push_back(i);
01905 piv2a.push_back(zai);
01906 if(i == jr) jr2piv = piv2a.size()-1;
01907 continue;
01908 }
01909 int zgi = z2g[i];
01910 if(zgi >= 0)
01911 {
01912 non2z.push_back(i);
01913 non2g.push_back(zgi);
01914 if(i == jr) jr2non = non2g.size()-1;
01915 }
01916 }
01917 assert(piv2g.size() == piv2a.size());
01918 int n_pivot = piv2g.size(), n_none = n_vars-n_pivot;
01919 int jr2a = z2a[jr], jr2g = z2g[jr];
01920 static fMat Maa, b, x;
01921 Maa.resize(n_pivot, n_pivot);
01922 b.resize(n_pivot, 2);
01923 x.resize(n_pivot, 2);
01924 b.zero();
01925
01926
01927
01928 if(jr2a >= 0)
01929 {
01930 assert(jr2piv >= 0);
01931 b(jr2piv, 0) = 1.0;
01932 }
01933 else
01934 {
01935 assert(jr2g >= 0);
01936 assert(jr2non >= 0);
01937 for(i=0; i<n_pivot; i++)
01938 {
01939 b(i, 0) = M(piv2a[i], jr2g);
01940 }
01941 }
01942 for(i=0; i<n_pivot; i++)
01943 {
01944 b(i, 1) = q(piv2a[i]);
01945 }
01946 for(i=0; i<n_pivot; i++)
01947 {
01948 int pai = piv2a[i];
01949 for(int j=0; j<n_pivot; j++)
01950 {
01951 Maa(i, j) = M(pai, piv2g[j]);
01952 }
01953 }
01954 x.lineq_svd(Maa, b);
01955 if(jr2a >= 0)
01956 {
01957 for(i=0; i<n_pivot; i++)
01958 {
01959 m_jr(piv2w[i], 0) = x(i, 0);
01960 }
01961 for(i=0, non2w_iter=non2w.begin(), non2a_iter=non2a.begin(); i<n_none; i++, non2w_iter++, non2a_iter++)
01962 {
01963 int nwi = *non2w_iter, nai = *non2a_iter;
01964 m_jr(nwi, 0) = 0.0;
01965 int j;
01966 for(j=0, piv2g_iter=piv2g.begin(), piv2w_iter=piv2w.begin(); j<n_pivot; j++, piv2g_iter++, piv2w_iter++)
01967 {
01968 m_jr(nwi, 0) += M(nai, *piv2g_iter) * m_jr(*piv2w_iter, 0);
01969 }
01970 }
01971 }
01972 else
01973 {
01974 for(i=0; i<n_pivot; i++)
01975 {
01976 m_jr(piv2w[i], 0) = -x(i, 0);
01977 }
01978 for(i=0, non2w_iter=non2w.begin(), non2a_iter=non2a.begin(); i<n_none; i++, non2w_iter++, non2a_iter++)
01979 {
01980 int nwi = *non2w_iter, nai = *non2a_iter;
01981 m_jr(nwi, 0) = M(nai, jr2g);
01982 int j;
01983 for(j=0, piv2g_iter=piv2g.begin(), piv2w_iter=piv2w.begin(); j<n_pivot; j++, piv2g_iter++, piv2w_iter++)
01984 {
01985 m_jr(nwi, 0) += M(nai, *piv2g_iter) * m_jr(*piv2w_iter, 0);
01986 }
01987 }
01988 }
01989
01990 q_new.resize(n_vars);
01991 q_new.zero();
01992 for(i=0; i<n_pivot; i++)
01993 {
01994 q_new(piv2w[i]) = -x(i, 1);
01995 }
01996
01997 for(i=0, non2w_iter=non2w.begin(), non2a_iter=non2a.begin(); i<n_none; i++, non2w_iter++, non2a_iter++)
01998 {
01999 int nwi = *non2w_iter, nai = *non2a_iter;
02000 q_new(nwi) = q(nai);
02001 int j;
02002 for(j=0, piv2g_iter=piv2g.begin(), piv2w_iter=piv2w.begin(); j<n_pivot; j++, piv2g_iter++, piv2w_iter++)
02003 {
02004 q_new(nwi) += M(nai, *piv2g_iter) * q_new(*piv2w_iter);
02005 }
02006 }
02007 }
02008
02009 double LCP::CheckPivotResult(const fVec& q_new, std::vector<int>& w2a, std::vector<int>& w2g)
02010 {
02011 static fVec a, g, c;
02012 a.resize(n_vars);
02013 g.resize(n_vars);
02014 c.resize(n_vars);
02015 double g0=0.0;
02016 int g0_found = false;
02017 a.zero();
02018 g.zero();
02019 c.zero();
02020 for(int j=0; j<n_vars; j++)
02021 {
02022 if(w2a[j] >= 0)
02023 {
02024 a(w2a[j]) = q_new(j);
02025 }
02026 else if(w2g[j] >= 0)
02027 {
02028 if(w2g[j] == n_vars)
02029 {
02030 g0 = q_new(j);
02031 g0_found = true;
02032 }
02033 else
02034 {
02035 g(w2g[j]) = q_new(j);
02036 }
02037 }
02038 }
02039 if(g0_found)
02040 {
02041 #ifdef VERBOSE
02042
02043 #endif
02044 c = g0;
02045 }
02046 static fVec error;
02047 error.resize(n_vars);
02048 error.mul(N, g);
02049 error -= a;
02050 error += r;
02051 error += c;
02052 #ifdef VERBOSE
02053
02054 #endif
02055 return error.length();
02056 }