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)
25 int size = idx1.size();
28 int m = idx1[
i],
n = idx2[
i];
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;
34 int w2a_org = w2a[m], z2g_org = z2g[
n];
42 void swap_index(
int idx1,
int idx2, std::vector<int>& w2a, std::vector<int>& w2g, std::vector<int>& z2a, std::vector<int>& z2g)
44 int w2a_org = w2a[idx1], z2g_org = z2g[idx2];
45 w2a[idx1] = z2a[idx2];
46 z2g[idx2] = w2g[idx1];
59 const vector<int>& _w2a,
const vector<int>& _w2g,
60 const vector<int>& _z2a,
const vector<int>& _z2g,
62 int _js,
int _jr,
double _cost) :
dpNode() {
102 cerr <<
"new node: step = " <<
n_steps <<
", cost = " <<
cost <<
", js = " <<
js <<
", jr = " <<
jr << endl;
107 cerr <<
"another pivot to terminate" << endl;
109 static fMat m_jr_dummy;
112 #ifdef INVERSE_FROM_SCRATCH
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);
141 cerr <<
"g_in_w = [" << flush;
144 cerr << *
i <<
" " << flush;
147 cerr <<
"a_in_z = [" << flush;
150 cerr << *
i <<
" " << flush;
163 int new_yr2a=0, new_yr2g=0;
179 if(new_yr2a ==
z2a[
i])
186 else if(new_yr2g >= 0)
190 if(new_yr2g ==
z2g[
i])
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;
210 cerr <<
"new_jr = " <<
new_jr << endl;
221 #ifndef INVERSE_FROM_SCRATCH
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;
248 cerr <<
"initial node" << endl;
264 cerr <<
"loop; don't open" << endl;
271 #ifdef INVERSE_FROM_SCRATCH // solve linear equation every time
273 #else // update Maa_inv
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);
282 cerr <<
"error is too large (" <<
error <<
"); don't open" << endl;
289 #ifdef PIVOT_MINIMUM_ONLY
290 int min_q_m_index = -1;
291 double min_q_m = 0.0;
293 std::vector<int> js_cand;
294 std::vector<double> js_cost;
295 std::vector<double> js_g0;
299 int current_g0_index = -1;
300 for(
int j=0; j<
n_vars; j++)
304 current_g0_index = j;
310 for(
int j=0; j<
n_vars; j++)
312 if(
js < 0 || m_jr(j,0) < 0.0)
314 double new_q_g0 = 0.0;
326 #ifdef PIVOT_MINIMUM_ONLY
327 if(q_m >= -
max_error && (min_q_m_index < 0 || q_m > min_q_m))
335 js_cand.push_back(j);
336 js_cost.push_back(q_m);
337 js_g0.push_back(new_q_g0);
339 cerr << j <<
": q_m = " << q_m <<
", q_g0 = " << new_q_g0 << endl;
346 if(js_cand.size() == 0)
348 cerr <<
"no candidate" << endl;
355 cerr <<
"z0 can be pivoted" << endl;
359 LCPNode* node =
new LCPNode(
lcp,
this, q,
max_error,
w2a,
w2g,
z2a,
z2g,
n_steps+1, g0_index,
new_jr, -1.0);
363 #ifdef PIVOT_MINIMUM_ONLY
364 if(min_q_m_index >= 0)
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);
372 #else // #ifdef PIVOT_MINIMUM_ONLY
375 if(js_cand.size() > 1)
383 for(
int j=0; j<=
n_vars; j++)
402 for(
int j=0; j<
n_vars; j++)
413 int minimum_defined =
false;
415 double min_diff = 1e-3;
417 cerr <<
"column " <<
i <<
": beta = " << flush;
419 for(
int j=0; j<js_cand.size(); j++)
423 min_beta = beta0(js_cand[j],0);
425 else if(fabs(beta0(js_cand[j],0)-min_beta) < min_diff)
427 minimum_defined =
false;
429 else if(beta0(js_cand[j],0) < min_beta - min_diff)
431 minimum_defined =
true;
432 min_beta = beta0(js_cand[j],0);
435 cerr << beta0(js_cand[j],0) <<
" " << flush;
444 cerr <<
"minimum found" << endl;
446 for(
int j=0; j<js_cand.size(); j++)
448 js_cost[j] = -beta0(js_cand[j],0);
457 nodes =
new dpNode* [js_cand.size()];
458 for(
unsigned int j=0; j<js_cand.size(); j++)
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]);
463 return js_cand.size();
477 #ifndef ACTIVATE_SAME_STATE
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;
499 double q_m = - q(piv) / m_jr(piv, 0);
503 q_new(
i) += m_jr(
i,0)*q_m;
509 double q_m = - q(piv) / m_jr(piv, 0);
516 double qi = q(
i) + m_jr(
i,0)*q_m;
528 if(z0_index >= 0 && new_q_z0)
536 *new_q_z0 = q(z0_index) + m_jr(z0_index,0)*q_m;
566 if(n_nodes) *n_nodes = 0;
568 std::vector<int> z2g, w2g, z2a, w2a;
591 int max_nodes = _max_nodes;
592 double max_error = _max_error;
593 int terminate =
true;
595 cerr <<
"LCP::SolvePivot2(" <<
n_vars <<
")" << endl;
604 cerr <<
"--- step 0 ---" << endl;
606 for(
int j=0; j<
n_vars; j++)
617 cerr <<
"q >= 0: trivial solution" << endl;
626 LCPNode* snode =
new LCPNode(
this, NULL,
q, max_error, w2a, w2g, z2a, z2g, 0, -1,
n_vars, 0.0);
630 if(n_nodes) *n_nodes = dp->
NumNodes();
640 cerr <<
"[LCP] solution not found (number of nodes = " << dp->
NumNodes() <<
", number of variables = " <<
n_vars <<
")" << endl;
642 cerr <<
"M = " <<
M << endl;
643 cerr <<
"q = " <<
tran(
q) << endl;
650 cerr <<
"goal found (" << dp->
NumNodes() <<
"/" << lgoal->
NumStep() <<
")" << endl;
652 cerr <<
"w =" << flush;
654 for(
int j=0; j<
n_vars; j++)
656 if(lgoal->
w2a[j] >= 0)
659 cerr <<
"\ta[" << lgoal->
w2a[j] <<
"]" << flush;
666 cerr <<
"\tg[" << lgoal->
w2g[j] <<
"]" << flush;
677 cerr <<
"a = " <<
tran(
a) << endl;
678 cerr <<
"g = " <<
tran(g) << endl;
680 if((
int)_g2w.size() ==
n_vars)
690 _g2w[lgoal->
w2g[
i]] =
i;
700 if(n_iteration) *n_iteration = 0;
711 std::vector<int> z2g, w2g, z2a, w2a;
712 int yr2g = -1, yr2a = -1, ys2g = -1, ys2a = -1;
727 int max_iteration = _max_iteration;
728 double max_error = _max_error;
729 int terminate =
true;
731 cerr <<
"LCP::SolvePivot" << endl;
732 cerr <<
"--- inputs ---" << endl;
733 cerr <<
"M = " <<
M << endl;
734 cerr <<
"q = " <<
tran(
q) << endl;
740 cerr <<
"--- step 0 ---" << endl;
742 for(
int j=0; j<
n_vars; j++)
753 cerr <<
"q >= 0: trivial solution" << endl;
763 double min_q_c = 0.0;
764 for(
int j=0; j<
n_vars; j++)
767 if(jr < 0 || q_c < min_q_c)
776 std::vector<int> idx1, idx2;
782 Pivot(idx1, idx2,
M,
q, M_new, q_new);
788 for(n_iter=0; n_iter<max_iteration; n_iter++)
793 cerr <<
"w[" <<
i <<
"]: a/g = " << w2a[
i] <<
"/" << w2g[
i] << endl;
797 cerr <<
"z[" <<
i <<
"]: a/g = " << z2a[
i] <<
"/" << z2g[
i] << endl;
804 cerr <<
"--- step 1 [iteration " << n_iter <<
"] ---" << endl;
805 cerr <<
"jr = " << jr << endl;
810 double min_q_m = 0.0;
812 std::vector<int> js_cand;
813 for(
int j=0; j<
n_vars; j++)
815 if(q_new(j) >= 0.0 && m_jr(j,0) < 0.0)
817 double q_m = - q_new(j) / m_jr(j,0);
819 cerr <<
"[" << j <<
"]: z = " << w2g[j] <<
", q_new/m_jr = " << q_new(j) <<
"/" << m_jr(j,0) <<
" = " << q_m << endl;
824 js_cand.push_back(j);
828 else if(fabs(q_m-min_q_m) < max_error)
831 min_q_m = (min_q_m*js_cand.size() + q_m)/(js_cand.size()+1);
832 js_cand.push_back(j);
834 else if(q_m < min_q_m)
838 js_cand.push_back(j);
846 cerr <<
"[LCP] m_jr >= 0: no solution " << endl;
851 int n_js_cand = js_cand.size();
852 double min_q_negative = 0.0;
854 cerr <<
"n_js_cand = " << n_js_cand << endl;
856 for(
int j=0; j<n_js_cand; j++)
858 idx1[0] = js_cand[j];
861 Pivot(w2a, w2g, z2a, z2g,
M,
q, M_new, q_new);
866 cerr <<
"js_cand[" << j <<
"] = " << js_cand[j] << endl;
867 cerr <<
"q_negative = " << q_negative << endl;
869 if(w2g[js_cand[j]] ==
n_vars && q_negative <= max_error)
874 if(js < 0 || q_negative < min_q_negative)
877 min_q_negative = q_negative;
883 cerr <<
"[LCP] no pivot found" << endl;
888 cerr <<
"js = " << js << endl;
895 cerr <<
"ys is a[" << ys2a <<
"]" << endl;
898 else if(w2g[js] >= 0)
903 cerr <<
"ys is g[" << ys2g <<
"]" << endl;
914 cerr <<
"--- step 2 [iteration " << n_iter <<
"] ---" << endl;
920 Pivot(w2a, w2g, z2a, z2g,
M,
q, M_new, q_new);
924 cerr <<
"q_new = " <<
tran(q_new) << endl;
938 cerr <<
"success" << endl;
948 cerr <<
"yr is a[" << yr2a <<
"]" << endl;
951 for(
int j=0; j<=
n_vars; j++)
966 cerr <<
"yr is g[" << yr2g <<
"]" << endl;
969 for(
int j=0; j<=
n_vars; j++)
984 cerr <<
"g2w = " << endl;
985 int* __g2w =
new int [
n_vars];
999 cerr <<
" " << __g2w[
i] << flush;
1005 if(n_iter == max_iteration)
1007 cerr <<
"[LCP] did not converge" << endl;
1016 cerr <<
"q_new = " <<
tran(q_new) << endl;
1018 for(
int j=0; j<
n_vars; j++)
1023 cerr <<
"w2a[" << j <<
"] = " << w2a[j] << endl;
1025 a(w2a[j]) = q_new(j);
1027 else if(w2g[j] >= 0 && w2g[j] !=
n_vars)
1030 cerr <<
"w2g[" << j <<
"] = " << w2g[j] << endl;
1032 g(w2g[j]) = q_new(j);
1040 cerr <<
"a = " <<
tran(
a) << endl;
1041 cerr <<
"g = " <<
tran(g) << endl;
1044 if(n_iteration) *n_iteration = n_iter;
1045 if((
int)_g2w.size() ==
n_vars)
1064 std::vector<int>& z2a, std::vector<int>& z2g,
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);
1073 for(
int i=0;
i<n_row;
i++)
1078 g_piv.push_back(w2g[
i]);
1082 a_bar.push_back(w2a[
i]);
1085 for(
int i=0;
i<n_col;
i++)
1088 if(z2a[
i] >= 0) a_piv.push_back(z2a[
i]);
1089 else g_bar.push_back(z2g[
i]);
1091 int n_pivot = g_piv.size();
1093 cerr <<
"n_pivot = " << n_pivot << endl;
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++)
1108 q1(
i) =
q(a_piv[
i]);
1109 for(
int j=0; j<n_pivot; j++)
1111 M12(
i, j) =
M(a_piv[
i], g_piv[j]);
1113 for(
int j=0; j<n_col_bar; j++)
1115 M1(
i, j) =
M(a_piv[
i], g_bar[j]);
1118 for(
int i=0;
i<n_row_bar;
i++)
1120 q1bar(
i) =
q(a_bar[
i]);
1121 for(
int j=0; j<n_pivot; j++)
1123 M2(
i, j) =
M(a_bar[
i], g_piv[j]);
1125 for(
int j=0; j<n_row_bar; j++)
1127 M12bar(
i, j) =
M(a_bar[
i], g_bar[j]);
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;
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);
1144 pivot_body(M12, M1, M2, M12bar, q1, q1bar, Md12, Md1, Md2, Md12bar, qd1, qd1bar);
1147 int i_piv = 0, i_bar = 0;
1148 for(
int i=0;
i<n_row;
i++)
1152 q_new(
i) = qd1(i_piv);
1153 int j_piv = 0, j_bar = 0;
1154 for(
int j=0; j<n_col; j++)
1158 M_new(
i, j) = Md12(i_piv, j_piv);
1163 M_new(
i, j) = Md1(i_piv, j_bar);
1171 q_new(
i) = qd1bar(i_bar);
1172 int j_piv = 0, j_bar = 0;
1173 for(
int j=0; j<n_col; j++)
1177 M_new(
i, j) = Md2(i_bar, j_piv);
1182 M_new(
i, j) = Md12bar(i_bar, j_bar);
1200 assert(idx1.size() == idx2.size());
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;
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);
1218 for(
int i=0;
i<n_row;
i++)
1220 int i_piv = row_pivot_index[
i];
1225 for(
int j=0; j<n_col; j++)
1227 int j_piv = col_pivot_index[j];
1230 M12(i_piv, j_piv) =
M(
i, j);
1234 M1(i_piv, j_bar) =
M(
i, j);
1241 q1bar(i_bar) =
q(
i);
1243 for(
int j=0; j<n_col; j++)
1245 int j_piv = col_pivot_index[j];
1248 M2(i_bar, j_piv) =
M(
i, j);
1252 M12bar(i_bar, j_bar) =
M(
i, j);
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);
1270 pivot_body(M12, M1, M2, M12bar, q1, q1bar, Md12, Md1, Md2, Md12bar, qd1, qd1bar);
1273 M_new.
resize(n_row, n_col);
1276 for(
int i=0;
i<n_row;
i++)
1278 int i_piv = row_pivot_index[
i];
1281 q_new(
i) = qd1(i_piv);
1283 for(
int j=0; j<n_col; j++)
1285 int j_piv = col_pivot_index[j];
1288 M_new(
i, j) = Md12(i_piv, j_piv);
1292 M_new(
i, j) = Md1(i_piv, j_bar);
1299 q_new(
i) = qd1bar(i_bar);
1301 for(
int j=0; j<n_col; j++)
1303 int j_piv = col_pivot_index[j];
1306 M_new(
i, j) = Md2(i_bar, j_piv);
1310 M_new(
i, j) = Md12bar(i_bar, j_bar);
1325 int n_row =
M.
row(), n_col =
M.
col();
1326 static fMat M12bar, M1, M2, M12;
1327 static fVec q1, q1bar;
1329 M12bar.
resize(n_row-1, n_col-1);
1335 for(
int i=0;
i<n_row;
i++)
1341 for(
int j=0; j<n_col; j++)
1345 M12(0, 0) =
M(
i, j);
1349 M1(0, j_bar) =
M(
i, j);
1356 q1bar(i_bar) =
q(
i);
1358 for(
int j=0; j<n_col; j++)
1362 M2(i_bar, 0) =
M(
i, j);
1366 M12bar(i_bar, j_bar) =
M(
i, j);
1377 static fMat Md12bar;
1378 static fVec qd1, qd1bar;
1382 Md12bar.
resize(n_row-1, n_col-1);
1385 pivot_body(M12, M1, M2, M12bar, q1, q1bar, Md12, Md1, Md2, Md12bar, qd1, qd1bar);
1388 M_new.
resize(n_row, n_col);
1391 for(
int i=0;
i<n_row;
i++)
1397 for(
int j=0; j<n_col; j++)
1401 M_new(
i, j) = Md12(0, 0);
1405 M_new(
i, j) = Md1(0, j_bar);
1412 q_new(
i) = qd1bar(i_bar);
1414 for(
int j=0; j<n_col; j++)
1418 M_new(
i, j) = Md2(i_bar, 0);
1422 M_new(
i, j) = Md12bar(i_bar, j_bar);
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);
1447 Md12bar.
set(M12bar);
1451 M2Md12M1.
mul(M2, Md1);
1452 Md12bar += M2Md12M1;
1456 cerr <<
"M12*Md12 = " << M12*Md12 << endl;
1462 static fVec qd1bar_temp;
1463 qd1bar_temp.
resize(n_row_bar);
1469 qd1bar_temp.
mul(M2, qd1);
1470 qd1bar += qd1bar_temp;
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,
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;
1502 if(old_w2g[
i] >= 0) old_piv2g.push_back(old_w2g[
i]);
1507 piv2g.push_back(wgi);
1514 non2a.push_back(wai);
1517 int jr2piv = -1, jr2non = -1;
1522 if(old_z2a[
i] >= 0) old_piv2a.push_back(old_z2a[
i]);
1527 piv2a.push_back(zai);
1528 if(
i == jr) jr2piv = piv2a.size()-1;
1535 non2g.push_back(zgi);
1536 if(
i == jr) jr2non = non2g.size()-1;
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);
1550 assert(jr2piv >= 0);
1556 assert(jr2non >= 0);
1557 for(
i=0;
i<n_pivot;
i++)
1559 b(
i, 0) =
M(piv2a[
i], jr2g);
1562 for(
i=0;
i<n_pivot;
i++)
1564 b(
i, 1) =
q(piv2a[
i]);
1566 for(
i=0;
i<n_pivot;
i++)
1569 for(
int j=0; j<n_pivot; j++)
1571 Maa(
i, j) =
M(pai, piv2g[j]);
1575 newMinv.
resize(n_pivot, n_pivot);
1581 cerr <<
"row_replaced" << endl;
1583 static fMat P,
q, m2d,
X, y;
1584 P.
resize(n_pivot, n_pivot-1);
1588 for(
i=0;
i<n_pivot;
i++)
1590 if(piv2a[
i] == ys2a)
1592 for(
int j=0; j<n_pivot; j++)
1594 q(j, 0) = oldMinv(j,
i);
1599 for(
int j=0; j<n_pivot; j++)
1601 P(j, count) = oldMinv(j,
i);
1606 for(
int j=0; j<n_pivot; j++)
1608 m2d(0, j) =
M(ys2a, piv2g[j]);
1611 for(
i=0;
i<n_pivot;
i++)
1614 for(
int j=0; j<n_pivot; j++)
1616 if(piv2a[j] == ys2a)
1618 newMinv(
i,j) = y(
i, 0);
1622 newMinv(
i,j) =
X(
i, count);
1631 cerr <<
"enlarge" << endl;
1633 static fMat m12, m21, m22,
X, y, z, w;
1634 m12.
resize(n_pivot-1, 1);
1635 m21.
resize(1, n_pivot-1);
1637 int count1 = 0, count2 = 0;
1638 for(
i=0;
i<n_pivot;
i++)
1640 if(piv2a[
i] != ys2a) m12(count1++, 0) =
M(piv2a[
i], yr2g);
1641 if(piv2g[
i] != yr2g) m21(0, count2++) =
M(ys2a, piv2g[
i]);
1643 m22(0, 0) =
M(ys2a, yr2g);
1646 for(
i=0;
i<n_pivot;
i++)
1648 if(piv2g[
i] == yr2g)
1651 for(
int j=0; j<n_pivot; j++)
1653 if(piv2a[j] == ys2a)
1655 newMinv(
i,j) = w(0,0);
1659 newMinv(
i,j) = z(0, count2);
1667 for(
int j=0; j<n_pivot; j++)
1669 if(piv2a[j] == ys2a)
1671 newMinv(
i,j) = y(count1, 0);
1675 newMinv(
i,j) =
X(count1, count2);
1700 cerr <<
"shrink" << endl;
1703 P.
resize(n_pivot, n_pivot);
1708 for(
i=0;
i<n_pivot+1;
i++)
1710 if(old_piv2g[
i] == ys2g)
1713 for(
int j=0; j<n_pivot+1; j++)
1715 if(old_piv2a[j] == yr2a)
1717 s(0,0) = oldMinv(
i,j);
1721 r(0, count2) = oldMinv(
i,j);
1729 for(
int j=0; j<n_pivot+1; j++)
1731 if(old_piv2a[j] == yr2a)
1733 q(count1,0) = oldMinv(
i,j);
1737 P(count1, count2) = oldMinv(
i,j);
1749 cerr <<
"col_replaced" << endl;
1751 static fMat P,
q, m2d,
X, y;
1752 P.
resize(n_pivot-1, n_pivot);
1756 for(
i=0;
i<n_pivot;
i++)
1758 if(piv2g[
i] == yr2g)
1760 for(
int j=0; j<n_pivot; j++)
1762 q(0, j) = oldMinv(
i, j);
1767 for(
int j=0; j<n_pivot; j++)
1769 P(count, j) = oldMinv(
i, j);
1774 for(
int j=0; j<n_pivot; j++)
1776 m2d(j, 0) =
M(piv2a[j], yr2g);
1780 for(
i=0;
i<n_pivot;
i++)
1782 if(piv2g[
i] == yr2g)
1784 for(
int j=0; j<n_pivot; j++)
1786 newMinv(
i,j) = y(0, j);
1791 for(
int j=0; j<n_pivot; j++)
1793 newMinv(
i,j) =
X(count, j);
1810 cerr <<
"newMinv * Maa = " << newMinv*Maa << endl;
1814 for(
i=0;
i<n_pivot;
i++)
1816 m_jr(piv2w[
i], 0) = x(
i, 0);
1818 for(
i=0, non2w_iter=non2w.begin(), non2a_iter=non2a.begin();
i<n_none;
i++, non2w_iter++, non2a_iter++)
1820 int nwi = *non2w_iter, nai = *non2a_iter;
1823 for(j=0, piv2g_iter=piv2g.begin(), piv2w_iter=piv2w.begin(); j<n_pivot; j++, piv2g_iter++, piv2w_iter++)
1825 m_jr(nwi, 0) +=
M(nai, *piv2g_iter) * m_jr(*piv2w_iter, 0);
1831 for(
i=0;
i<n_pivot;
i++)
1833 m_jr(piv2w[
i], 0) = -x(
i, 0);
1835 for(
i=0, non2w_iter=non2w.begin(), non2a_iter=non2a.begin();
i<n_none;
i++, non2w_iter++, non2a_iter++)
1837 int nwi = *non2w_iter, nai = *non2a_iter;
1838 m_jr(nwi, 0) =
M(nai, jr2g);
1840 for(j=0, piv2g_iter=piv2g.begin(), piv2w_iter=piv2w.begin(); j<n_pivot; j++, piv2g_iter++, piv2w_iter++)
1842 m_jr(nwi, 0) +=
M(nai, *piv2g_iter) * m_jr(*piv2w_iter, 0);
1849 for(
i=0;
i<n_pivot;
i++)
1851 q_new(piv2w[
i]) = -x(
i, 1);
1854 for(
i=0, non2w_iter=non2w.begin(), non2a_iter=non2a.begin();
i<n_none;
i++, non2w_iter++, non2a_iter++)
1856 int nwi = *non2w_iter, nai = *non2a_iter;
1857 q_new(nwi) =
q(nai);
1859 for(j=0, piv2g_iter=piv2g.begin(), piv2w_iter=piv2w.begin(); j<n_pivot; j++, piv2g_iter++, piv2w_iter++)
1861 q_new(nwi) +=
M(nai, *piv2g_iter) * q_new(*piv2w_iter);
1867 std::vector<int>& w2a, std::vector<int>& w2g,
1868 std::vector<int>& z2a, std::vector<int>& z2g,
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;
1886 piv2g.push_back(wgi);
1893 non2a.push_back(wai);
1896 int jr2piv = -1, jr2non = -1;
1905 piv2a.push_back(zai);
1906 if(
i == jr) jr2piv = piv2a.size()-1;
1913 non2g.push_back(zgi);
1914 if(
i == jr) jr2non = non2g.size()-1;
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);
1930 assert(jr2piv >= 0);
1936 assert(jr2non >= 0);
1937 for(
i=0;
i<n_pivot;
i++)
1939 b(
i, 0) =
M(piv2a[
i], jr2g);
1942 for(
i=0;
i<n_pivot;
i++)
1944 b(
i, 1) =
q(piv2a[
i]);
1946 for(
i=0;
i<n_pivot;
i++)
1949 for(
int j=0; j<n_pivot; j++)
1951 Maa(
i, j) =
M(pai, piv2g[j]);
1957 for(
i=0;
i<n_pivot;
i++)
1959 m_jr(piv2w[
i], 0) = x(
i, 0);
1961 for(
i=0, non2w_iter=non2w.begin(), non2a_iter=non2a.begin();
i<n_none;
i++, non2w_iter++, non2a_iter++)
1963 int nwi = *non2w_iter, nai = *non2a_iter;
1966 for(j=0, piv2g_iter=piv2g.begin(), piv2w_iter=piv2w.begin(); j<n_pivot; j++, piv2g_iter++, piv2w_iter++)
1968 m_jr(nwi, 0) +=
M(nai, *piv2g_iter) * m_jr(*piv2w_iter, 0);
1974 for(
i=0;
i<n_pivot;
i++)
1976 m_jr(piv2w[
i], 0) = -x(
i, 0);
1978 for(
i=0, non2w_iter=non2w.begin(), non2a_iter=non2a.begin();
i<n_none;
i++, non2w_iter++, non2a_iter++)
1980 int nwi = *non2w_iter, nai = *non2a_iter;
1981 m_jr(nwi, 0) =
M(nai, jr2g);
1983 for(j=0, piv2g_iter=piv2g.begin(), piv2w_iter=piv2w.begin(); j<n_pivot; j++, piv2g_iter++, piv2w_iter++)
1985 m_jr(nwi, 0) +=
M(nai, *piv2g_iter) * m_jr(*piv2w_iter, 0);
1992 for(
i=0;
i<n_pivot;
i++)
1994 q_new(piv2w[
i]) = -x(
i, 1);
1997 for(
i=0, non2w_iter=non2w.begin(), non2a_iter=non2a.begin();
i<n_none;
i++, non2w_iter++, non2a_iter++)
1999 int nwi = *non2w_iter, nai = *non2a_iter;
2000 q_new(nwi) =
q(nai);
2002 for(j=0, piv2g_iter=piv2g.begin(), piv2w_iter=piv2w.begin(); j<n_pivot; j++, piv2g_iter++, piv2w_iter++)
2004 q_new(nwi) +=
M(nai, *piv2g_iter) * q_new(*piv2w_iter);
2016 int g0_found =
false;
2020 for(
int j=0; j<
n_vars; j++)
2024 a(w2a[j]) = q_new(j);
2026 else if(w2g[j] >= 0)
2035 g(w2g[j]) = q_new(j);
2055 return error.length();