lcp_pivot.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
3  * All rights reserved. This program is made available under the terms of the
4  * Eclipse Public License v1.0 which accompanies this distribution, and is
5  * available at http://www.eclipse.org/legal/epl-v10.html
6  * Contributors:
7  * The University of Tokyo
8  */
16 #include "lcp.h"
17 #include <limits>
18 #include <dp.h>
19 #include <list>
20 
21 //#define VERBOSE
22 
23 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)
24 {
25  int size = idx1.size();
26  for(int i=0; i<size; i++)
27  {
28  int m = idx1[i], n = idx2[i];
29 #ifdef VERBOSE
30  cerr << "swap: w[" << m << "] <-> z[" << n << "]" << endl;
31  cerr << "w[" << m << "]: a/g = " << w2a[m] << "/" << w2g[m] << endl;
32  cerr << "z[" << n << "]: a/g = " << z2a[n] << "/" << z2g[n] << endl;
33 #endif
34  int w2a_org = w2a[m], z2g_org = z2g[n];
35  w2a[m] = z2a[n];
36  z2g[n] = w2g[m];
37  z2a[n] = w2a_org;
38  w2g[m] = z2g_org;
39  }
40 }
41 
42 void swap_index(int idx1, int idx2, std::vector<int>& w2a, std::vector<int>& w2g, std::vector<int>& z2a, std::vector<int>& z2g)
43 {
44  int w2a_org = w2a[idx1], z2g_org = z2g[idx2];
45  w2a[idx1] = z2a[idx2];
46  z2g[idx2] = w2g[idx1];
47  z2a[idx2] = w2a_org;
48  w2g[idx1] = z2g_org;
49 }
50 
51 class LCPNode
52  : public dpNode
53 {
54 public:
55  LCPNode(LCP* _lcp,
56  LCPNode* parent_lcp,
57  const fVec& _q_old,
58  double _max_error,
59  const vector<int>& _w2a, const vector<int>& _w2g,
60  const vector<int>& _z2a, const vector<int>& _z2g,
61  int _n_steps,
62  int _js, int _jr, double _cost) : dpNode() {
63  terminate = false;
64  n_vars = _w2a.size();
65  q_old.resize(n_vars);
66  q_old.set(_q_old);
67  max_error = _max_error;
68  lcp = _lcp;
69  js = _js;
70  jr = _jr;
71  w2a = _w2a;
72  w2g = _w2g;
73  z2a = _z2a;
74  z2g = _z2g;
75  n_steps = _n_steps;
76 // cost = _cost;
77 // cost = _cost-1.0;
78  cost = exp(_cost);
79 // cost = exp(_cost)-1.0;
80 // cost = exp(_cost/max_error)-1.0;
81  if(js >= 0)
82  {
83  ys2a = w2a[js];
84  ys2g = w2g[js];
85  }
86  else
87  {
88  ys2a = -1;
89  ys2g = -1;
90  }
91  if(jr >= 0)
92  {
93  yr2a = z2a[jr];
94  yr2g = z2g[jr];
95  }
96  else
97  {
98  yr2a = -1;
99  yr2g = -1;
100  }
101 #ifdef VERBOSE
102  cerr << "new node: step = " << n_steps << ", cost = " << cost << ", js = " << js << ", jr = " << jr << endl;
103 #endif
104  if(ys2g == n_vars)
105  {
106 #ifdef VERBOSE
107  cerr << "another pivot to terminate" << endl;
108 #endif
109  static fMat m_jr_dummy;
110  m_jr_dummy.resize(n_vars, 1);
111  swap_index(js, jr, w2a, w2g, z2a, z2g);
112 #ifdef INVERSE_FROM_SCRATCH
113  LCP::Pivot(lcp->Mref(), lcp->Qref(), w2a, w2g, z2a, z2g, 0, m_jr_dummy, q_old);
114 #else
115  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);
116 #endif
117  terminate = true;
118  }
119  else if(js >= 0)
120  {
121  swap_index(js, jr, w2a, w2g, z2a, z2g);
122  }
123  for(int i=0; i<n_vars; i++)
124  {
125  if(w2g[i] >= 0)
126  {
127  g_in_w.push_back(w2g[i]);
128  }
129  }
130  for(int i=0; i<=n_vars; i++)
131  {
132  if(z2a[i] >= 0)
133  {
134  a_in_z.push_back(z2a[i]);
135  }
136  }
137  g_in_w.sort();
138  a_in_z.sort();
139 #ifdef VERBOSE
140 #if 1
141  cerr << "g_in_w = [" << flush;
142  for(std::list<int>::iterator i=g_in_w.begin(); i!=g_in_w.end(); i++)
143  {
144  cerr << *i << " " << flush;
145  }
146  cerr << "]" << endl;
147  cerr << "a_in_z = [" << flush;
148  for(std::list<int>::iterator i=a_in_z.begin(); i!=a_in_z.end(); i++)
149  {
150  cerr << *i << " " << flush;
151  }
152  cerr << "]" << endl;
153 #endif
154 #endif
155  new_jr = -1;
156  if(js < 0)
157  {
158  new_jr = jr;
159  }
160  else
161  {
162  // set next yr2a
163  int new_yr2a=0, new_yr2g=0;
164  if(ys2a >= 0)
165  {
166  new_yr2a = -1;
167  new_yr2g = ys2a;
168  }
169  else if(ys2g >= 0)
170  {
171  new_yr2a = ys2g;
172  new_yr2g = -1;
173  }
174  // find next jr
175  if(new_yr2a >= 0)
176  {
177  for(int i=0; i<=n_vars; i++)
178  {
179  if(new_yr2a == z2a[i])
180  {
181  new_jr = i;
182  break;
183  }
184  }
185  }
186  else if(new_yr2g >= 0)
187  {
188  for(int i=0; i<=n_vars; i++)
189  {
190  if(new_yr2g == z2g[i])
191  {
192  new_jr = i;
193  break;
194  }
195  }
196  }
197 #ifdef VERBOSE
198  if(new_jr < 0)
199  {
200  cerr << "new_jr = " << new_jr << endl;
201  cerr << "ys2a = " << ys2a << endl;
202  cerr << "ys2g = " << ys2g << endl;
203  cerr << "new_yr2a = " << new_yr2a << endl;
204  cerr << "new_yr2g = " << new_yr2g << endl;
205  }
206 #endif
207  assert(terminate || new_jr >= 0);
208  }
209 #ifdef VERBOSE
210  cerr << "new_jr = " << new_jr << endl;
211 #endif
212  }
213 
214  ~LCPNode() {
215  }
216 
217  int NumStep() {
218  return n_steps;
219  }
220 
221 #ifndef INVERSE_FROM_SCRATCH
222  fMat Maa_inv;
223 #endif
224  fVec q_old;
225  vector<int> w2a, w2g, z2a, z2g;
226 
227 
228  static int num_loops;
229  static int num_errors;
230 protected:
231  std::list<int> g_in_w, a_in_z;
232  int new_jr;
233 
234  int create_child_nodes(dpNode**& nodes) {
235 #ifdef VERBOSE
236  cerr << "=== create_child_nodes (step = " << n_steps << ", cost = " << cost << ", total cost = " << total_cost << ", total_astar_cost = " << total_astar_cost << ", js = " << js << ", jr = " << jr << ")" << endl;
237 #endif
238  // compute new M, q
239  static fVec q;
240  q.resize(n_vars);
241  static fMat m_jr;
242  m_jr.resize(n_vars, 1);
243  if(js < 0) // initial node: no pivot
244  {
245  q.set(q_old);
246  m_jr.get_submat(0, jr, lcp->Mref());
247 #ifdef VERBOSE
248  cerr << "initial node" << endl;
249 #endif
250  }
251  else
252  {
253 #if 0
254  // duplicate check
255  dpNode* n;
256  for(n=Parent(); n; n=n->Parent())
257  {
258  LCPNode* ln = (LCPNode*)n;
259 // if(ln->w2a == w2a && ln->w2g == w2g)
260  if(ln->g_in_w == g_in_w && ln->a_in_z == a_in_z)
261  {
262  num_loops++;
263 #ifdef VERBOSE
264  cerr << "loop; don't open" << endl;
265 #endif
266  q_old.resize(0);
267  return 0;
268  }
269  }
270 #endif
271 #ifdef INVERSE_FROM_SCRATCH // solve linear equation every time
272  LCP::Pivot(lcp->Mref(), lcp->Qref(), w2a, w2g, z2a, z2g, new_jr, m_jr, q);
273 #else // update Maa_inv
274  LCPNode* lcpnode = (LCPNode*)parent;
275  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);
276 #endif
277  double error = lcp->CheckPivotResult(q, w2a, w2g);
278  if(error >= max_error)
279  {
280  num_errors++;
281 #ifdef VERBOSE
282  cerr << "error is too large (" << error << "); don't open" << endl;
283 #endif
284  return 0;
285  }
286  }
287  q_old.resize(0);
288  // find candidates of ys
289 #ifdef PIVOT_MINIMUM_ONLY
290  int min_q_m_index = -1;
291  double min_q_m = 0.0;
292 #else
293  std::vector<int> js_cand;
294  std::vector<double> js_cost;
295  std::vector<double> js_g0;
296 #endif
297  // create children
298  // find current g0_index
299  int current_g0_index = -1;
300  for(int j=0; j<n_vars; j++)
301  {
302  if(w2g[j] == n_vars)
303  {
304  current_g0_index = j;
305  break;
306  }
307  }
308  // find smallest ratio and get index of g0 in w
309  int g0_index = -1;
310  for(int j=0; j<n_vars; j++)
311  {
312  if(js < 0 || m_jr(j,0) < 0.0)
313  {
314  double new_q_g0 = 0.0;
315  // compute new q when pivoted here
316  double q_m = min_new_q(q, m_jr, j, max_error, current_g0_index, &new_q_g0);
317  if(w2g[j] == n_vars && q_m >= -max_error)
318  {
319  g0_index = j;
320  break;
321  }
322 #ifdef VERBOSE
323 // if(j<50)
324 // cerr << "[" << j << "]: w(a/g) = " << w2a[j] << "/" << w2g[j] << ", q/m_jr = " << q(j) << "/" << m_jr(j,0) << ", q_m = " << q_m << endl;
325 #endif
326 #ifdef PIVOT_MINIMUM_ONLY
327  if(q_m >= -max_error && (min_q_m_index < 0 || q_m > min_q_m))
328  {
329  min_q_m_index = j;
330  min_q_m = q_m;
331  }
332 #else
333  if(q_m >= -max_error)
334  {
335  js_cand.push_back(j);
336  js_cost.push_back(q_m);
337  js_g0.push_back(new_q_g0);
338 #ifdef VERBOSE
339  cerr << j << ": q_m = " << q_m << ", q_g0 = " << new_q_g0 << endl;
340 #endif
341  }
342 #endif
343  }
344  }
345 #ifdef VERBOSE
346  if(js_cand.size() == 0)
347  {
348  cerr << "no candidate" << endl;
349  }
350 #endif
351  // check if we can pivot g0
352  if(g0_index >= 0)
353  {
354 #ifdef VERBOSE
355  cerr << "z0 can be pivoted" << endl;
356 #endif
357  nodes = new dpNode* [1];
358 // LCPNode* node = new LCPNode(lcp, this, q, max_error, w2a, w2g, z2a, z2g, n_steps+1, g0_index, new_jr, 0.0);
359  LCPNode* node = new LCPNode(lcp, this, q, max_error, w2a, w2g, z2a, z2g, n_steps+1, g0_index, new_jr, -1.0);
360  nodes[0] = (dpNode*)node;
361  return 1;
362  }
363 #ifdef PIVOT_MINIMUM_ONLY
364  if(min_q_m_index >= 0)
365  {
366  nodes = new dpNode* [1];
367  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);
368  nodes[0] = (dpNode*)node;
369  return 1;
370  }
371  return 0;
372 #else // #ifdef PIVOT_MINIMUM_ONLY
373 #if 0
374  // test->
375  if(js_cand.size() > 1)
376  {
377  for(int i=0; i<n_vars; i++)
378  {
379  static fMat beta0;
380  beta0.resize(n_vars, 1);
381  // is a[i] in z?
382  int a2z = -1;
383  for(int j=0; j<=n_vars; j++)
384  {
385  if(z2a[j] == i)
386  {
387  a2z = j;
388  break;
389  }
390  }
391  if(a2z >= 0)
392  {
393  static fVec q0;
394  q0.resize(n_vars);
395  LCP::Pivot(lcp->Mref(), lcp->Qref(), w2a, w2g, z2a, z2g, a2z, beta0, q0);
396  beta0 *= -1.0;
397  }
398  else
399  {
400  // find a[i] in w
401  int a2w = -1;
402  for(int j=0; j<n_vars; j++)
403  {
404  if(w2a[j] == i)
405  {
406  a2w = j;
407  break;
408  }
409  }
410  beta0.zero();
411  beta0(a2w, 0) = 1.0;
412  }
413  int minimum_defined = false;
414  double min_beta = std::numeric_limits<double>::max();
415  double min_diff = 1e-3;
416 #ifdef VERBOSE
417  cerr << "column " << i << ": beta = " << flush;
418 #endif
419  for(int j=0; j<js_cand.size(); j++)
420  {
421  if(j == 0)
422  {
423  min_beta = beta0(js_cand[j],0);
424  }
425  else if(fabs(beta0(js_cand[j],0)-min_beta) < min_diff)
426  {
427  minimum_defined = false;
428  }
429  else if(beta0(js_cand[j],0) < min_beta - min_diff)
430  {
431  minimum_defined = true;
432  min_beta = beta0(js_cand[j],0);
433  }
434 #ifdef VERBOSE
435  cerr << beta0(js_cand[j],0) << " " << flush;
436 #endif
437  }
438 #ifdef VERBOSE
439  cerr << endl;
440 #endif
441  if(minimum_defined)
442  {
443 #ifdef VERBOSE
444  cerr << "minimum found" << endl;
445 #endif
446  for(int j=0; j<js_cand.size(); j++)
447  {
448  js_cost[j] = -beta0(js_cand[j],0);
449  }
450  break;
451  }
452  }
453  }
454  // <-test
455 #endif
456 
457  nodes = new dpNode* [js_cand.size()];
458  for(unsigned int j=0; j<js_cand.size(); j++)
459  {
460  LCPNode* node = new LCPNode(lcp, this, q, max_error, w2a, w2g, z2a, z2g, n_steps+1, js_cand[j], new_jr, -js_cost[j]);
461  nodes[j] = (dpNode*)node;
462  }
463  return js_cand.size();
464 #endif
465  }
466 
467  double calc_cost() {
468  return cost;
469  }
470 
471  double calc_astar_cost(dpNode* potential_parent) {
472 // return 0.0;
473  return -n_steps;
474  }
475 
476  int same_state(dpNode* refnode) {
477 #ifndef ACTIVATE_SAME_STATE
478  return false;
479 #else
480  if(terminate) return false;
481  LCPNode* lcp_refnode = (LCPNode*)refnode;
482  int ret = (lcp_refnode->z2a[lcp_refnode->new_jr] == z2a[new_jr] && lcp_refnode->z2g[lcp_refnode->new_jr] == z2g[new_jr] &&
483  lcp_refnode->g_in_w == g_in_w && lcp_refnode->a_in_z == a_in_z);
484 #ifdef VERBOSE
485  if(ret)
486  {
487  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;
488  }
489 #endif
490  return ret;
491 #endif
492  }
493 
494  int is_goal() {
495  return terminate;
496  }
497 
498  void calc_new_q(const fVec& q, const fMat& m_jr, int piv, fVec& q_new) {
499  double q_m = - q(piv) / m_jr(piv, 0);
500  q_new.set(q);
501  for(int i=0; i<n_vars; i++)
502  {
503  q_new(i) += m_jr(i,0)*q_m;
504  }
505  q_new(piv) = q_m;
506  }
507 
508  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) {
509  double q_m = - q(piv) / m_jr(piv, 0);
510 // double min_q = 0.0;
511  double min_q = q_m;
512  // q'(piv) = q_m >= 0
513  // q(piv) + m_jr(piv)*q_m = 0 so it always passes the check
514  for(int i=0; i<n_vars; i++)
515  {
516  double qi = q(i) + m_jr(i,0)*q_m;
517  if(qi < -max_error)
518  {
519  return qi;
520  }
521 // if(qi < min_q && i != piv)
522  if(qi < min_q)
523  {
524  min_q = qi;
525  }
526  }
527  // compute the element of new q corresponding to z0_index
528  if(z0_index >= 0 && new_q_z0)
529  {
530  if(z0_index == piv)
531  {
532  *new_q_z0 = q_m;
533  }
534  else
535  {
536  *new_q_z0 = q(z0_index) + m_jr(z0_index,0)*q_m;
537  }
538  }
539  return min_q;
540  }
541 
542  double max_error;
543  int js, jr;
544  int ys2a, ys2g, yr2a, yr2g;
545  int n_vars;
546  LCP* lcp;
547  int terminate;
548  int n_steps;
549 };
550 
551 int LCPNode::num_loops = 0;
552 int LCPNode::num_errors = 0;
553 
554 // query statistics
555 int LCP::NumLoops() {
556  return LCPNode::num_loops;
557 }
558 int LCP::NumErrors() {
559  return LCPNode::num_errors;
560 }
561 
562 int LCP::SolvePivot2(fVec& g, fVec& a, double _max_error, int _max_nodes, int* n_nodes, std::vector<int>& _g2w)
563 {
564  LCPNode::num_loops = 0;
565  LCPNode::num_errors = 0;
566  if(n_nodes) *n_nodes = 0;
567  // index mapping
568  std::vector<int> z2g, w2g, z2a, w2a;
569  z2g.resize(n_vars+1);
570  z2a.resize(n_vars+1);
571  w2g.resize(n_vars);
572  w2a.resize(n_vars);
573  for(int i=0; i<n_vars; i++)
574  {
575  z2g[i] = i;
576  z2a[i] = -1;
577  w2g[i] = -1;
578  w2a[i] = i;
579  }
580  z2g[n_vars] = n_vars; // z0
581  z2a[n_vars] = -1;
582  // set initial M, q
583  // w = Mz + q, M = [N c]
584  fMat c(n_vars, 1);
585  c = 1.0;
586  M.resize(n_vars, n_vars+1);
587  q.resize(n_vars);
588  M.set_submat(0, 0, N);
589  M.set_submat(0, n_vars, c);
590  q.set(r);
591  int max_nodes = _max_nodes;
592  double max_error = _max_error;
593  int terminate = true;
594 #ifdef VERBOSE
595  cerr << "LCP::SolvePivot2(" << n_vars << ")" << endl;
596 // cerr << "--- inputs ---" << endl;
597 // cerr << "M = " << M << endl;
598 // cerr << "q = " << tran(q) << endl;
599 #endif
600  //
601  // step 0
602  //
603 #ifdef VERBOSE
604  cerr << "--- step 0 ---" << endl;
605 #endif
606  for(int j=0; j<n_vars; j++)
607  {
608  if(q(j) < 0.0)
609  {
610  terminate = false;
611  break;
612  }
613  }
614  if(terminate)
615  {
616 #ifdef VERBOSE
617  cerr << "q >= 0: trivial solution" << endl;
618 #endif
619  g.resize(n_vars);
620  a.resize(n_vars);
621  g.zero();
622  a.set(r);
623  return 0;
624  }
625  dpMain* dp = new dpMain;
626  LCPNode* snode = new LCPNode(this, NULL, q, max_error, w2a, w2g, z2a, z2g, 0, -1, n_vars, 0.0);
627  dp->SetStartNode((dpNode*)snode);
628  dp->Search(max_nodes, 1);
629  dpNode* goal = dp->BestGoal();
630  if(n_nodes) *n_nodes = dp->NumNodes();
631 // cerr << "pivot result:" << endl;
632 // cerr << "M_new = " << M_new << endl;
633 // cerr << "q_new = " << tran(q_new) << endl;
634  g.resize(n_vars);
635  a.resize(n_vars);
636  g.zero();
637  a.zero();
638  if(!goal)
639  {
640  cerr << "[LCP] solution not found (number of nodes = " << dp->NumNodes() << ", number of variables = " << n_vars << ")" << endl;
641 #ifdef VERBOSE
642  cerr << "M = " << M << endl;
643  cerr << "q = " << tran(q) << endl;
644 #endif
645  delete dp;
646  return -1;
647  }
648  LCPNode* lgoal = (LCPNode*)goal;
649 #ifdef VERBOSE
650  cerr << "goal found (" << dp->NumNodes() << "/" << lgoal->NumStep() << ")" << endl;
651  cerr << "num_loops = " << LCPNode::num_loops << endl;
652  cerr << "w =" << flush;
653 #endif
654  for(int j=0; j<n_vars; j++)
655  {
656  if(lgoal->w2a[j] >= 0)
657  {
658 #ifdef VERBOSE
659  cerr << "\ta[" << lgoal->w2a[j] << "]" << flush;
660 #endif
661  a(lgoal->w2a[j]) = lgoal->q_old(j);
662  }
663  else if(lgoal->w2g[j] >= 0 && lgoal->w2g[j] != n_vars)
664  {
665 #ifdef VERBOSE
666  cerr << "\tg[" << lgoal->w2g[j] << "]" << flush;
667 #endif
668  g(lgoal->w2g[j]) = lgoal->q_old(j);
669  }
670  else
671  {
672  assert(0);
673  }
674  }
675 #ifdef VERBOSE
676  cerr << endl;
677  cerr << "a = " << tran(a) << endl;
678  cerr << "g = " << tran(g) << endl;
679 #endif
680  if((int)_g2w.size() == n_vars)
681  {
682  for(int i=0; i<n_vars; i++)
683  {
684  _g2w[i] = -1;
685  }
686  for(int i=0; i<n_vars; i++)
687  {
688  if(lgoal->w2g[i] >= 0 && lgoal->w2g[i] < n_vars)
689  {
690  _g2w[lgoal->w2g[i]] = i;
691  }
692  }
693  }
694  delete dp;
695  return 0;
696 }
697 
698 int LCP::SolvePivot(fVec& g, fVec& a, double _max_error, int _max_iteration, int* n_iteration, std::vector<int>& _g2w)
699 {
700  if(n_iteration) *n_iteration = 0;
701  // w = Mz + q, M = [N c]
702  fMat M(n_vars, n_vars+1);
703  fMat c(n_vars, 1);
704  fVec q(n_vars);
705  c = 1.0;
706  M.set_submat(0, 0, N);
707  M.set_submat(0, n_vars, c);
708  q.set(r);
709 
710  // index mapping
711  std::vector<int> z2g, w2g, z2a, w2a;
712  int yr2g = -1, yr2a = -1, ys2g = -1, ys2a = -1;
713  z2g.resize(n_vars+1);
714  z2a.resize(n_vars+1);
715  w2g.resize(n_vars);
716  w2a.resize(n_vars);
717  for(int i=0; i<n_vars; i++)
718  {
719  z2g[i] = i;
720  z2a[i] = -1;
721  w2g[i] = -1;
722  w2a[i] = i;
723  }
724  z2g[n_vars] = n_vars; // z0
725  z2a[n_vars] = -1;
726 
727  int max_iteration = _max_iteration;
728  double max_error = _max_error;
729  int terminate = true;
730 #ifdef VERBOSE
731  cerr << "LCP::SolvePivot" << endl;
732  cerr << "--- inputs ---" << endl;
733  cerr << "M = " << M << endl;
734  cerr << "q = " << tran(q) << endl;
735 #endif
736  //
737  // step 0
738  //
739 #ifdef VERBOSE
740  cerr << "--- step 0 ---" << endl;
741 #endif
742  for(int j=0; j<n_vars; j++)
743  {
744  if(q(j) < 0.0)
745  {
746  terminate = false;
747  break;
748  }
749  }
750  if(terminate)
751  {
752 #ifdef VERBOSE
753  cerr << "q >= 0: trivial solution" << endl;
754 #endif
755  g.resize(n_vars);
756  a.resize(n_vars);
757  g.zero();
758  a.set(r);
759  return 0;
760  }
761  // find jr
762  int jr = -1;
763  double min_q_c = 0.0;
764  for(int j=0; j<n_vars; j++)
765  {
766  double q_c = q(j) / M(j, n_vars);
767  if(jr < 0 || q_c < min_q_c)
768  {
769  jr = j;
770  min_q_c = q_c;
771  }
772  }
773  // pivot w_jr <-> z0
774  fMat M_new(n_vars, n_vars+1);
775  fVec q_new(n_vars);
776  std::vector<int> idx1, idx2;
777  idx1.resize(1);
778  idx2.resize(1);
779  idx1[0] = jr;
780  idx2[0] = n_vars;
781  swap_index(idx1, idx2, w2a, w2g, z2a, z2g);
782  Pivot(idx1, idx2, M, q, M_new, q_new);
783  yr2g = z2g[jr];
784 // cerr << "pivot result:" << endl;
785 // cerr << "M_new = " << M_new << endl;
786 // cerr << "q_new = " << tran(q_new) << endl;
787  int n_iter;
788  for(n_iter=0; n_iter<max_iteration; n_iter++)
789  {
790 #if 0
791  for(int i=0; i<n_vars; i++)
792  {
793  cerr << "w[" << i << "]: a/g = " << w2a[i] << "/" << w2g[i] << endl;
794  }
795  for(int i=0; i<n_vars+1; i++)
796  {
797  cerr << "z[" << i << "]: a/g = " << z2a[i] << "/" << z2g[i] << endl;
798  }
799 #endif
800  //
801  // step1
802  //
803 #ifdef VERBOSE
804  cerr << "--- step 1 [iteration " << n_iter << "] ---" << endl;
805  cerr << "jr = " << jr << endl;
806 #endif
807  fMat m_jr(n_vars, 1);
808  m_jr.get_submat(0, jr, M_new);
809  // find js
810  double min_q_m = 0.0;
811  int js = -1;
812  std::vector<int> js_cand;
813  for(int j=0; j<n_vars; j++)
814  {
815  if(q_new(j) >= 0.0 && m_jr(j,0) < 0.0)
816  {
817  double q_m = - q_new(j) / m_jr(j,0);
818 #ifdef VERBOSE
819  cerr << "[" << j << "]: z = " << w2g[j] << ", q_new/m_jr = " << q_new(j) << "/" << m_jr(j,0) << " = " << q_m << endl;
820 #endif
821  if(js_cand.empty())
822  {
823 // js = j;
824  js_cand.push_back(j);
825  min_q_m = q_m;
826  }
827 // else if(fabs(q_m-min_q_m) < numeric_limits<double>::epsilon())
828  else if(fabs(q_m-min_q_m) < max_error)
829  {
830 // js = j;
831  min_q_m = (min_q_m*js_cand.size() + q_m)/(js_cand.size()+1);
832  js_cand.push_back(j);
833  }
834  else if(q_m < min_q_m)
835  {
836 // js = j;
837  js_cand.clear();
838  js_cand.push_back(j);
839  min_q_m = q_m;
840  }
841  }
842  }
843  if(js_cand.empty())
844  {
845 //#ifdef VERBOSE
846  cerr << "[LCP] m_jr >= 0: no solution " << endl;
847 //#endif
848  terminate = false;
849  break;
850  }
851  int n_js_cand = js_cand.size();
852  double min_q_negative = 0.0;
853 #ifdef VERBOSE
854  cerr << "n_js_cand = " << n_js_cand << endl;
855 #endif
856  for(int j=0; j<n_js_cand; j++)
857  {
858  idx1[0] = js_cand[j];
859  idx2[0] = jr;
860  swap_index(idx1, idx2, w2a, w2g, z2a, z2g);
861  Pivot(w2a, w2g, z2a, z2g, M, q, M_new, q_new);
862  swap_index(idx1, idx2, w2a, w2g, z2a, z2g);
863  q_new *= -1.0;
864  double q_negative = q_new.max_value();
865 #ifdef VERBOSE
866  cerr << "js_cand[" << j << "] = " << js_cand[j] << endl;
867  cerr << "q_negative = " << q_negative << endl;
868 #endif
869  if(w2g[js_cand[j]] == n_vars && q_negative <= max_error)
870  {
871  js = js_cand[j];
872  break;
873  }
874  if(js < 0 || q_negative < min_q_negative)
875  {
876  js = js_cand[j];
877  min_q_negative = q_negative;
878  }
879  }
880 // if(js < 0 || min_q_negative > max_error)
881  if(js < 0)
882  {
883  cerr << "[LCP] no pivot found" << endl;
884  terminate = false;
885  break;
886  }
887 #ifdef VERBOSE
888  cerr << "js = " << js << endl;
889 #endif
890  if(w2a[js] >= 0)
891  {
892  ys2a = w2a[js];
893  ys2g = -1;
894 #ifdef VERBOSE
895  cerr << "ys is a[" << ys2a << "]" << endl;
896 #endif
897  }
898  else if(w2g[js] >= 0)
899  {
900  ys2g = w2g[js];
901  ys2a = -1;
902 #ifdef VERBOSE
903  cerr << "ys is g[" << ys2g << "]" << endl;
904 #endif
905  }
906  else
907  {
908  assert(0);
909  }
910  //
911  // step 2
912  //
913 #ifdef VERBOSE
914  cerr << "--- step 2 [iteration " << n_iter << "] ---" << endl;
915 #endif
916  // pivot w[js] <-> z[jr]
917  idx1[0] = js;
918  idx2[0] = jr;
919  swap_index(idx1, idx2, w2a, w2g, z2a, z2g);
920  Pivot(w2a, w2g, z2a, z2g, M, q, M_new, q_new);
921 // cerr << "pivot result:" << endl;
922 // cerr << "M_new = " << M_new << endl;
923 #ifdef VERBOSE
924  cerr << "q_new = " << tran(q_new) << endl;
925 #endif
926 #if 1
927  double error = CheckPivotResult(q_new, w2a, w2g);
928  if(error > 1e-2)
929  {
930 // cerr << "[LCP] too large error" << endl;
931  terminate = false;
932  break;
933  }
934 #endif
935  if(ys2g == n_vars) // ys == z0
936  {
937 #ifdef VERBOSE
938  cerr << "success" << endl;
939 #endif
940  terminate = true;
941  break;
942  }
943  if(ys2g >= 0)
944  {
945  yr2a = ys2g;
946  yr2g = -1;
947 #ifdef VERBOSE
948  cerr << "yr is a[" << yr2a << "]" << endl;
949 #endif
950  jr = -1;
951  for(int j=0; j<=n_vars; j++)
952  {
953  if(z2a[j] == yr2a)
954  {
955  jr = j;
956  break;
957  }
958  }
959  assert(jr >= 0);
960  }
961  else if(ys2a >= 0)
962  {
963  yr2g = ys2a;
964  yr2a = -1;
965 #ifdef VERBOSE
966  cerr << "yr is g[" << yr2g << "]" << endl;
967 #endif
968  jr = -1;
969  for(int j=0; j<=n_vars; j++)
970  {
971  if(z2g[j] == yr2g)
972  {
973  jr = j;
974  break;
975  }
976  }
977  assert(jr >= 0);
978  }
979  else
980  {
981  assert(0);
982  }
983 #ifdef VERBOSE
984  cerr << "g2w = " << endl;
985  int* __g2w = new int [n_vars];
986  for(int i=0; i<n_vars; i++)
987  {
988  __g2w[i] = 0;
989  }
990  for(int i=0; i<n_vars; i++)
991  {
992  if(w2g[i] >= 0 && w2g[i] < n_vars)
993  {
994  __g2w[w2g[i]] = 1;
995  }
996  }
997  for(int i=0; i<n_vars; i++)
998  {
999  cerr << " " << __g2w[i] << flush;
1000  }
1001  cerr << endl;
1002  delete[] __g2w;
1003 #endif
1004  }
1005  if(n_iter == max_iteration)
1006  {
1007  cerr << "[LCP] did not converge" << endl;
1008  }
1009  g.resize(n_vars);
1010  a.resize(n_vars);
1011  g.zero();
1012  a.zero();
1013  if(terminate)
1014  {
1015 #ifdef VERBOSE
1016  cerr << "q_new = " << tran(q_new) << endl;
1017 #endif
1018  for(int j=0; j<n_vars; j++)
1019  {
1020  if(w2a[j] >= 0)
1021  {
1022 #ifdef VERBOSE
1023  cerr << "w2a[" << j << "] = " << w2a[j] << endl;
1024 #endif
1025  a(w2a[j]) = q_new(j);
1026  }
1027  else if(w2g[j] >= 0 && w2g[j] != n_vars)
1028  {
1029 #ifdef VERBOSE
1030  cerr << "w2g[" << j << "] = " << w2g[j] << endl;
1031 #endif
1032  g(w2g[j]) = q_new(j);
1033  }
1034  else
1035  {
1036  assert(0);
1037  }
1038  }
1039 #ifdef VERBOSE
1040  cerr << "a = " << tran(a) << endl;
1041  cerr << "g = " << tran(g) << endl;
1042 #endif
1043  }
1044  if(n_iteration) *n_iteration = n_iter;
1045  if((int)_g2w.size() == n_vars)
1046  {
1047  for(int i=0; i<n_vars; i++)
1048  {
1049  _g2w[i] = -1;
1050  }
1051  for(int i=0; i<n_vars; i++)
1052  {
1053  if(w2g[i] >= 0 && w2g[i] < n_vars)
1054  {
1055  _g2w[w2g[i]] = i;
1056  }
1057  }
1058  }
1059  return !terminate;
1060 }
1061 
1062 //
1063 void LCP::Pivot(std::vector<int>& w2a, std::vector<int>& w2g,
1064  std::vector<int>& z2a, std::vector<int>& z2g,
1065  const fMat& M, const fVec& q,
1066  fMat& M_new, fVec& q_new)
1067 {
1068  int n_row = M.row(), n_col = M.col();
1069  std::vector<int> a_piv, a_bar, g_piv, g_bar;
1070  M_new.resize(n_row, n_col);
1071  q_new.resize(n_row);
1072  // count number of g elements in w
1073  for(int i=0; i<n_row; i++)
1074  {
1075 // cerr << "w[" << i << "]: a/g = " << w2a[i] << "/" << w2g[i] << endl;
1076  if(w2g[i] >= 0)
1077  {
1078  g_piv.push_back(w2g[i]);
1079  }
1080  else
1081  {
1082  a_bar.push_back(w2a[i]);
1083  }
1084  }
1085  for(int i=0; i<n_col; i++)
1086  {
1087 // cerr << "z[" << i << "]: a/g = " << z2a[i] << "/" << z2g[i] << endl;
1088  if(z2a[i] >= 0) a_piv.push_back(z2a[i]);
1089  else g_bar.push_back(z2g[i]);
1090  }
1091  int n_pivot = g_piv.size();
1092 #ifdef VERBOSE
1093  cerr << "n_pivot = " << n_pivot << endl;
1094 #endif
1095  if(n_pivot == 0)
1096  {
1097  M_new.set(M);
1098  q_new.set(q);
1099  return;
1100  }
1101  int n_row_bar = n_row - n_pivot, n_col_bar = n_col - n_pivot;
1102  fMat M12bar(n_row-n_pivot, n_col-n_pivot);
1103  fMat M1(n_pivot, n_col-n_pivot), M2(n_row-n_pivot, n_pivot);
1104  fMat M12(n_pivot, n_pivot);
1105  fVec q1(n_pivot), q1bar(n_row-n_pivot);
1106  for(int i=0; i<n_pivot; i++)
1107  {
1108  q1(i) = q(a_piv[i]);
1109  for(int j=0; j<n_pivot; j++)
1110  {
1111  M12(i, j) = M(a_piv[i], g_piv[j]);
1112  }
1113  for(int j=0; j<n_col_bar; j++)
1114  {
1115  M1(i, j) = M(a_piv[i], g_bar[j]);
1116  }
1117  }
1118  for(int i=0; i<n_row_bar; i++)
1119  {
1120  q1bar(i) = q(a_bar[i]);
1121  for(int j=0; j<n_pivot; j++)
1122  {
1123  M2(i, j) = M(a_bar[i], g_piv[j]);
1124  }
1125  for(int j=0; j<n_row_bar; j++)
1126  {
1127  M12bar(i, j) = M(a_bar[i], g_bar[j]);
1128  }
1129  }
1130 #if 0
1131  cerr << "M12 = " << M12 << endl;
1132  cerr << "M1 = " << M1 << endl;
1133  cerr << "M2 = " << M2 << endl;
1134  cerr << "M12bar = " << M12bar << endl;
1135  cerr << "q1 = " << tran(q1) << endl;
1136  cerr << "q1bar = " << tran(q1bar) << endl;
1137 #endif
1138  fMat Md12(n_pivot, n_pivot);
1139  fMat Md1(n_pivot, n_col-n_pivot);
1140  fMat Md2(n_row-n_pivot, n_pivot);
1141  fMat Md12bar(n_row-n_pivot, n_col-n_pivot);
1142  fVec qd1(n_pivot), qd1bar(n_row-n_pivot);
1143 
1144  pivot_body(M12, M1, M2, M12bar, q1, q1bar, Md12, Md1, Md2, Md12bar, qd1, qd1bar);
1145 
1146  // output
1147  int i_piv = 0, i_bar = 0;
1148  for(int i=0; i<n_row; i++)
1149  {
1150  if(w2g[i] >= 0)
1151  {
1152  q_new(i) = qd1(i_piv);
1153  int j_piv = 0, j_bar = 0;
1154  for(int j=0; j<n_col; j++)
1155  {
1156  if(z2a[j] >= 0)
1157  {
1158  M_new(i, j) = Md12(i_piv, j_piv);
1159  j_piv++;
1160  }
1161  else
1162  {
1163  M_new(i, j) = Md1(i_piv, j_bar);
1164  j_bar++;
1165  }
1166  }
1167  i_piv++;
1168  }
1169  else
1170  {
1171  q_new(i) = qd1bar(i_bar);
1172  int j_piv = 0, j_bar = 0;
1173  for(int j=0; j<n_col; j++)
1174  {
1175  if(z2a[j] >= 0)
1176  {
1177  M_new(i, j) = Md2(i_bar, j_piv);
1178  j_piv++;
1179  }
1180  else
1181  {
1182  M_new(i, j) = Md12bar(i_bar, j_bar);
1183  j_bar++;
1184  }
1185  }
1186  i_bar++;
1187  }
1188  }
1189 #if 0
1190 // cerr << "M_new = " << M_new << endl;
1191 // cerr << "q_new = " << tran(q_new) << endl;
1192 #endif
1193 }
1194 
1195 // pivot w[idx1] and z[idx2] in w=Mz+q
1196 void LCP::Pivot(std::vector<int>& idx1, std::vector<int>& idx2,
1197  const fMat& M, const fVec& q,
1198  fMat& M_new, fVec& q_new)
1199 {
1200  assert(idx1.size() == idx2.size());
1201 // cerr << "pivot->" << endl;
1202  int n_pivot = idx1.size();
1203  int n_row = M.row(), n_col = M.col();
1204  std::vector<int> row_pivot_index;
1205  std::vector<int> col_pivot_index;
1206  row_pivot_index.resize(n_row);
1207  col_pivot_index.resize(n_col);
1208  for(int i=0; i<n_row; i++) row_pivot_index[i] = -1;
1209  for(int i=0; i<n_col; i++) col_pivot_index[i] = -1;
1210  for(int i=0; i<n_pivot; i++) row_pivot_index[idx1[i]] = i;
1211  for(int i=0; i<n_pivot; i++) col_pivot_index[idx2[i]] = i;
1212 
1213  fMat M12bar(n_row-n_pivot, n_col-n_pivot);
1214  fMat M1(n_pivot, n_col-n_pivot), M2(n_row-n_pivot, n_pivot);
1215  fMat M12(n_pivot, n_pivot);
1216  fVec q1(n_pivot), q1bar(n_row-n_pivot);
1217  int i_bar = 0;
1218  for(int i=0; i<n_row; i++)
1219  {
1220  int i_piv = row_pivot_index[i];
1221  if(i_piv >= 0)
1222  {
1223  q1(i_piv) = q(i);
1224  int j_bar = 0;
1225  for(int j=0; j<n_col; j++)
1226  {
1227  int j_piv = col_pivot_index[j];
1228  if(j_piv >= 0)
1229  {
1230  M12(i_piv, j_piv) = M(i, j);
1231  }
1232  else
1233  {
1234  M1(i_piv, j_bar) = M(i, j);
1235  j_bar++;
1236  }
1237  }
1238  }
1239  else
1240  {
1241  q1bar(i_bar) = q(i);
1242  int j_bar = 0;
1243  for(int j=0; j<n_col; j++)
1244  {
1245  int j_piv = col_pivot_index[j];
1246  if(j_piv >= 0)
1247  {
1248  M2(i_bar, j_piv) = M(i, j);
1249  }
1250  else
1251  {
1252  M12bar(i_bar, j_bar) = M(i, j);
1253  j_bar++;
1254  }
1255  }
1256  i_bar++;
1257  }
1258  }
1259 // cerr << "M12 = " << M12 << endl;
1260 // cerr << "M1 = " << M1 << endl;
1261 // cerr << "M2 = " << M2 << endl;
1262 // cerr << "M12bar = " << M12bar << endl;
1263  // new M
1264  fMat Md12(n_pivot, n_pivot);
1265  fMat Md1(n_pivot, n_col-n_pivot);
1266  fMat Md2(n_row-n_pivot, n_pivot);
1267  fMat Md12bar(n_row-n_pivot, n_col-n_pivot);
1268  fVec qd1(n_pivot), qd1bar(n_row-n_pivot);
1269 
1270  pivot_body(M12, M1, M2, M12bar, q1, q1bar, Md12, Md1, Md2, Md12bar, qd1, qd1bar);
1271 
1272  // output
1273  M_new.resize(n_row, n_col);
1274  q_new.resize(n_row);
1275  i_bar = 0;
1276  for(int i=0; i<n_row; i++)
1277  {
1278  int i_piv = row_pivot_index[i];
1279  if(i_piv >= 0)
1280  {
1281  q_new(i) = qd1(i_piv);
1282  int j_bar = 0;
1283  for(int j=0; j<n_col; j++)
1284  {
1285  int j_piv = col_pivot_index[j];
1286  if(j_piv >= 0)
1287  {
1288  M_new(i, j) = Md12(i_piv, j_piv);
1289  }
1290  else
1291  {
1292  M_new(i, j) = Md1(i_piv, j_bar);
1293  j_bar++;
1294  }
1295  }
1296  }
1297  else
1298  {
1299  q_new(i) = qd1bar(i_bar);
1300  int j_bar = 0;
1301  for(int j=0; j<n_col; j++)
1302  {
1303  int j_piv = col_pivot_index[j];
1304  if(j_piv >= 0)
1305  {
1306  M_new(i, j) = Md2(i_bar, j_piv);
1307  }
1308  else
1309  {
1310  M_new(i, j) = Md12bar(i_bar, j_bar);
1311  j_bar++;
1312  }
1313  }
1314  i_bar++;
1315  }
1316  }
1317 // cerr << "M_new = " << M_new << endl;
1318 // cerr << "<- pivot" << endl;
1319 }
1320 
1321 void LCP::Pivot(int idx1, int idx2,
1322  const fMat& M, const fVec& q,
1323  fMat& M_new, fVec& q_new)
1324 {
1325  int n_row = M.row(), n_col = M.col();
1326  static fMat M12bar, M1, M2, M12;
1327  static fVec q1, q1bar;
1328  int i_bar = 0;
1329  M12bar.resize(n_row-1, n_col-1);
1330  M1.resize(1, n_col-1);
1331  M2.resize(n_row-1, 1);
1332  M12.resize(1, 1);
1333  q1.resize(1);
1334  q1bar.resize(n_row-1);
1335  for(int i=0; i<n_row; i++)
1336  {
1337  if(i == idx1)
1338  {
1339  q1(0) = q(i);
1340  int j_bar = 0;
1341  for(int j=0; j<n_col; j++)
1342  {
1343  if(j == idx2)
1344  {
1345  M12(0, 0) = M(i, j);
1346  }
1347  else
1348  {
1349  M1(0, j_bar) = M(i, j);
1350  j_bar++;
1351  }
1352  }
1353  }
1354  else
1355  {
1356  q1bar(i_bar) = q(i);
1357  int j_bar = 0;
1358  for(int j=0; j<n_col; j++)
1359  {
1360  if(j == idx2)
1361  {
1362  M2(i_bar, 0) = M(i, j);
1363  }
1364  else
1365  {
1366  M12bar(i_bar, j_bar) = M(i, j);
1367  j_bar++;
1368  }
1369  }
1370  i_bar++;
1371  }
1372  }
1373  // new M
1374  static fMat Md12;
1375  static fMat Md1;
1376  static fMat Md2;
1377  static fMat Md12bar;
1378  static fVec qd1, qd1bar;
1379  Md12.resize(1, 1);
1380  Md1.resize(1, n_col-1);
1381  Md2.resize(n_row-1, 1);
1382  Md12bar.resize(n_row-1, n_col-1);
1383  qd1.resize(1);
1384  qd1bar.resize(n_row-1);
1385  pivot_body(M12, M1, M2, M12bar, q1, q1bar, Md12, Md1, Md2, Md12bar, qd1, qd1bar);
1386 
1387  // output
1388  M_new.resize(n_row, n_col);
1389  q_new.resize(n_row);
1390  i_bar = 0;
1391  for(int i=0; i<n_row; i++)
1392  {
1393  if(i == idx1)
1394  {
1395  q_new(i) = qd1(0);
1396  int j_bar = 0;
1397  for(int j=0; j<n_col; j++)
1398  {
1399  if(j == idx2)
1400  {
1401  M_new(i, j) = Md12(0, 0);
1402  }
1403  else
1404  {
1405  M_new(i, j) = Md1(0, j_bar);
1406  j_bar++;
1407  }
1408  }
1409  }
1410  else
1411  {
1412  q_new(i) = qd1bar(i_bar);
1413  int j_bar = 0;
1414  for(int j=0; j<n_col; j++)
1415  {
1416  if(j == idx2)
1417  {
1418  M_new(i, j) = Md2(i_bar, 0);
1419  }
1420  else
1421  {
1422  M_new(i, j) = Md12bar(i_bar, j_bar);
1423  j_bar++;
1424  }
1425  }
1426  i_bar++;
1427  }
1428  }
1429 // cerr << "M_new = " << M_new << endl;
1430 // cerr << "<- pivot" << endl;
1431 }
1432 
1433 void LCP::pivot_body(const fMat& M12, const fMat& M1, const fMat& M2, const fMat& M12bar, const fVec& q1, const fVec& q1bar,
1434  fMat& Md12, fMat& Md1, fMat& Md2, fMat& Md12bar, fVec& qd1, fVec& qd1bar)
1435 {
1436  int n_pivot = M12.row();
1437  int n_row_bar = M2.row();
1438  int n_col_bar = M1.col();
1439  static fMat M2Md12, M2Md12M1;
1440  M2Md12.resize(n_row_bar, n_pivot);
1441  M2Md12M1.resize(n_row_bar, n_col_bar);
1442 
1443  Md12.inv_svd(M12);
1444  Md1.mul(Md12, M1);
1445  Md1 *= -1.0;
1446  Md2.mul(M2, Md12);
1447  Md12bar.set(M12bar);
1448 // M2Md12.mul(M2, Md12);
1449 // M2Md12M1.mul(M2Md12, M1);
1450 // Md12bar -= M2Md12M1;
1451  M2Md12M1.mul(M2, Md1);
1452  Md12bar += M2Md12M1;
1453 
1454 #ifdef VERBOSE
1455 // cerr << "Md12 = " << Md12 << endl;
1456  cerr << "M12*Md12 = " << M12*Md12 << endl;
1457 // cerr << "Md1 = " << Md1 << endl;
1458 // cerr << "Md2 = " << Md2 << endl;
1459 // cerr << "Md_bar = " << Md_bar << endl;
1460 #endif
1461  // new q
1462  static fVec qd1bar_temp;
1463  qd1bar_temp.resize(n_row_bar);
1464  qd1.mul(Md12, q1);
1465  qd1 *= -1.0;
1466  qd1bar.set(q1bar);
1467 // qd1bar_temp.mul(M2Md12, q1);
1468 // qd1bar -= qd1bar_temp;
1469  qd1bar_temp.mul(M2, qd1);
1470  qd1bar += qd1bar_temp;
1471 #ifdef VERBOSE
1472 // cerr << "Md12 = " << Md12 << endl;
1473 // cerr << "Md1 = " << Md1 << endl;
1474 // cerr << "Md2 = " << Md2 << endl;
1475 // cerr << "Md12bar = " << Md12bar << endl;
1476 // cerr << "qd1 = " << tran(qd1) << endl;
1477 // cerr << "qd1bar = " << tran(qd1bar) << endl;
1478 #endif
1479 }
1480 
1481 void LCP::Pivot(const fMat& M, const fVec& q,
1482  const fMat& oldMinv,
1483  std::vector<int>& old_w2a, std::vector<int>& old_w2g,
1484  std::vector<int>& old_z2a, std::vector<int>& old_z2g,
1485  int ys2a, int ys2g, int yr2a, int yr2g,
1486  std::vector<int>& w2a, std::vector<int>& w2g,
1487  std::vector<int>& z2a, std::vector<int>& z2g,
1488  int jr,
1489  fMat& newMinv, fMat& m_jr, fVec& q_new)
1490 {
1491  int n_vars = w2a.size();
1492  std::vector<int> piv2g, piv2a, piv2w, piv2z;
1493  std::vector<int> non2g, non2a, non2w, non2z;
1494  std::vector<int> old_piv2g, old_piv2a;
1495  std::vector<int>::iterator piv2g_iter, piv2w_iter;
1496  std::vector<int>::iterator non2w_iter, non2a_iter;
1497  int i;
1498 // cerr << "w = " << endl;
1499  for(i=0; i<n_vars; i++)
1500  {
1501 // cerr << " [" << i << "] g/a = " << w2g[i] << "/" << w2a[i] << endl;
1502  if(old_w2g[i] >= 0) old_piv2g.push_back(old_w2g[i]);
1503  int wgi = w2g[i];
1504  if(wgi >= 0)
1505  {
1506  piv2w.push_back(i);
1507  piv2g.push_back(wgi);
1508  continue;
1509  }
1510  int wai = w2a[i];
1511  if(wai >= 0)
1512  {
1513  non2w.push_back(i);
1514  non2a.push_back(wai);
1515  }
1516  }
1517  int jr2piv = -1, jr2non = -1;
1518 // cerr << "z = " << endl;
1519  for(i=0; i<=n_vars; i++) // z has n_vars+1 elements
1520  {
1521 // cerr << " [" << i << "] a/g = " << z2a[i] << "/" << z2g[i] << endl;
1522  if(old_z2a[i] >= 0) old_piv2a.push_back(old_z2a[i]);
1523  int zai = z2a[i];
1524  if(zai >= 0)
1525  {
1526  piv2z.push_back(i);
1527  piv2a.push_back(zai);
1528  if(i == jr) jr2piv = piv2a.size()-1;
1529  continue;
1530  }
1531  int zgi = z2g[i];
1532  if(zgi >= 0)
1533  {
1534  non2z.push_back(i);
1535  non2g.push_back(zgi);
1536  if(i == jr) jr2non = non2g.size()-1;
1537  }
1538  }
1539  assert(piv2g.size() == piv2a.size());
1540  int n_pivot = piv2g.size(), n_none = n_vars-n_pivot;
1541  int jr2a = z2a[jr], jr2g = z2g[jr];
1542  static fMat Maa, b, x;
1543  Maa.resize(n_pivot, n_pivot);
1544  b.resize(n_pivot, 2);
1545  x.resize(n_pivot, 2);
1546  b.zero();
1547  // b = [e(jr) or m_ab(jr) | q_a]
1548  if(jr2a >= 0)
1549  {
1550  assert(jr2piv >= 0);
1551  b(jr2piv, 0) = 1.0;
1552  }
1553  else
1554  {
1555  assert(jr2g >= 0);
1556  assert(jr2non >= 0);
1557  for(i=0; i<n_pivot; i++)
1558  {
1559  b(i, 0) = M(piv2a[i], jr2g);
1560  }
1561  }
1562  for(i=0; i<n_pivot; i++)
1563  {
1564  b(i, 1) = q(piv2a[i]);
1565  }
1566  for(i=0; i<n_pivot; i++)
1567  {
1568  int pai = piv2a[i];
1569  for(int j=0; j<n_pivot; j++)
1570  {
1571  Maa(i, j) = M(pai, piv2g[j]);
1572  }
1573  }
1574  // compute new Minv
1575  newMinv.resize(n_pivot, n_pivot);
1576  if(ys2a >=0)
1577  {
1578  if(yr2a >= 0) // row_replaced
1579  {
1580 #ifdef VERBOSE
1581  cerr << "row_replaced" << endl;
1582 #endif
1583  static fMat P, q, m2d, X, y;
1584  P.resize(n_pivot, n_pivot-1);
1585  q.resize(n_pivot, 1);
1586  m2d.resize(1, n_pivot);
1587  int count = 0;
1588  for(i=0; i<n_pivot; i++)
1589  {
1590  if(piv2a[i] == ys2a)
1591  {
1592  for(int j=0; j<n_pivot; j++)
1593  {
1594  q(j, 0) = oldMinv(j, i);
1595  }
1596  }
1597  else
1598  {
1599  for(int j=0; j<n_pivot; j++)
1600  {
1601  P(j, count) = oldMinv(j, i);
1602  }
1603  count++;
1604  }
1605  }
1606  for(int j=0; j<n_pivot; j++)
1607  {
1608  m2d(0, j) = M(ys2a, piv2g[j]);
1609  }
1610  inv_row_replaced(P, q, m2d, X, y);
1611  for(i=0; i<n_pivot; i++)
1612  {
1613  count = 0;
1614  for(int j=0; j<n_pivot; j++)
1615  {
1616  if(piv2a[j] == ys2a)
1617  {
1618  newMinv(i,j) = y(i, 0);
1619  }
1620  else
1621  {
1622  newMinv(i,j) = X(i, count);
1623  count++;
1624  }
1625  }
1626  }
1627  }
1628  else if(yr2g >= 0) // enlarge
1629  {
1630 #ifdef VERBOSE
1631  cerr << "enlarge" << endl;
1632 #endif
1633  static fMat m12, m21, m22, X, y, z, w;
1634  m12.resize(n_pivot-1, 1);
1635  m21.resize(1, n_pivot-1);
1636  m22.resize(1, 1);
1637  int count1 = 0, count2 = 0;
1638  for(i=0; i<n_pivot; i++)
1639  {
1640  if(piv2a[i] != ys2a) m12(count1++, 0) = M(piv2a[i], yr2g);
1641  if(piv2g[i] != yr2g) m21(0, count2++) = M(ys2a, piv2g[i]);
1642  }
1643  m22(0, 0) = M(ys2a, yr2g);
1644  inv_enlarge(m12, m21, m22, oldMinv, X, y, z, w);
1645  count1 = 0;
1646  for(i=0; i<n_pivot; i++)
1647  {
1648  if(piv2g[i] == yr2g)
1649  {
1650  int count2 = 0;
1651  for(int j=0; j<n_pivot; j++)
1652  {
1653  if(piv2a[j] == ys2a)
1654  {
1655  newMinv(i,j) = w(0,0);
1656  }
1657  else
1658  {
1659  newMinv(i,j) = z(0, count2);
1660  count2++;
1661  }
1662  }
1663  }
1664  else
1665  {
1666  int count2 = 0;
1667  for(int j=0; j<n_pivot; j++)
1668  {
1669  if(piv2a[j] == ys2a)
1670  {
1671  newMinv(i,j) = y(count1, 0);
1672  }
1673  else
1674  {
1675  newMinv(i,j) = X(count1, count2);
1676  count2++;
1677  }
1678  }
1679  count1++;
1680  }
1681  }
1682 #ifdef VERBOSE
1683 // cerr << "Maa = " << Maa << endl;
1684 // cerr << "m12 = " << m12 << endl;
1685 // cerr << "m21 = " << m21 << endl;
1686 // cerr << "m22 = " << m22 << endl;
1687 // cerr << "newMinv = " << newMinv << endl;
1688 #endif
1689  }
1690  else
1691  {
1692  assert(0);
1693  }
1694  }
1695  else if(ys2g >= 0)
1696  {
1697  if(yr2a >= 0) // shrink
1698  {
1699 #ifdef VERBOSE
1700  cerr << "shrink" << endl;
1701 #endif
1702  static fMat P, q, r, s;
1703  P.resize(n_pivot, n_pivot);
1704  q.resize(n_pivot, 1);
1705  r.resize(1, n_pivot);
1706  s.resize(1, 1);
1707  int count1 = 0;
1708  for(i=0; i<n_pivot+1; i++)
1709  {
1710  if(old_piv2g[i] == ys2g)
1711  {
1712  int count2 = 0;
1713  for(int j=0; j<n_pivot+1; j++)
1714  {
1715  if(old_piv2a[j] == yr2a)
1716  {
1717  s(0,0) = oldMinv(i,j);
1718  }
1719  else
1720  {
1721  r(0, count2) = oldMinv(i,j);
1722  count2++;
1723  }
1724  }
1725  }
1726  else
1727  {
1728  int count2 = 0;
1729  for(int j=0; j<n_pivot+1; j++)
1730  {
1731  if(old_piv2a[j] == yr2a)
1732  {
1733  q(count1,0) = oldMinv(i,j);
1734  }
1735  else
1736  {
1737  P(count1, count2) = oldMinv(i,j);
1738  count2++;
1739  }
1740  }
1741  count1++;
1742  }
1743  }
1744  inv_shrink(P, q, r, s, newMinv);
1745  }
1746  else if(yr2g >= 0) // col_replaced
1747  {
1748 #ifdef VERBOSE
1749  cerr << "col_replaced" << endl;
1750 #endif
1751  static fMat P, q, m2d, X, y;
1752  P.resize(n_pivot-1, n_pivot);
1753  q.resize(1, n_pivot);
1754  m2d.resize(n_pivot, 1);
1755  int count = 0;
1756  for(i=0; i<n_pivot; i++)
1757  {
1758  if(piv2g[i] == yr2g)
1759  {
1760  for(int j=0; j<n_pivot; j++)
1761  {
1762  q(0, j) = oldMinv(i, j);
1763  }
1764  }
1765  else
1766  {
1767  for(int j=0; j<n_pivot; j++)
1768  {
1769  P(count, j) = oldMinv(i, j);
1770  }
1771  count++;
1772  }
1773  }
1774  for(int j=0; j<n_pivot; j++)
1775  {
1776  m2d(j, 0) = M(piv2a[j], yr2g);
1777  }
1778  inv_col_replaced(P, q, m2d, X, y);
1779  count = 0;
1780  for(i=0; i<n_pivot; i++)
1781  {
1782  if(piv2g[i] == yr2g)
1783  {
1784  for(int j=0; j<n_pivot; j++)
1785  {
1786  newMinv(i,j) = y(0, j);
1787  }
1788  }
1789  else
1790  {
1791  for(int j=0; j<n_pivot; j++)
1792  {
1793  newMinv(i,j) = X(count, j);
1794  }
1795  count++;
1796  }
1797  }
1798  }
1799  else
1800  {
1801  assert(0);
1802  }
1803  }
1804  else
1805  {
1806  assert(0);
1807  }
1808  x.mul(newMinv, b);
1809 #ifdef VERBOSE
1810  cerr << "newMinv * Maa = " << newMinv*Maa << endl;
1811 #endif
1812  if(jr2a >= 0)
1813  {
1814  for(i=0; i<n_pivot; i++)
1815  {
1816  m_jr(piv2w[i], 0) = x(i, 0);
1817  }
1818  for(i=0, non2w_iter=non2w.begin(), non2a_iter=non2a.begin(); i<n_none; i++, non2w_iter++, non2a_iter++)
1819  {
1820  int nwi = *non2w_iter, nai = *non2a_iter;
1821  m_jr(nwi, 0) = 0.0;
1822  int j;
1823  for(j=0, piv2g_iter=piv2g.begin(), piv2w_iter=piv2w.begin(); j<n_pivot; j++, piv2g_iter++, piv2w_iter++)
1824  {
1825  m_jr(nwi, 0) += M(nai, *piv2g_iter) * m_jr(*piv2w_iter, 0);
1826  }
1827  }
1828  }
1829  else
1830  {
1831  for(i=0; i<n_pivot; i++)
1832  {
1833  m_jr(piv2w[i], 0) = -x(i, 0);
1834  }
1835  for(i=0, non2w_iter=non2w.begin(), non2a_iter=non2a.begin(); i<n_none; i++, non2w_iter++, non2a_iter++)
1836  {
1837  int nwi = *non2w_iter, nai = *non2a_iter;
1838  m_jr(nwi, 0) = M(nai, jr2g);
1839  int j;
1840  for(j=0, piv2g_iter=piv2g.begin(), piv2w_iter=piv2w.begin(); j<n_pivot; j++, piv2g_iter++, piv2w_iter++)
1841  {
1842  m_jr(nwi, 0) += M(nai, *piv2g_iter) * m_jr(*piv2w_iter, 0);
1843  }
1844  }
1845  }
1846 // cerr << "m_jr = " << tran(m_jr) << endl;
1847  q_new.resize(n_vars);
1848  q_new.zero();
1849  for(i=0; i<n_pivot; i++)
1850  {
1851  q_new(piv2w[i]) = -x(i, 1);
1852  }
1853 // cerr << "q_new(temp) = " << tran(q_new) << endl;
1854  for(i=0, non2w_iter=non2w.begin(), non2a_iter=non2a.begin(); i<n_none; i++, non2w_iter++, non2a_iter++)
1855  {
1856  int nwi = *non2w_iter, nai = *non2a_iter;
1857  q_new(nwi) = q(nai);
1858  int j;
1859  for(j=0, piv2g_iter=piv2g.begin(), piv2w_iter=piv2w.begin(); j<n_pivot; j++, piv2g_iter++, piv2w_iter++)
1860  {
1861  q_new(nwi) += M(nai, *piv2g_iter) * q_new(*piv2w_iter);
1862  }
1863  }
1864 }
1865 
1866 void LCP::Pivot(const fMat& M, const fVec& q,
1867  std::vector<int>& w2a, std::vector<int>& w2g,
1868  std::vector<int>& z2a, std::vector<int>& z2g,
1869  int jr,
1870  fMat& m_jr, fVec& q_new)
1871 {
1872  int n_vars = w2a.size();
1873  std::vector<int> piv2g, piv2a, piv2w, piv2z;
1874  std::vector<int> non2g, non2a, non2w, non2z;
1875  std::vector<int>::iterator piv2g_iter, piv2w_iter;
1876  std::vector<int>::iterator non2w_iter, non2a_iter;
1877  int i;
1878 // cerr << "w = " << endl;
1879  for(i=0; i<n_vars; i++)
1880  {
1881 // cerr << " [" << i << "] g/a = " << w2g[i] << "/" << w2a[i] << endl;
1882  int wgi = w2g[i];
1883  if(wgi >= 0)
1884  {
1885  piv2w.push_back(i);
1886  piv2g.push_back(wgi);
1887  continue;
1888  }
1889  int wai = w2a[i];
1890  if(wai >= 0)
1891  {
1892  non2w.push_back(i);
1893  non2a.push_back(wai);
1894  }
1895  }
1896  int jr2piv = -1, jr2non = -1;
1897 // cerr << "z = " << endl;
1898  for(i=0; i<=n_vars; i++) // z has n_vars+1 elements
1899  {
1900 // cerr << " [" << i << "] a/g = " << z2a[i] << "/" << z2g[i] << endl;
1901  int zai = z2a[i];
1902  if(zai >= 0)
1903  {
1904  piv2z.push_back(i);
1905  piv2a.push_back(zai);
1906  if(i == jr) jr2piv = piv2a.size()-1;
1907  continue;
1908  }
1909  int zgi = z2g[i];
1910  if(zgi >= 0)
1911  {
1912  non2z.push_back(i);
1913  non2g.push_back(zgi);
1914  if(i == jr) jr2non = non2g.size()-1;
1915  }
1916  }
1917  assert(piv2g.size() == piv2a.size());
1918  int n_pivot = piv2g.size(), n_none = n_vars-n_pivot;
1919  int jr2a = z2a[jr], jr2g = z2g[jr];
1920  static fMat Maa, b, x;
1921  Maa.resize(n_pivot, n_pivot);
1922  b.resize(n_pivot, 2);
1923  x.resize(n_pivot, 2);
1924  b.zero();
1925 // cerr << "jr = " << jr << ", jr2piv = " << jr2piv << ", jr2non = " << jr2non << endl;
1926 // cerr << "original q = " << tran(q) << endl;
1927  // b = [e(jr) or m_ab(jr) | q_a]
1928  if(jr2a >= 0)
1929  {
1930  assert(jr2piv >= 0);
1931  b(jr2piv, 0) = 1.0;
1932  }
1933  else
1934  {
1935  assert(jr2g >= 0);
1936  assert(jr2non >= 0);
1937  for(i=0; i<n_pivot; i++)
1938  {
1939  b(i, 0) = M(piv2a[i], jr2g);
1940  }
1941  }
1942  for(i=0; i<n_pivot; i++)
1943  {
1944  b(i, 1) = q(piv2a[i]);
1945  }
1946  for(i=0; i<n_pivot; i++)
1947  {
1948  int pai = piv2a[i];
1949  for(int j=0; j<n_pivot; j++)
1950  {
1951  Maa(i, j) = M(pai, piv2g[j]);
1952  }
1953  }
1954  x.lineq_svd(Maa, b);
1955  if(jr2a >= 0)
1956  {
1957  for(i=0; i<n_pivot; i++)
1958  {
1959  m_jr(piv2w[i], 0) = x(i, 0);
1960  }
1961  for(i=0, non2w_iter=non2w.begin(), non2a_iter=non2a.begin(); i<n_none; i++, non2w_iter++, non2a_iter++)
1962  {
1963  int nwi = *non2w_iter, nai = *non2a_iter;
1964  m_jr(nwi, 0) = 0.0;
1965  int j;
1966  for(j=0, piv2g_iter=piv2g.begin(), piv2w_iter=piv2w.begin(); j<n_pivot; j++, piv2g_iter++, piv2w_iter++)
1967  {
1968  m_jr(nwi, 0) += M(nai, *piv2g_iter) * m_jr(*piv2w_iter, 0);
1969  }
1970  }
1971  }
1972  else
1973  {
1974  for(i=0; i<n_pivot; i++)
1975  {
1976  m_jr(piv2w[i], 0) = -x(i, 0);
1977  }
1978  for(i=0, non2w_iter=non2w.begin(), non2a_iter=non2a.begin(); i<n_none; i++, non2w_iter++, non2a_iter++)
1979  {
1980  int nwi = *non2w_iter, nai = *non2a_iter;
1981  m_jr(nwi, 0) = M(nai, jr2g);
1982  int j;
1983  for(j=0, piv2g_iter=piv2g.begin(), piv2w_iter=piv2w.begin(); j<n_pivot; j++, piv2g_iter++, piv2w_iter++)
1984  {
1985  m_jr(nwi, 0) += M(nai, *piv2g_iter) * m_jr(*piv2w_iter, 0);
1986  }
1987  }
1988  }
1989 // cerr << "m_jr = " << tran(m_jr) << endl;
1990  q_new.resize(n_vars);
1991  q_new.zero();
1992  for(i=0; i<n_pivot; i++)
1993  {
1994  q_new(piv2w[i]) = -x(i, 1);
1995  }
1996 // cerr << "q_new(temp) = " << tran(q_new) << endl;
1997  for(i=0, non2w_iter=non2w.begin(), non2a_iter=non2a.begin(); i<n_none; i++, non2w_iter++, non2a_iter++)
1998  {
1999  int nwi = *non2w_iter, nai = *non2a_iter;
2000  q_new(nwi) = q(nai);
2001  int j;
2002  for(j=0, piv2g_iter=piv2g.begin(), piv2w_iter=piv2w.begin(); j<n_pivot; j++, piv2g_iter++, piv2w_iter++)
2003  {
2004  q_new(nwi) += M(nai, *piv2g_iter) * q_new(*piv2w_iter);
2005  }
2006  }
2007 }
2008 
2009 double LCP::CheckPivotResult(const fVec& q_new, std::vector<int>& w2a, std::vector<int>& w2g)
2010 {
2011  static fVec a, g, c;
2012  a.resize(n_vars);
2013  g.resize(n_vars);
2014  c.resize(n_vars);
2015  double g0=0.0;
2016  int g0_found = false;
2017  a.zero();
2018  g.zero();
2019  c.zero();
2020  for(int j=0; j<n_vars; j++)
2021  {
2022  if(w2a[j] >= 0)
2023  {
2024  a(w2a[j]) = q_new(j);
2025  }
2026  else if(w2g[j] >= 0)
2027  {
2028  if(w2g[j] == n_vars)
2029  {
2030  g0 = q_new(j);
2031  g0_found = true;
2032  }
2033  else
2034  {
2035  g(w2g[j]) = q_new(j);
2036  }
2037  }
2038  }
2039  if(g0_found)
2040  {
2041 #ifdef VERBOSE
2042 // cerr << "g0 = " << g0 << endl;
2043 #endif
2044  c = g0;
2045  }
2046  static fVec error;
2047  error.resize(n_vars);
2048  error.mul(N, g);
2049  error -= a;
2050  error += r;
2051  error += c;
2052 #ifdef VERBOSE
2053 // cerr << "error = " << tran(error) << endl;
2054 #endif
2055  return error.length();
2056 }
inv_row_replaced
int inv_row_replaced(const fMat &P, const fMat &q, const fMat &m2d, fMat &X, fMat &y)
Definition: fMatrix.cpp:1334
autoplay.c
int c
Definition: autoplay.py:16
LCPNode::calc_cost
double calc_cost()
Compute the local cost at the node.
Definition: lcp_pivot.cpp:467
LCP::CheckPivotResult
double CheckPivotResult(const fVec &q_new, std::vector< int > &w2a, std::vector< int > &w2g)
Definition: lcp_pivot.cpp:2009
dpNode::total_astar_cost
double total_astar_cost
Definition: dp.h:164
fMat::resize
int resize(int i, int j)
Changes the matrix size.
Definition: fMatrix.h:133
dpNode::total_cost
double total_cost
Definition: dp.h:163
LCPNode::yr2g
int yr2g
Definition: lcp_pivot.cpp:544
LCPNode::n_vars
int n_vars
Definition: lcp_pivot.cpp:545
LCP::NumErrors
int NumErrors()
Definition: lcp_pivot.cpp:558
LCPNode::w2g
vector< int > w2g
Definition: lcp_pivot.cpp:225
x
* x
Definition: IceUtils.h:98
LCPNode::NumStep
int NumStep()
Definition: lcp_pivot.cpp:217
fMat::col
int col() const
Returns the number of columns.
Definition: fMatrix.h:154
LCPNode::lcp
LCP * lcp
Definition: lcp_pivot.cpp:546
X
Definition: clap.cpp:13
fMat::row
int row() const
Returns the number of rows.
Definition: fMatrix.h:158
size
png_uint_32 size
Definition: png.h:1521
ret
RTC::ReturnCode_t ret(RTC::Local::ReturnCode_t r)
LCP::Pivot
static void Pivot(int idx1, int idx2, const fMat &M, const fVec &q, fMat &M_new, fVec &q_new)
Definition: lcp_pivot.cpp:1321
dpMain::NumNodes
int NumNodes()
Definition: dp.h:345
dpMain::Search
int Search(int _max_nodes=-1, int _max_goals=-1)
Dijkstra or A* search: find the node with the smallest cost.
Definition: dp.cpp:17
inv_shrink
int inv_shrink(const fMat &P, const fMat &q, const fMat &r, const fMat &s, fMat &X)
Definition: fMatrix.cpp:1318
dp.h
Base class for generic discrete search.
error
void error(char *msg) const
Definition: minigzip.c:87
LCP::pivot_body
static void pivot_body(const fMat &M12, const fMat &M1, const fMat &M2, const fMat &M12bar, const fVec &q1, const fVec &q1bar, fMat &Md12, fMat &Md1, fMat &Md2, fMat &Md12bar, fVec &qd1, fVec &qd1bar)
Definition: lcp_pivot.cpp:1433
w
w
swap_index
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)
Definition: lcp_pivot.cpp:23
i
png_uint_32 i
Definition: png.h:2735
LCPNode::min_new_q
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)
Definition: lcp_pivot.cpp:508
b
long b
Definition: jpegint.h:371
LCPNode::calc_new_q
void calc_new_q(const fVec &q, const fMat &m_jr, int piv, fVec &q_new)
Definition: lcp_pivot.cpp:498
dpNode::cost
double cost
Definition: dp.h:156
LCPNode::ys2g
int ys2g
Definition: lcp_pivot.cpp:544
dpNode::parent
dpNode * parent
Definition: dp.h:160
dpMain::SetStartNode
void SetStartNode(dpNode *_n)
Set the initial node for search.
Definition: dp.h:303
LCPNode::g_in_w
std::list< int > g_in_w
Definition: lcp_pivot.cpp:231
fMat::mul
void mul(const fMat &mat1, const fMat &mat2)
Definition: fMatrix.cpp:1217
fMat::zero
void zero()
Creates a zero matrix.
Definition: fMatrix.h:249
LCPNode::ys2a
int ys2a
Definition: lcp_pivot.cpp:544
LCPNode::create_child_nodes
int create_child_nodes(dpNode **&nodes)
Create (potential) child nodes.
Definition: lcp_pivot.cpp:234
LCPNode::calc_astar_cost
double calc_astar_cost(dpNode *potential_parent)
Compute the A-star cost at the node (optional).
Definition: lcp_pivot.cpp:471
fVec::set
void set(double *_d)
Sets all elements.
Definition: fMatrix.h:533
LCP::SolvePivot
int SolvePivot(fVec &g, fVec &a, double _max_error, int _max_iteration, int *n_iteration, std::vector< int > &_g2w)
Solve using Lemke&#39;s method.
Definition: lcp_pivot.cpp:698
fVec::mul
void mul(const fVec &vec, double d)
Definition: fMatrix.cpp:1620
LCPNode::q_old
fVec q_old
Definition: lcp_pivot.cpp:224
LCPNode::Maa_inv
fMat Maa_inv
Definition: lcp_pivot.cpp:222
dpNode::dpMain
friend class dpMain
Definition: dp.h:32
j
def j(str, encoding="cp932")
LCPNode::n_steps
int n_steps
Definition: lcp_pivot.cpp:548
LCPNode::num_errors
static int num_errors
Definition: lcp_pivot.cpp:229
LCP::NumLoops
int NumLoops()
Definition: lcp_pivot.cpp:555
lcp.h
Solves a Linear Complementarity Problem (LCP)
fMat::lineq_svd
friend fMat lineq_svd(const fMat &A, const fMat &b, int lwork)
solve linear equation Ax = b
Definition: fMatrix.cpp:416
fMat::get_submat
void get_submat(int row_start, int col_start, const fMat &allM)
Extract a sub-matrix and set to myself.
Definition: fMatrix.cpp:32
a
string a
LCPNode::is_goal
int is_goal()
Check if the state is goal.
Definition: lcp_pivot.cpp:494
fVec::resize
void resize(int i)
Change the size.
Definition: fMatrix.h:511
LCPNode::same_state
int same_state(dpNode *refnode)
Check if the node&#39;s state is the same as refnode. (to remove duplicates)
Definition: lcp_pivot.cpp:476
fVec
Vector of generic size.
Definition: fMatrix.h:491
dpMain
Definition: dp.h:286
LCP
Definition: lcp.h:32
dpNode::Parent
dpNode * Parent()
Definition: dp.h:52
tran
fMat tran(const fMat &mat)
Definition: fMatrix.cpp:1013
LCPNode::w2a
vector< int > w2a
Definition: lcp_pivot.cpp:225
LCPNode::max_error
double max_error
Definition: lcp_pivot.cpp:542
LCPNode::z2g
vector< int > z2g
Definition: lcp_pivot.cpp:225
fVec::zero
void zero()
Creates a zero vector.
Definition: fMatrix.h:575
fMat::set_submat
void set_submat(int row_start, int col_start, const fMat &subM)
Sets a sub-matrix of myself.
Definition: fMatrix.cpp:48
inv_enlarge
int inv_enlarge(const fMat &m12, const fMat &m21, const fMat &m22, const fMat &P, fMat &X, fMat &y, fMat &z, fMat &w)
Definition: fMatrix.cpp:1288
fMat::inv_svd
friend fMat inv_svd(const fMat &mat, int lwork)
inverse
Definition: fMatrix.cpp:484
fMat::set
void set(double *_d)
Sets the values from an array.
Definition: fMatrix.h:187
LCP::SolvePivot2
int SolvePivot2(fVec &g, fVec &a, double _max_error, int _max_iteration, int *n_iteration, std::vector< int > &_g2w)
Solve by pivot using path planning algorithm.
Definition: lcp_pivot.cpp:562
fVec::length
friend double length(const fVec &v)
Length of the vector.
Definition: fMatrix.h:549
LCPNode::terminate
int terminate
Definition: lcp_pivot.cpp:547
LCPNode::z2a
vector< int > z2a
Definition: lcp_pivot.cpp:225
LCPNode::new_jr
int new_jr
Definition: lcp_pivot.cpp:232
LCPNode::num_loops
static int num_loops
Definition: lcp_pivot.cpp:228
LCP::Qref
const fVec & Qref()
Definition: lcp.h:124
inv_col_replaced
int inv_col_replaced(const fMat &P, const fMat &q, const fMat &m2d, fMat &X, fMat &y)
Definition: fMatrix.cpp:1357
fVec::max_value
double max_value()
Returns the maximum value.
Definition: fMatrix.cpp:1478
y
* y
Definition: IceUtils.h:97
LCPNode::LCPNode
LCPNode(LCP *_lcp, LCPNode *parent_lcp, const fVec &_q_old, double _max_error, const vector< int > &_w2a, const vector< int > &_w2g, const vector< int > &_z2a, const vector< int > &_z2g, int _n_steps, int _js, int _jr, double _cost)
Definition: lcp_pivot.cpp:55
dpMain::BestGoal
dpNode * BestGoal(dpNode *ref=0)
Extract the goal with the smallest cost (if any).
Definition: dp.h:336
dpNode
Definition: dp.h:29
LCP::Mref
const fMat & Mref()
Definition: lcp.h:121
hrp::max
static int max(int a, int b)
Definition: MatrixSolvers.cpp:54
fMat
Matrix of generic size. The elements are stored in a one-dimensional array in row-major order...
Definition: fMatrix.h:46
LCPNode::a_in_z
std::list< int > a_in_z
Definition: lcp_pivot.cpp:231
LCPNode::yr2a
int yr2a
Definition: lcp_pivot.cpp:544

openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sat May 8 2021 02:42:39