lcp_pivot.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00003  * All rights reserved. This program is made available under the terms of the
00004  * Eclipse Public License v1.0 which accompanies this distribution, and is
00005  * available at http://www.eclipse.org/legal/epl-v10.html
00006  * Contributors:
00007  * The University of Tokyo
00008  */
00016 #include "lcp.h"
00017 #include <limits>
00018 #include <dp.h>
00019 #include <list>
00020 
00021 //#define VERBOSE
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 //              cost = _cost;
00077 //              cost = _cost-1.0;
00078                 cost = exp(_cost);
00079 //              cost = exp(_cost)-1.0;
00080 //              cost = exp(_cost/max_error)-1.0;
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                         // set next yr2a
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                         // find next jr
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                 // compute new M, q
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)  // initial node: no pivot
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                         // duplicate check
00255                         dpNode* n;
00256                         for(n=Parent(); n; n=n->Parent())
00257                         {
00258                                 LCPNode* ln = (LCPNode*)n;
00259 //                              if(ln->w2a == w2a && ln->w2g == w2g)
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                 // find candidates of ys
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                 // create children
00298                 // find current g0_index
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                 // find smallest ratio and get index of g0 in w
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                                 // compute new q when pivoted here
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 //                              if(j<50)
00324 //                                      cerr << "[" << j << "]: w(a/g) = " << w2a[j] << "/" << w2g[j] << ", q/m_jr = " << q(j) << "/" << m_jr(j,0) << ", q_m = " << q_m << endl;
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                 // check if we can pivot g0
00352                 if(g0_index >= 0)
00353                 {
00354 #ifdef VERBOSE
00355                         cerr << "z0 can be pivoted" << endl;
00356 #endif
00357                         nodes = new dpNode* [1];
00358 //                      LCPNode* node = new LCPNode(lcp, this, q, max_error, w2a, w2g, z2a, z2g, n_steps+1, g0_index, new_jr, 0.0);
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                 // test->
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                         // is a[i] in z?
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                                 // find a[i] in w
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                 // <-test
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 //              return 0.0;
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 //              double min_q = 0.0;
00511                 double min_q = q_m;
00512                 // q'(piv) = q_m >= 0
00513                 // q(piv) + m_jr(piv)*q_m = 0 so it always passes the check
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 //                      if(qi < min_q && i != piv)
00522                         if(qi < min_q)
00523                         {
00524                                 min_q = qi;
00525                         }
00526                 }
00527                 // compute the element of new q corresponding to z0_index
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 // query statistics
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         // index mapping
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;  // z0
00581         z2a[n_vars] = -1;
00582         // set initial M, q
00583         // w = Mz + q, M = [N c]
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 //      cerr << "--- inputs ---" << endl;
00597 //      cerr << "M = " << M << endl;
00598 //      cerr << "q = " << tran(q) << endl;
00599 #endif
00600         //
00601         // step 0
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 //      cerr << "pivot result:" << endl;
00632 //      cerr << "M_new = " << M_new << endl;
00633 //      cerr << "q_new = " << tran(q_new) << endl;
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         // w = Mz + q, M = [N c]
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         // index mapping
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;  // z0
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         // step 0
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         // find jr
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         // pivot w_jr <-> z0
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 //      cerr << "pivot result:" << endl;
00785 //      cerr << "M_new = " << M_new << endl;
00786 //      cerr << "q_new = " << tran(q_new) << endl;
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                 // step1
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                 // find js
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 //                                      js = j;
00824                                         js_cand.push_back(j);
00825                                         min_q_m = q_m;
00826                                 }
00827 //                              else if(fabs(q_m-min_q_m) < numeric_limits<double>::epsilon())
00828                                 else if(fabs(q_m-min_q_m) < max_error)
00829                                 {
00830 //                                      js = j;
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 //                                      js = j;
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 //#ifdef VERBOSE
00846                         cerr << "[LCP] m_jr >= 0: no solution " << endl;
00847 //#endif
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 //              if(js < 0 || min_q_negative > max_error)
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                 // step 2
00912                 //
00913 #ifdef VERBOSE
00914                 cerr << "--- step 2 [iteration " << n_iter << "] ---" << endl;
00915 #endif
00916                 // pivot w[js] <-> z[jr]
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 //              cerr << "pivot result:" << endl;
00922 //              cerr << "M_new = " << M_new << endl;
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 //                      cerr << "[LCP] too large error" << endl;
00931                         terminate = false;
00932                         break;
00933                 }
00934 #endif
00935                 if(ys2g == n_vars)  // ys == z0
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         // count number of g elements in w
01073         for(int i=0; i<n_row; i++)
01074         {
01075 //              cerr << "w[" << i << "]: a/g = " << w2a[i] << "/" << w2g[i] << endl;
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 //              cerr << "z[" << i << "]: a/g = " << z2a[i] << "/" << z2g[i] << endl;
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         // output
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 //      cerr << "M_new = " << M_new << endl;
01191 //      cerr << "q_new = " << tran(q_new) << endl;
01192 #endif
01193 }
01194 
01195 // pivot w[idx1] and z[idx2] in w=Mz+q
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 //      cerr << "pivot->" << endl;
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 //      cerr << "M12 = " << M12 << endl;
01260 //      cerr << "M1 = " << M1 << endl;
01261 //      cerr << "M2 = " << M2 << endl;
01262 //      cerr << "M12bar = " << M12bar << endl;
01263         // new M
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         // output
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 //      cerr << "M_new = " << M_new << endl;
01318 //      cerr << "<- pivot" << endl;
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         // new M
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         // output
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 //      cerr << "M_new = " << M_new << endl;
01430 //      cerr << "<- pivot" << endl;
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 //      M2Md12.mul(M2, Md12);
01449 //      M2Md12M1.mul(M2Md12, M1);
01450 //      Md12bar -= M2Md12M1;
01451         M2Md12M1.mul(M2, Md1);
01452         Md12bar += M2Md12M1;
01453 
01454 #ifdef VERBOSE
01455 //      cerr << "Md12 = " << Md12 << endl;
01456         cerr << "M12*Md12 = " << M12*Md12 << endl;
01457 //      cerr << "Md1 = " << Md1 << endl;
01458 //      cerr << "Md2 = " << Md2 << endl;
01459 //      cerr << "Md_bar = " << Md_bar << endl;
01460 #endif
01461         // new q
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 //      qd1bar_temp.mul(M2Md12, q1);
01468 //      qd1bar -= qd1bar_temp;
01469         qd1bar_temp.mul(M2, qd1);
01470         qd1bar += qd1bar_temp;
01471 #ifdef VERBOSE
01472 //      cerr << "Md12 = " << Md12 << endl;
01473 //      cerr << "Md1 = " << Md1 << endl;
01474 //      cerr << "Md2 = " << Md2 << endl;
01475 //      cerr << "Md12bar = " << Md12bar << endl;
01476 //      cerr << "qd1 = " << tran(qd1) << endl;
01477 //      cerr << "qd1bar = " << tran(qd1bar) << endl;
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 //      cerr << "w = " << endl;
01499         for(i=0; i<n_vars; i++)
01500         {
01501 //              cerr << " [" << i << "] g/a = " << w2g[i] << "/" << w2a[i] << endl;
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 //      cerr << "z = " << endl;
01519         for(i=0; i<=n_vars; i++)  // z has n_vars+1 elements
01520         {
01521 //              cerr << " [" << i << "] a/g = " << z2a[i] << "/" << z2g[i] << endl;
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         // b = [e(jr) or m_ab(jr) | q_a]
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         // compute new Minv
01575         newMinv.resize(n_pivot, n_pivot);
01576         if(ys2a >=0)
01577         {
01578                 if(yr2a >= 0) // row_replaced
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) // enlarge
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 //                      cerr << "Maa = " << Maa << endl;
01684 //                      cerr << "m12 = " << m12 << endl;
01685 //                      cerr << "m21 = " << m21 << endl;
01686 //                      cerr << "m22 = " << m22 << endl;
01687 //                      cerr << "newMinv = " << newMinv << endl;
01688 #endif
01689                 }
01690                 else
01691                 {
01692                         assert(0);
01693                 }
01694         }
01695         else if(ys2g >= 0)
01696         {
01697                 if(yr2a >= 0) // shrink
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) // col_replaced
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 //      cerr << "m_jr = " << tran(m_jr) << endl;
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 //      cerr << "q_new(temp) = " << tran(q_new) << endl;
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 //      cerr << "w = " << endl;
01879         for(i=0; i<n_vars; i++)
01880         {
01881 //              cerr << " [" << i << "] g/a = " << w2g[i] << "/" << w2a[i] << endl;
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 //      cerr << "z = " << endl;
01898         for(i=0; i<=n_vars; i++)  // z has n_vars+1 elements
01899         {
01900 //              cerr << " [" << i << "] a/g = " << z2a[i] << "/" << z2g[i] << endl;
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 //      cerr << "jr = " << jr << ", jr2piv = " << jr2piv << ", jr2non = " << jr2non << endl;
01926 //      cerr << "original q = " << tran(q) << endl;
01927         // b = [e(jr) or m_ab(jr) | q_a]
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 //      cerr << "m_jr = " << tran(m_jr) << endl;
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 //      cerr << "q_new(temp) = " << tran(q_new) << endl;
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 //              cerr << "g0 = " << g0 << endl;
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 //      cerr << "error = " << tran(error) << endl;
02054 #endif
02055         return error.length();
02056 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:55