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;
317 if(w2g[j] == n_vars && q_m >= -
max_error)
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;
586 M.
resize(n_vars, n_vars+1);
588 M.set_submat(0, 0, N);
589 M.set_submat(0, n_vars, c);
591 int max_nodes = _max_nodes;
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;
663 else if(lgoal->
w2g[j] >= 0 && lgoal->
w2g[j] != n_vars)
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)
688 if(lgoal->
w2g[
i] >= 0 && lgoal->
w2g[
i] < n_vars)
690 _g2w[lgoal->
w2g[
i]] =
i;
700 if(n_iteration) *n_iteration = 0;
727 int max_iteration = _max_iteration;
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++)
766 double q_c = q(j) / M(j, n_vars);
767 if(jr < 0 || q_c < min_q_c)
774 fMat M_new(n_vars, n_vars+1);
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;
795 for(
int i=0;
i<n_vars+1;
i++)
797 cerr <<
"z[" <<
i <<
"]: a/g = " << z2a[
i] <<
"/" << z2g[
i] << endl;
804 cerr <<
"--- step 1 [iteration " << n_iter <<
"] ---" << endl;
805 cerr <<
"jr = " << jr << endl;
807 fMat m_jr(n_vars, 1);
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;
927 double error = CheckPivotResult(q_new, w2a, w2g);
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];
992 if(w2g[
i] >= 0 && w2g[
i] < 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)
1053 if(w2g[
i] >= 0 && w2g[
i] < 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,
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];
1543 Maa.
resize(n_pivot, n_pivot);
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;
1702 static fMat P, q, r, s;
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];
1921 Maa.
resize(n_pivot, n_pivot);
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)
2028 if(w2g[j] == n_vars)
2035 g(w2g[j]) = q_new(j);
int inv_row_replaced(const fMat &P, const fMat &q, const fMat &m2d, fMat &X, fMat &y)
double calc_cost()
Compute the local cost at the node.
double CheckPivotResult(const fVec &q_new, std::vector< int > &w2a, std::vector< int > &w2g)
int resize(int i, int j)
Changes the matrix size.
RTC::ReturnCode_t ret(RTC::Local::ReturnCode_t r)
static void Pivot(int idx1, int idx2, const fMat &M, const fVec &q, fMat &M_new, fVec &q_new)
int col() const
Returns the number of columns.
int Search(int _max_nodes=-1, int _max_goals=-1)
Dijkstra or A* search: find the node with the smallest cost.
int inv_shrink(const fMat &P, const fMat &q, const fMat &r, const fMat &s, fMat &X)
Base class for generic discrete search.
void error(char *msg) const
static void pivot_body(const fMat &M12, const fMat &M1, const fMat &M2, const fMat &M12bar, const fVec &q1, const fVec &q1bar, fMat &Md12, fMat &Md1, fMat &Md2, fMat &Md12bar, fVec &qd1, fVec &qd1bar)
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)
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)
void calc_new_q(const fVec &q, const fMat &m_jr, int piv, fVec &q_new)
void SetStartNode(dpNode *_n)
Set the initial node for search.
void mul(const fMat &mat1, const fMat &mat2)
void zero()
Creates a zero matrix.
int create_child_nodes(dpNode **&nodes)
Create (potential) child nodes.
double calc_astar_cost(dpNode *potential_parent)
Compute the A-star cost at the node (optional).
void set(double *_d)
Sets all elements.
int SolvePivot(fVec &g, fVec &a, double _max_error, int _max_iteration, int *n_iteration, std::vector< int > &_g2w)
Solve using Lemke's method.
void mul(const fVec &vec, double d)
Solves a Linear Complementarity Problem (LCP)
friend fMat lineq_svd(const fMat &A, const fMat &b, int lwork)
solve linear equation Ax = b
void get_submat(int row_start, int col_start, const fMat &allM)
Extract a sub-matrix and set to myself.
int is_goal()
Check if the state is goal.
void resize(int i)
Change the size.
int same_state(dpNode *refnode)
Check if the node's state is the same as refnode. (to remove duplicates)
int row() const
Returns the number of rows.
fMat tran(const fMat &mat)
void zero()
Creates a zero vector.
void set_submat(int row_start, int col_start, const fMat &subM)
Sets a sub-matrix of myself.
int inv_enlarge(const fMat &m12, const fMat &m21, const fMat &m22, const fMat &P, fMat &X, fMat &y, fMat &z, fMat &w)
friend fMat inv_svd(const fMat &mat, int lwork)
inverse
void set(double *_d)
Sets the values from an array.
int SolvePivot2(fVec &g, fVec &a, double _max_error, int _max_iteration, int *n_iteration, std::vector< int > &_g2w)
Solve by pivot using path planning algorithm.
friend double length(const fVec &v)
Length of the vector.
int inv_col_replaced(const fMat &P, const fMat &q, const fMat &m2d, fMat &X, fMat &y)
double max_value()
Returns the maximum value.
LCPNode(LCP *_lcp, LCPNode *parent_lcp, const fVec &_q_old, double _max_error, const vector< int > &_w2a, const vector< int > &_w2g, const vector< int > &_z2a, const vector< int > &_z2g, int _n_steps, int _js, int _jr, double _cost)
dpNode * BestGoal(dpNode *ref=0)
Extract the goal with the smallest cost (if any).
static int max(int a, int b)
Matrix of generic size. The elements are stored in a one-dimensional array in row-major order...