00001
00002
00003
00004
00005
00006
00007
00008
00014 #include "psim.h"
00015 #include <dp.h>
00016
00017 #define TINY_MASS 1.0e-16
00018 #define N_2_COEF 1.6 // alpha
00019 #define N_COEF 1.0 // beta
00020 #define DOF_COEF -1.0 // gamma
00021 #define CONST_COEF 14.4 // delta
00022
00023 static pSim* sim = 0;
00024 static int max_procs = 0;
00025
00026 #include <fstream>
00027 ofstream logfile("schedule.log");
00028
00029
00030
00031
00032
00033
00034
00035 class dpScheduleNode
00036 : public dpNode
00037 {
00038 public:
00039 dpScheduleNode(dpScheduleNode* potential_parent,
00040 Joint* _last_joint, const fVec& _proc_costs,
00041 dpScheduleNode* _schedule_parent, int child_id,
00042 int _first_proc, int _last_proc,
00043 const joint_list& org_internal_joints,
00044 const p_joint_list& org_outer_joints): dpNode() {
00045 last_joint = _last_joint;
00046 if(last_joint)
00047 {
00048 sim->GetPJoint(last_joint, last_pjoints);
00049 }
00050 else
00051 {
00052 last_pjoints[0] = 0;
00053 last_pjoints[1] = 0;
00054 }
00055 schedule_parent = _schedule_parent;
00056 first_proc = _first_proc;
00057 last_proc = _last_proc;
00058 if(schedule_parent)
00059 {
00060 schedule_depth = schedule_parent->schedule_depth + 1;
00061 if(schedule_parent->last_joint)
00062 {
00063 set_outer_joints(schedule_parent->last_pjoints[child_id], org_outer_joints, outer_joints);
00064 }
00065 }
00066 else
00067 {
00068 schedule_depth = -1;
00069 for(p_joint_list::const_iterator j=org_outer_joints.begin(); j!=org_outer_joints.end(); j++)
00070 {
00071 outer_joints.push_back(*j);
00072 }
00073 }
00074 single_joint_cost = 0.0;
00075 proc_costs.resize(max_procs);
00076 proc_costs.set(_proc_costs);
00077 mycost = 0.0;
00078 if(last_joint)
00079 {
00080 logfile << "new node: " << last_joint->name << " (schedule depth = " << schedule_depth << flush;
00081 logfile << ", procs = [" << first_proc << ", " << last_proc << "]" << flush;
00082 if(schedule_parent && schedule_parent->last_joint)
00083 logfile << ", schedule_parent = " << schedule_parent->last_joint->name << ")" << endl;
00084 else
00085 logfile << ")" << endl;
00086
00087 split_internal_joints(last_joint, org_internal_joints, internal_joints[0], internal_joints[1]);
00088
00089 if(last_proc - first_proc == 1)
00090 {
00091 joint_list added_joints;
00092 single_joint_cost = calc_min_subchain_cost(outer_joints, added_joints);
00093 }
00094 else
00095 {
00096 single_joint_cost = calc_single_joint_cost(last_joint->n_dof, outer_joints.size());
00097 }
00098 }
00099 else
00100 {
00101 for(joint_list::const_iterator i=org_internal_joints.begin(); i!=org_internal_joints.end(); i++)
00102 {
00103 internal_joints[0].push_back(*i);
00104 }
00105 }
00106 logfile << " single_joint_cost = " << single_joint_cost << endl;
00107 for(int i=first_proc; i<last_proc; i++)
00108 {
00109 proc_costs(i) += single_joint_cost;
00110 }
00111 mycost = proc_costs.max_value();
00112 if(potential_parent)
00113 {
00114 mycost -= potential_parent->proc_costs.max_value();
00115 }
00116 logfile << " proc_costs = " << tran(proc_costs) << endl;
00117 logfile << " mycost = " << mycost << endl;
00118 }
00119 ~dpScheduleNode() {
00120 }
00121
00122 fVec& ProcCosts() {
00123 return proc_costs;
00124 }
00125
00126 Joint* LastJoint() {
00127 return last_joint;
00128 }
00129 int ScheduleDepth() {
00130 return schedule_depth;
00131 }
00132 int FirstProc() {
00133 return first_proc;
00134 }
00135 int LastProc() {
00136 return last_proc;
00137 }
00138 int ListAllInternalJoints(joint_list& all_internal);
00139
00140 protected:
00141 int list_all_internal_joints_sub(Joint* cur, joint_list& all_internal);
00142 int list_all_internal_joints_rev(Joint* cur, joint_list& all_internal);
00143 int split_internal_joints(Joint* _last_joint, const joint_list& org_internal_joints, joint_list& _internal_joints_0, joint_list& _internal_joints_1);
00144 int set_outer_joints(pJoint* last_pjoint, const p_joint_list& org_outer_joints, p_joint_list& new_outer_joints);
00145
00146 dpScheduleNode* find_available_parent(int target_depth, int& child_id);
00147 dpScheduleNode* find_available_parent_sub(dpScheduleNode* cur, int target_depth, dpNode* cur_leaf, int& child_id);
00148
00149
00150 int create_child_nodes(dpNode**& nodes) {
00151 if(is_goal()) return 0;
00152 logfile << "create_child_node [" << TotalAstarCost() << "] (joints = " << flush;
00153 dpNode* n;
00154 for(n=this; n; n=n->Parent())
00155 {
00156 dpScheduleNode* s = (dpScheduleNode*)n;
00157 if(s->last_joint)
00158 logfile << "[" << s->last_joint->name << "/" << s->schedule_depth << "]" << flush;
00159 }
00160 logfile << ")" << endl;
00161
00162 if(depth == sim->NumJoint()-1)
00163 {
00164 logfile << "create goal (0)" << endl;
00165 nodes = new dpNode* [1];
00166 nodes[0] = new dpScheduleNode(this, 0, proc_costs, this, 0, first_proc, last_proc, joint_list(), p_joint_list());
00167 return 1;
00168 }
00169
00170 dpScheduleNode* target_parent = 0;
00171 int child_id = -1;
00172 target_parent = find_available_parent(schedule_depth-1, child_id);
00173 if(!target_parent)
00174 {
00175
00176 target_parent = find_available_parent(schedule_depth, child_id);
00177
00178 if(!target_parent)
00179 {
00180 logfile << "create goal (1)" << endl;
00181 nodes = new dpNode* [1];
00182 nodes[0] = new dpScheduleNode(this, 0, proc_costs, this, 0, first_proc, last_proc, joint_list(), p_joint_list());
00183 return 1;
00184 }
00185 }
00187 if(target_parent->last_joint)
00188 {
00189 logfile << "target_parent = " << target_parent->last_joint->name << ", schedule_depth = " << schedule_depth << ", child_id = " << child_id << endl;
00190 }
00191 else
00192 {
00193 logfile << "target_parent = null, schedule_depth = " << schedule_depth << ", child_id = " << child_id << endl;
00194 }
00195 int _first_proc = 0, _last_proc = max_procs;
00196 if(child_id == 0)
00197 {
00198 if(target_parent->internal_joints[1].size() > 0)
00199 {
00200 _first_proc = target_parent->first_proc;
00201 _last_proc = target_parent->first_proc + (target_parent->last_proc-target_parent->first_proc)/2;
00202 }
00203 else
00204 {
00205 _first_proc = target_parent->first_proc;
00206 _last_proc = target_parent->last_proc;
00207 }
00208 }
00209 else
00210 {
00211 if(target_parent->internal_joints[0].size() > 0)
00212 {
00213 _first_proc = target_parent->first_proc + (target_parent->last_proc-target_parent->first_proc)/2;
00214 _last_proc = target_parent->last_proc;
00215 }
00216 else
00217 {
00218 _first_proc = target_parent->first_proc;
00219 _last_proc = target_parent->last_proc;
00220 }
00221 }
00222 joint_list& internal_joints = target_parent->internal_joints[child_id];
00223
00224 if(_last_proc - _first_proc == 1)
00225 {
00226 nodes = new dpNode* [1];
00227
00228 Joint* next_last = 0;
00229 if(target_parent->last_joint)
00230 {
00231 joint_list& in_joints = target_parent->internal_joints[child_id];
00232 for(joint_list::iterator i=in_joints.begin(); i!=in_joints.end(); i++)
00233 {
00234 if(*i == target_parent->last_joint->parent ||
00235 (*i)->parent == target_parent->last_joint ||
00236 (*i)->parent == target_parent->last_joint->parent)
00237 {
00238 next_last = *i;
00239 break;
00240 }
00241 }
00242 }
00243 else
00244 {
00245 next_last = sim->Root();
00246 }
00247 dpScheduleNode* n = new dpScheduleNode(this, next_last, proc_costs, target_parent, child_id, _first_proc, _last_proc, internal_joints, target_parent->outer_joints);
00248 nodes[0] = (dpNode*)n;
00249 return 1;
00250 }
00251
00252 int n_internal_joints = internal_joints.size();
00253 nodes = new dpNode* [n_internal_joints];
00254 joint_list::iterator j;
00255 int i;
00256 for(i=0, j=internal_joints.begin(); j!=internal_joints.end(); i++, j++)
00257 {
00258 dpScheduleNode* n = new dpScheduleNode(this, *j, proc_costs, target_parent, child_id, _first_proc, _last_proc, internal_joints, target_parent->outer_joints);
00259 #if 1 // always generate two non-empty subchains
00260 if(n->internal_joints[0].size() == 0 ||
00261 n->internal_joints[1].size() == 0)
00262 {
00263 delete n;
00264 nodes[i] = 0;
00265 }
00266 else
00267 #endif
00268 {
00269 nodes[i] = (dpNode*)n;
00270 }
00271 }
00272 return n_internal_joints;
00273 }
00274
00275 double calc_cost() {
00276 return mycost;
00277 }
00278
00279 double calc_astar_cost(dpNode* potential_parent) {
00280 double c = 0.0;
00281 logfile << "calc_astar_cost(" << flush;
00282 if(last_joint)
00283 {
00284 logfile << last_joint->name << "/" << schedule_depth << ")" << endl;
00285 p_joint_list new_outer_joints;
00286 if(last_proc - first_proc == 1)
00287 {
00288 #if 0
00289 double tmp = calc_min_subchain_cost(outer_joints);
00290 logfile << " one process: " << tmp << endl;
00291 if(tmp > c) c = tmp;
00292 #endif
00293 }
00294 else
00295 {
00296
00297 if(internal_joints[0].size() > 0)
00298 {
00299 int n_myprocs = last_proc - first_proc;
00300 joint_list added_joints;
00301 if(internal_joints[1].size() > 0)
00302 {
00303 n_myprocs = (last_proc - first_proc)/2;
00304 }
00305 set_outer_joints(last_pjoints[0], outer_joints, new_outer_joints);
00306 double tmp = calc_min_subchain_cost(new_outer_joints, added_joints)/n_myprocs;
00307 logfile << " child 0: " << tmp << " [" << n_myprocs << "]" << endl;
00308 if(tmp > c) c = tmp;
00309 }
00310
00311 if(internal_joints[1].size() > 0)
00312 {
00313 joint_list added_joints;
00314 int n_myprocs = last_proc - first_proc;
00315 if(internal_joints[0].size() > 0)
00316 {
00317 n_myprocs -= (last_proc-first_proc)/2;
00318 }
00319 set_outer_joints(last_pjoints[1], outer_joints, new_outer_joints);
00320 double tmp = calc_min_subchain_cost(new_outer_joints, added_joints)/n_myprocs;
00321 logfile << " child 1: " << tmp << " [" << n_myprocs << "]" << endl;
00322 if(tmp > c) c = tmp;
00323 }
00324 }
00325 }
00326 else
00327 {
00328 logfile << "default)" << endl;
00329 }
00330 logfile << " final = " << c << endl;
00331 return c;
00332 }
00333
00334 int same_state(dpNode* refnode) {
00335 return false;
00336 }
00337
00338 int is_goal() {
00339 return ((!last_joint && depth > 0) || depth == sim->NumJoint());
00340 }
00341
00342 private:
00343 Joint* last_joint;
00344 pJoint* last_pjoints[2];
00345
00346 int schedule_depth;
00347 dpScheduleNode* schedule_parent;
00348 p_joint_list outer_joints;
00349 joint_list internal_joints[2];
00350
00351 double single_joint_cost;
00352 double mycost;
00353 fVec proc_costs;
00354 int first_proc, last_proc;
00355
00356 double calc_single_joint_cost(int n_dof, int n_outer) {
00357
00358 return N_2_COEF*n_outer*n_outer + N_COEF*n_outer + DOF_COEF*n_dof + CONST_COEF;
00359 }
00360
00361 double calc_min_subchain_cost(const p_joint_list& _outer_joints, joint_list& added_joints);
00362 double calc_min_subchain_cost_sub(Joint* cur, const p_joint_list& _outer_joints, int* n_outer_parent_side, int* n_outer_child_side, joint_list& added_joints);
00363
00364 double add_to_child_side(Joint* cur, joint_list& added_joints, int* n_outer_parent_side, int* n_outer_child_side);
00365 double add_to_parent_side(Joint* cur, joint_list& added_joints, int* n_outer_parent_side, int* n_outer_child_side);
00366 };
00367
00368 int dpScheduleNode::ListAllInternalJoints(joint_list& all_internal)
00369 {
00370 #if 0
00371 all_internal.clear();
00372 if(!last_joint) return 0;
00373 if(last_proc - first_proc > 1)
00374 {
00375 all_internal.push_back(last_joint);
00376 return 0;
00377 }
00378 pJoint* top_pjoint = 0;
00379
00380 for(p_joint_list::const_iterator j=outer_joints.begin(); j!=outer_joints.end(); j++)
00381 {
00382 if(!(*j)->ParentSide())
00383 {
00384 top_pjoint = *j;
00385 break;
00386 }
00387 }
00388 if(!top_pjoint)
00389 {
00390 list_all_internal_joints_rev(sim->Root(), all_internal);
00391 }
00392 else
00393 {
00394 list_all_internal_joints_sub(top_pjoint->GetJoint()->child, all_internal);
00395 }
00396 #else
00397 if(last_proc - first_proc == 1)
00398 {
00399 calc_min_subchain_cost(outer_joints, all_internal);
00400 }
00401 else if(last_joint)
00402 {
00403 all_internal.push_back(last_joint);
00404 }
00405 #endif
00406 return 0;
00407 }
00408
00409 int dpScheduleNode::list_all_internal_joints_rev(Joint* cur, joint_list& all_internal)
00410 {
00411 if(!cur) return 0;
00412 pJoint* pjoints[2];
00413 sim->GetPJoint(cur, pjoints);
00414 for(p_joint_list::const_iterator j=outer_joints.begin(); j!=outer_joints.end(); j++)
00415 {
00416 if(pjoints[1] == *j)
00417 {
00418 return 0;
00419 }
00420 }
00421 all_internal.push_back(cur);
00422 list_all_internal_joints_rev(cur->child, all_internal);
00423 list_all_internal_joints_rev(cur->brother, all_internal);
00424 return 0;
00425 }
00426
00427 int dpScheduleNode::list_all_internal_joints_sub(Joint* cur, joint_list& all_internal)
00428 {
00429 if(!cur) return 0;
00430 pJoint* pjoints[2];
00431 sim->GetPJoint(cur, pjoints);
00432 for(p_joint_list::const_iterator j=outer_joints.begin(); j!=outer_joints.end(); j++)
00433 {
00434 if(pjoints[1] == *j)
00435 {
00436 return 0;
00437 }
00438 }
00439 list_all_internal_joints_sub(cur->child, all_internal);
00440 list_all_internal_joints_sub(cur->brother, all_internal);
00441 all_internal.push_back(cur);
00442 return 0;
00443 }
00444
00445 double dpScheduleNode::calc_min_subchain_cost(const p_joint_list& _outer_joints, joint_list& added_joints)
00446 {
00447 pJoint* top_pjoint = 0;
00448 logfile << "=== calc_min_subchain_cost ===" << endl;
00449 logfile << "outer = [" << flush;
00450
00451 for(p_joint_list::const_iterator j=_outer_joints.begin(); j!=_outer_joints.end(); j++)
00452 {
00453 logfile << " " << (*j)->GetJoint()->name << flush;
00454 if(!(*j)->ParentSide())
00455 {
00456 top_pjoint = *j;
00457
00458 }
00459 }
00460 logfile << "]" << endl;
00461
00462 int* n_outer_parent_side = new int [sim->NumJoint()];
00463 int* n_outer_child_side = new int [sim->NumJoint()];
00464 for(int i=0; i<sim->NumJoint(); i++)
00465 {
00466 n_outer_parent_side[i] = 0;
00467 n_outer_child_side[i] = 0;
00468 }
00469 for(p_joint_list::const_iterator j=_outer_joints.begin(); j!=_outer_joints.end(); j++)
00470 {
00471 if((*j)->ParentSide())
00472 {
00473 Joint* cur = (*j)->GetJoint();
00474 for(Joint* p=cur->parent; p; p=p->parent)
00475 {
00476 n_outer_child_side[p->i_joint]++;
00477 }
00478 }
00479 }
00480 for(int i=0; i<sim->NumJoint(); i++)
00481 {
00482 n_outer_parent_side[i] = _outer_joints.size() - n_outer_child_side[i];
00483
00484 }
00485 double ret = 0.0;
00486 added_joints.clear();
00487 if(!top_pjoint)
00488 {
00489 ret = calc_min_subchain_cost_sub(sim->Root(), _outer_joints, n_outer_parent_side, n_outer_child_side, added_joints);
00490 }
00491 else
00492 {
00493 joint_list sorted_child;
00494 for(Joint* c = top_pjoint->GetJoint()->child; c; c=c->brother)
00495 {
00496 int done = false;
00497 for(joint_list::iterator j=sorted_child.begin(); j!=sorted_child.end(); j++)
00498 {
00499 if(n_outer_child_side[c->i_joint] <= n_outer_child_side[(*j)->i_joint])
00500 {
00501 done = true;
00502 sorted_child.insert(j, c);
00503 break;
00504 }
00505 }
00506 if(!done) sorted_child.push_back(c);
00507 }
00508 for(joint_list::iterator j=sorted_child.begin(); j!=sorted_child.end(); j++)
00509 {
00510 ret += calc_min_subchain_cost_sub(*j, _outer_joints, n_outer_parent_side, n_outer_child_side, added_joints);
00511 }
00512 }
00513 delete[] n_outer_parent_side;
00514 delete[] n_outer_child_side;
00515 return ret;
00516 }
00517
00518 int is_in(const joint_list& jlist, Joint* jnt)
00519 {
00520 for(joint_list::const_iterator i=jlist.begin(); i!=jlist.end(); i++)
00521 {
00522 if(*i == jnt) return true;
00523 }
00524 return false;
00525 }
00526
00527 double dpScheduleNode::calc_min_subchain_cost_sub(Joint* cur, const p_joint_list& _outer_joints, int* n_outer_parent_side, int* n_outer_child_side, joint_list& added_joints)
00528 {
00529 if(!cur) return 0.0;
00530
00531 pJoint* pjoints[2];
00532 sim->GetPJoint(cur, pjoints);
00533 for(p_joint_list::const_iterator j=_outer_joints.begin(); j!=_outer_joints.end(); j++)
00534 {
00535 if(pjoints[1] == *j)
00536 {
00537 return 0.0;
00538 }
00539 }
00540 double ret = 0.0;
00541 int descend = false;
00542 int my_outer = n_outer_child_side[cur->i_joint];
00543 if(n_outer_parent_side[cur->i_joint] < n_outer_child_side[cur->i_joint])
00544 {
00545 descend = true;
00546 my_outer = n_outer_parent_side[cur->i_joint];
00547 }
00548
00549 if(!descend)
00550 {
00551 joint_list sorted_child;
00552 for(Joint* c = cur->child; c; c=c->brother)
00553 {
00554 int done = false;
00555 for(joint_list::iterator j=sorted_child.begin(); j!=sorted_child.end(); j++)
00556 {
00557 if(n_outer_child_side[c->i_joint] <= n_outer_child_side[(*j)->i_joint])
00558 {
00559 done = true;
00560 sorted_child.insert(j, c);
00561 break;
00562 }
00563 }
00564 if(!done) sorted_child.push_back(c);
00565 }
00566 for(joint_list::iterator j=sorted_child.begin(); j!=sorted_child.end(); j++)
00567 {
00568 ret += calc_min_subchain_cost_sub(*j, _outer_joints, n_outer_parent_side, n_outer_child_side, added_joints);
00569 }
00570 ret += add_to_parent_side(cur, added_joints, n_outer_parent_side, n_outer_child_side);
00571 }
00572
00573
00574 else
00575 {
00576 joint_list smaller_child, larger_child;
00577 for(Joint* c=cur->child; c; c=c->brother)
00578 {
00579 int n_child_outer = n_outer_child_side[c->i_joint];
00580 if(n_child_outer <= my_outer)
00581 {
00582
00583 int done = false;
00584 for(joint_list::iterator i=smaller_child.begin(); i!=smaller_child.end(); i++)
00585 {
00586 int n = n_outer_child_side[(*i)->i_joint];
00587 if(n_child_outer <= n)
00588 {
00589 smaller_child.insert(i, c);
00590 done = true;
00591 break;
00592 }
00593 }
00594 if(!done)
00595 {
00596 smaller_child.push_back(c);
00597 }
00598 }
00599 else
00600 {
00601
00602 int done = false;
00603 for(joint_list::iterator i=larger_child.begin(); i!=larger_child.end(); i++)
00604 {
00605 int n = n_outer_child_side[(*i)->i_joint];
00606 if(n_child_outer <= n)
00607 {
00608 larger_child.insert(i, c);
00609 done = true;
00610 break;
00611 }
00612 }
00613 if(!done)
00614 {
00615 larger_child.push_back(c);
00616 }
00617 }
00618 }
00619
00620 for(joint_list::iterator j=smaller_child.begin(); j!=smaller_child.end(); j++)
00621 {
00622
00623 ret += calc_min_subchain_cost_sub(*j, _outer_joints, n_outer_parent_side, n_outer_child_side, added_joints);
00624 }
00625 ret += add_to_child_side(cur, added_joints, n_outer_parent_side, n_outer_child_side);
00626
00627 for(joint_list::iterator j=larger_child.begin(); j!=larger_child.end(); j++)
00628 {
00629
00630 ret += calc_min_subchain_cost_sub(*j, _outer_joints, n_outer_parent_side, n_outer_child_side, added_joints);
00631 }
00632 }
00633
00634 return ret;
00635 }
00636
00637 double dpScheduleNode::add_to_child_side(Joint* cur, joint_list& added_joints, int* n_outer_parent_side, int* n_outer_child_side)
00638 {
00639 double ret = 0.0;
00640 int n_outer = n_outer_parent_side[cur->i_joint];
00641 int n_link_outer = 0;
00642 for(Joint* c=cur->child; c; c=c->brother)
00643 {
00644 if(!is_in(added_joints, c))
00645 {
00646 n_link_outer++;
00647 }
00648 }
00649 added_joints.push_back(cur);
00650 ret = calc_single_joint_cost(cur->n_dof, n_outer+n_link_outer);
00651 logfile << " adding " << cur->name << ", n_outer = " << n_outer << ", n_link_outer = " << n_link_outer << ", cost = " << ret << endl;
00652 return ret;
00653 }
00654
00655 double dpScheduleNode::add_to_parent_side(Joint* cur, joint_list& added_joints, int* n_outer_parent_side, int* n_outer_child_side)
00656 {
00657 double ret = 0.0;
00658 int n_outer = n_outer_child_side[cur->i_joint];
00659 int n_link_outer = 0;
00660 if(cur->parent && !is_in(added_joints, cur->parent))
00661 {
00662 n_link_outer++;
00663 }
00664 if(cur->parent)
00665 {
00666 for(Joint* c=cur->parent->child; c; c=c->brother)
00667 {
00668 if(!is_in(added_joints, c) && c!=cur)
00669 {
00670 n_link_outer++;
00671 }
00672 }
00673 }
00674 added_joints.push_back(cur);
00675 ret = calc_single_joint_cost(cur->n_dof, n_outer+n_link_outer);
00676 logfile << " adding " << cur->name << ", n_outer = " << n_outer << ", n_link_outer = " << n_link_outer << ", cost = " << ret << endl;
00677 return ret;
00678 }
00679
00680 dpScheduleNode* dpScheduleNode::find_available_parent(int target_depth, int& child_id)
00681 {
00682 dpScheduleNode* s, *ret = 0;
00683 for(s=this; s; s=(dpScheduleNode*)s->parent)
00684 {
00685 if((ret = find_available_parent_sub(s, target_depth, this, child_id)))
00686 return ret;
00687 }
00688 return 0;
00689 }
00690
00691 dpScheduleNode* dpScheduleNode::find_available_parent_sub(dpScheduleNode* cur, int target_depth, dpNode* cur_leaf, int& child_id)
00692 {
00693 if(!cur) return 0;
00694 if(cur->last_proc - cur->first_proc == 1) return 0;
00695 if(cur->schedule_depth == target_depth)
00696 {
00697 if(cur->internal_joints[0].size() > 0)
00698 {
00699 int failed = false;
00700 joint_list::iterator f;
00701 for(f=cur->internal_joints[0].begin(); f!=cur->internal_joints[0].end(); f++)
00702 {
00703 dpNode* n;
00704 for(n=cur_leaf; n; n=n->Parent())
00705 {
00706 dpScheduleNode* s = (dpScheduleNode*)n;
00707 if(*f == s->last_joint)
00708 {
00709 failed = true;
00710 break;
00711 }
00712 }
00713 if(failed) break;
00714 }
00715 if(!failed)
00716 {
00717 child_id = 0;
00718 return cur;
00719 }
00720 }
00721 if(cur->internal_joints[1].size() > 0)
00722 {
00723 int failed = false;
00724 joint_list::iterator f;
00725 for(f=cur->internal_joints[1].begin(); f!=cur->internal_joints[1].end(); f++)
00726 {
00727 dpNode* n;
00728 for(n=cur_leaf; n; n=n->Parent())
00729 {
00730 dpScheduleNode* s = (dpScheduleNode*)n;
00731 if(*f == s->last_joint)
00732 {
00733 failed = true;
00734 break;
00735 }
00736 }
00737 if(failed) break;
00738 }
00739 if(!failed)
00740 {
00741 child_id = 1;
00742 return cur;
00743 }
00744 }
00745 }
00746 return 0;
00747 }
00748
00749 static void get_all_joints(Joint* cur, joint_list& all_joints, joint_list& all_vjoints)
00750 {
00751 if(!cur) return;
00752 if(cur->real)
00753 {
00754 all_vjoints.push_back(cur);
00755 }
00756 else
00757 {
00758 all_joints.push_back(cur);
00759 }
00760 get_all_joints(cur->child, all_joints, all_vjoints);
00761 get_all_joints(cur->brother, all_joints, all_vjoints);
00762 return;
00763 }
00764
00765 int pSim::AutoSchedule(int _max_procs)
00766 {
00767 logfile << "AutoSchedule(max_procs = " << _max_procs << ")" << endl;
00768
00769 sim = this;
00770 max_procs = _max_procs;
00771
00772 joint_list init_internal_joints;
00773 p_joint_list init_outer_joints;
00774 joint_list all_vjoints;
00775
00776 get_all_joints(root, init_internal_joints, all_vjoints);
00777
00778 for(joint_list::iterator j=all_vjoints.begin(); j!=all_vjoints.end(); j++)
00779 {
00780 pJoint* v_pjoints[2];
00781 GetPJoint(*j, v_pjoints);
00782 init_outer_joints.push_back(v_pjoints[0]);
00783 init_outer_joints.push_back(v_pjoints[1]);
00784 }
00785 dpMain* dp = new dpMain;
00786 fVec init_proc_costs(max_procs);
00787 init_proc_costs.zero();
00788 dpScheduleNode* start_node = new dpScheduleNode(0, 0, init_proc_costs, 0, 0, 0, max_procs, init_internal_joints, init_outer_joints);
00789
00790 dp->SetStartNode(start_node);
00791 dp->Search(-1, 1);
00792 dpNode* goal = dp->BestGoal();
00793 if(!goal)
00794 {
00795 logfile << "pSim::AutoSchedule: goal not found" << endl;
00796 delete dp;
00797 return -1;
00798 }
00799 cerr << "goal found" << endl;
00800 cerr << "cost = " << goal->TotalCost() << endl;
00801 fVec& proc_costs = ((dpScheduleNode*)goal)->ProcCosts();
00802 cerr << "proc_costs = " << tran(proc_costs) << endl;
00803 Joint** joints = new Joint* [n_joint+all_vjoints.size()];
00804 dpNode* n;
00805 int count = 0;
00806 for(n=goal->Parent(); n; n=n->Parent())
00807 {
00808 dpScheduleNode* s = (dpScheduleNode*)n;
00809 if(s->LastJoint())
00810 {
00811 cerr << "[" << s->LastJoint()->name << "/" << s->ScheduleDepth() << "] cost = " << n->TotalCost() << ", procs = [" << s->FirstProc() << ", " << s->LastProc() << "]" << endl;
00812 }
00813 else
00814 {
00815 cerr << "[default/" << s->ScheduleDepth() << "]" << endl;
00816 }
00817 joint_list all_internal;
00818 s->ListAllInternalJoints(all_internal);
00819 logfile << "joints = " << endl;
00820 for(joint_list::iterator j=all_internal.begin(); j!=all_internal.end(); j++)
00821 {
00822 logfile << " " << count << " " << (*j)->name << endl;
00823 joints[count] = *j;
00824 count++;
00825 }
00826 }
00827 logfile << "current count = " << count << ", number of vjoints = " << all_vjoints.size() << ", total n_joint = " << n_joint << endl;
00828 logfile << "joints = " << endl;
00829 for(int i=0; i<count; i++)
00830 {
00831 logfile << "[" << i << "] = " << joints[i]->name << endl;
00832 }
00833 for(joint_list::iterator j=all_vjoints.begin(); j!=all_vjoints.end(); j++)
00834 {
00835 joints[count] = *j;
00836 logfile << "[" << count << "] = " << joints[count]->name << endl;
00837 count++;
00838 }
00839 #if 1
00840 Schedule(joints);
00841 #endif
00842 delete[] joints;
00843 delete dp;
00844 return 0;
00845 }
00846
00847 int dpScheduleNode::split_internal_joints(Joint* _last_joint, const joint_list& org_internal_joints, joint_list& _internal_joints_0, joint_list& _internal_joints_1)
00848 {
00849 _internal_joints_0.clear();
00850 _internal_joints_1.clear();
00851 joint_list::const_iterator j;
00852
00853 for(j=org_internal_joints.begin(); j!=org_internal_joints.end(); j++)
00854 {
00855
00856 if(*j == _last_joint)
00857 {
00858
00859 }
00860 else if((*j)->isAscendant(_last_joint))
00861 {
00862 _internal_joints_0.push_back(*j);
00863
00864 }
00865 else
00866 {
00867 _internal_joints_1.push_back(*j);
00868
00869 }
00870 }
00871
00872 return 0;
00873 }
00874
00875 int dpScheduleNode::set_outer_joints(pJoint* last_pjoint, const p_joint_list& org_outer_joints, p_joint_list& new_outer_joints)
00876 {
00877 new_outer_joints.clear();
00878 new_outer_joints.push_back(last_pjoint);
00879 p_joint_list::const_iterator p;
00880 int child_id = (last_pjoint->ParentSide()) ? 1 : 0;
00881 for(p=org_outer_joints.begin(); p!=org_outer_joints.end(); p++)
00882 {
00883 if((*p)->GetJoint()->isAscendant(last_pjoint->GetJoint()))
00884 {
00885 if(child_id == 0)
00886 new_outer_joints.push_back(*p);
00887 }
00888 else if(child_id == 1)
00889 {
00890 new_outer_joints.push_back(*p);
00891 }
00892 }
00893 return 0;
00894 }
00895
00896 #ifdef USE_MPI
00897
00900 int pSim::AssignProcesses(int max_procs)
00901 {
00902 size = max_procs;
00903 subchains->assign_processes(0, max_procs);
00904
00905 all_acc_types = new MPI_Datatype [max_procs];
00906 int** lengths = new int* [max_procs];
00907 MPI_Aint** disps = new MPI_Aint* [max_procs];
00908 MPI_Datatype** oldtypes = new MPI_Datatype* [max_procs];
00909 int* n_proc_joints = new int [max_procs];
00910 int i;
00911 for(i=0; i<max_procs; i++)
00912 {
00913 lengths[i] = new int [3*n_joint];
00914 disps[i] = new MPI_Aint [3*n_joint];
00915 oldtypes[i] = new MPI_Datatype [3*n_joint];
00916 n_proc_joints[i] = 0;
00917 }
00918 subchains->create_types(n_proc_joints, lengths, disps, oldtypes);
00919 for(i=0; i<max_procs; i++)
00920 {
00921 MPI_Type_create_struct(n_proc_joints[i], lengths[i], disps[i], oldtypes[i], all_acc_types+i);
00922 MPI_Type_commit(all_acc_types+i);
00923 logfile << "[" << rank << "]: all_acc_types[" << i << "] = " << all_acc_types[i] << endl;
00924 }
00925
00926 for(i=0; i<max_procs; i++)
00927 {
00928 delete[] lengths[i];
00929 delete[] disps[i];
00930 delete[] oldtypes[i];
00931 }
00932 delete[] lengths;
00933 delete[] disps;
00934 delete[] oldtypes;
00935 delete[] n_proc_joints;
00936 return 0;
00937 }
00938
00939
00940 int pSubChain::assign_processes(int start_rank, int end_rank)
00941 {
00942 rank = start_rank;
00943
00944 if(children[0] && children[1] && children[0] != children[1] &&
00945 children[0]->last_joint && children[1]->last_joint &&
00946 end_rank > start_rank+1)
00947 {
00948 int n_my_procs = end_rank - start_rank;
00949 int n_half_procs = n_my_procs/2;
00950 children[0]->assign_processes(start_rank, start_rank+n_half_procs);
00951 children[1]->assign_processes(start_rank+n_half_procs, end_rank);
00952 }
00953
00954 else
00955 {
00956 if(children[0])
00957 {
00958 children[0]->assign_processes(start_rank, end_rank);
00959 }
00960 if(children[0] != children[1] && children[1])
00961 {
00962 children[1]->assign_processes(start_rank, end_rank);
00963 }
00964 }
00965 return 0;
00966 }
00967
00968 int pSubChain::create_types(int* n_proc_joints, int** _lengths, MPI_Aint** _disps, MPI_Datatype** _oldtypes)
00969 {
00970 if(!this) return 0;
00971
00972 if(parent && rank != parent->rank)
00973 {
00974 logfile << "[" << sim->rank << "] create_types " << last_joint->name << endl;
00975 int n_mat = n_outer_joints * n_outer_joints;
00976 int count = 0;
00977 int* lengths = new int [n_mat];
00978 MPI_Aint* disps = new int [n_mat];
00979 MPI_Datatype* oldtypes = new int [n_mat];
00980 for(int i=0; i<n_outer_joints; i++)
00981 {
00982 for(int j=0; j<n_outer_joints; j++)
00983 {
00984 lengths[count] = 36;
00985 oldtypes[count] = MPI_DOUBLE;
00986 MPI_Get_address(Lambda[i][j].data(), disps+count);
00987 count++;
00988 }
00989 }
00990 MPI_Type_create_struct(n_mat, lengths, disps, oldtypes, &parent_lambda_type);
00991 MPI_Type_commit(&parent_lambda_type);
00992 logfile << "[" << sim->rank << "]: parent_lambda_type = " << parent_lambda_type << endl;
00993
00994 for(int i=0; i<n_outer_joints; i++)
00995 {
00996 lengths[i] = 6;
00997 oldtypes[i] = MPI_DOUBLE;
00998 MPI_Get_address(acc_temp[i].data(), disps+i);
00999 count++;
01000 }
01001 MPI_Type_create_struct(n_outer_joints, lengths, disps, oldtypes, &parent_acc_type);
01002 MPI_Type_commit(&parent_acc_type);
01003 logfile << "[" << sim->rank << "]: parent_acc_type = " << parent_acc_type << endl;
01004 delete[] lengths;
01005 delete[] disps;
01006 delete[] oldtypes;
01007 }
01008 if(children[0] && rank != children[0]->rank || children[1] && rank != children[1]->rank)
01009 {
01010 int n_array = n_outer_joints + 2;
01011 int* lengths = new int [n_array];
01012 MPI_Aint* disps = new int [n_array];
01013 MPI_Datatype* oldtypes = new int [n_array];
01014
01015 for(int i=0; i<n_outer_joints; i++)
01016 {
01017 lengths[i] = 6;
01018 oldtypes[i] = MPI_DOUBLE;
01019 MPI_Get_address(outer_joints[i]->f_final.data(), disps+i);
01020 }
01021
01022 lengths[n_outer_joints] = 6;
01023 lengths[n_outer_joints+1] = 6;
01024 oldtypes[n_outer_joints] = MPI_DOUBLE;
01025 oldtypes[n_outer_joints+1] = MPI_DOUBLE;
01026 MPI_Get_address(last_pjoints[0]->f_final.data(), disps+n_outer_joints);
01027 MPI_Get_address(last_pjoints[1]->f_final.data(), disps+n_outer_joints+1);
01028 MPI_Type_create_struct(n_outer_joints+2, lengths, disps, oldtypes, &parent_force_type);
01029 MPI_Type_commit(&parent_force_type);
01030 logfile << "[" << sim->rank << "]: parent_force_type = " << parent_force_type << endl;
01031 delete[] lengths;
01032 delete[] disps;
01033 delete[] oldtypes;
01034 }
01035
01036 if(last_joint && last_joint->n_dof > 0)
01037 {
01038 int index = n_proc_joints[rank];
01039 _oldtypes[rank][index] = MPI_DOUBLE;
01040 _oldtypes[rank][index+1] = MPI_DOUBLE;
01041 _oldtypes[rank][index+2] = MPI_DOUBLE;
01042 MPI_Get_address(acc_final.data(), _disps[rank]+index);
01043 MPI_Get_address(last_pjoints[0]->f_final.data(), _disps[rank]+index+1);
01044 MPI_Get_address(last_pjoints[1]->f_final.data(), _disps[rank]+index+2);
01045 _lengths[rank][index] = acc_final.size();
01046 _lengths[rank][index+1] = 6;
01047 _lengths[rank][index+2] = 6;
01048 n_proc_joints[rank] += 3;
01049 }
01050 children[0]->create_types(n_proc_joints, _lengths, _disps, _oldtypes);
01051 if(children[0] != children[1]) children[1]->create_types(n_proc_joints, _lengths, _disps, _oldtypes);
01052 return 0;
01053 }
01054
01055 #endif
01056
01060 int pSim::Schedule()
01061 {
01062 if(subchains) delete subchains;
01063 subchains = 0;
01064
01065 subchains = default_schedule(0, root);
01066
01067 default_schedule_virtual(root);
01068 subchains->init();
01069 return 0;
01070 }
01071
01072 pSubChain* pSim::default_schedule(pSubChain* p, Joint* j)
01073 {
01074 if(!j) return 0;
01075 if(j->i_joint < 0) return 0;
01076 if(j->real) return 0;
01077
01078 if(j->mass < TINY_MASS && j->parent)
01079 {
01080 pSubChain* c1 = default_schedule(p, j->brother);
01081 if(c1) return c1;
01082 pSubChain* c0 = default_schedule(p, j->child);
01083 return c0;
01084 }
01085
01086 pJoint* pj0, *pj1;
01087 pj0 = joint_info[j->i_joint].pjoints[0];
01088 pj1 = joint_info[j->i_joint].pjoints[1];
01089 pSubChain* sc = new pSubChain(this, p, pj0, pj1);
01090
01091 pSubChain* c0 = 0, *c1 = 0;
01092
01093 if(j->brother)
01094 {
01095 c1 = default_schedule(sc, j->brother);
01096 }
01097 if(!c1 && j->parent && j->parent->i_joint >= 0)
01098 {
01099 pLink* pl = joint_info[j->parent->i_joint].plink;
01100 c1 = new pSubChain(this, sc, pl);
01101 }
01102
01103 if(j->child && !j->child->real)
01104 {
01105 c0 = default_schedule(sc, j->child);
01106 }
01107 if(!c0 && j->i_joint >= 0)
01108 {
01109 pLink* pl = joint_info[j->i_joint].plink;
01110 c0 = new pSubChain(this, sc, pl);
01111 }
01112 sc->children[0] = c0;
01113 sc->children[1] = c1;
01114 return sc;
01115 }
01116
01117 void pSim::default_schedule_virtual(Joint* j)
01118 {
01119 if(!j) return;
01120 if(j->i_joint < 0) return;
01121
01122 if(j->real && contact_vjoint_index(j) < 0)
01123 {
01124 pJoint* pj0, *pj1;
01125 pj0 = joint_info[j->i_joint].pjoints[0];
01126 pj1 = joint_info[j->i_joint].pjoints[1];
01127 pSubChain* sc = new pSubChain(this, subchains, pj0, pj1);
01128
01129
01130 sc->children[0] = subchains->children[0];
01131 sc->children[1] = subchains->children[0];
01132
01133 subchains->children[0]->parent = sc;
01134 subchains->children[0] = sc;
01135 }
01136 default_schedule_virtual(j->brother);
01137 default_schedule_virtual(j->child);
01138 }
01139
01140 void pSubChain::init()
01141 {
01142 if(!this) return;
01143 children[0]->init();
01144 if(children[0] != children[1]) children[1]->init();
01145 if(Lambda)
01146 {
01147 for(int i=0; i<n_outer_joints; i++) delete[] Lambda[i];
01148 delete[] Lambda;
01149 }
01150 Lambda = 0;
01151 if(acc_temp) delete[] acc_temp;
01152 acc_temp = 0;
01153 if(vel_temp) delete[] vel_temp;
01154 vel_temp = 0;
01155 n_outer_joints = 0;
01156 if(outer_joints) delete[] outer_joints;
01157 if(outer_joints_origin) delete[] outer_joints_origin;
01158 if(outer_joints_index) delete[] outer_joints_index;
01159 outer_joints = 0;
01160 outer_joints_origin = 0;
01161 outer_joints_index = 0;
01162
01163 if(!last_joint && n_links == 1)
01164 {
01165 int i;
01166
01167 Joint* p = links[0]->joint;
01168 links[0]->subchain = this;
01169 n_outer_joints = 1;
01170 Joint* cur;
01171 for(cur=p->child; cur; cur=cur->brother)
01172 {
01173 if(cur->real || cur->mass > TINY_MASS)
01174 n_outer_joints++;
01175 }
01176
01177 for(i=0; i<sim->n_joint; i++)
01178 {
01179 if(sim->joint_info[i].pjoints[0]->joint->real == links[0]->joint)
01180 n_outer_joints++;
01181 }
01182 outer_joints = new pJoint* [n_outer_joints];
01183 outer_joints_origin = new int [n_outer_joints];
01184 outer_joints_index = new int [n_outer_joints];
01185 outer_joints[0] = sim->joint_info[p->i_joint].pjoints[0];
01186 outer_joints_origin[0] = -1;
01187 outer_joints_index[0] = -1;
01188 n_outer_joints = 1;
01189 for(cur=p->child; cur; cur=cur->brother)
01190 {
01191 if(cur->real || cur->mass > TINY_MASS)
01192 {
01193 outer_joints[n_outer_joints] = sim->joint_info[cur->i_joint].pjoints[1];
01194 outer_joints[n_outer_joints]->subchain = this;
01195 outer_joints_origin[n_outer_joints] = -1;
01196 outer_joints_index[n_outer_joints] = -1;
01197 n_outer_joints++;
01198 }
01199 }
01200 for(i=0; i<sim->n_joint; i++)
01201 {
01202 if(sim->joint_info[i].pjoints[0]->joint->real == links[0]->joint)
01203 {
01204 outer_joints[n_outer_joints] = sim->joint_info[i].pjoints[0];
01205 outer_joints[n_outer_joints]->subchain = this;
01206 outer_joints_origin[n_outer_joints] = -1;
01207 outer_joints_index[n_outer_joints] = -1;
01208 n_outer_joints++;
01209 }
01210 }
01211 }
01212
01213 else
01214 {
01215 int i, count;
01216
01217 if(children[0]) last_index[0] = children[0]->get_outer_index(last_pjoints[0]);
01218 if(children[1]) last_index[1] = children[1]->get_outer_index(last_pjoints[1]);
01219
01220 n_outer_joints = children[0]->n_outer_joints - 1;
01221 if(children[0] == children[1])
01222 n_outer_joints--;
01223 else if(children[1])
01224 n_outer_joints += children[1]->n_outer_joints - 1;
01225 if(n_outer_joints > 0)
01226 {
01227 outer_joints = new pJoint* [n_outer_joints];
01228 outer_joints_origin = new int [n_outer_joints];
01229 outer_joints_index = new int [n_outer_joints];
01230 count = 0;
01231 for(i=0; i<children[0]->n_outer_joints; i++)
01232 {
01233 if(children[0]->outer_joints[i]->joint != last_joint)
01234 {
01235 outer_joints[count] = children[0]->outer_joints[i];
01236 outer_joints_origin[count] = 0;
01237 if(children[0] == children[1] &&
01238 outer_joints[count]->joint->real &&
01239 outer_joints[count]->joint->real != outer_joints[count]->link_side)
01240 {
01241 outer_joints_origin[count] = 1;
01242 }
01243 outer_joints_index[count] = i;
01244 count++;
01245 }
01246 }
01247 if(children[1] && children[0] != children[1])
01248 {
01249 for(i=0; i<children[1]->n_outer_joints; i++)
01250 {
01251 if(children[1]->outer_joints[i]->joint != last_joint)
01252 {
01253 outer_joints[count] = children[1]->outer_joints[i];
01254 outer_joints_origin[count] = 1;
01255 outer_joints_index[count] = i;
01256 count++;
01257 }
01258 }
01259 }
01260 }
01261
01262 n_links = children[0]->n_links;
01263 if(children[1] && children[0] != children[1])
01264 n_links += children[1]->n_links;
01265 if(links) delete[] links;
01266 links = 0;
01267 links = new pLink* [n_links];
01268 count = 0;
01269 for(i=0; i<children[0]->n_links; i++)
01270 {
01271 links[count] = children[0]->links[i];
01272 count++;
01273 }
01274 if(children[1] && children[0] != children[1])
01275 {
01276 for(i=0; i<children[1]->n_links; i++)
01277 {
01278 links[count] = children[1]->links[i];
01279 count++;
01280 }
01281 }
01282 }
01283
01284 P.resize(6, 6);
01285 P.zero();
01286 da6.resize(6);
01287 da6.zero();
01288 W.resize(6, 6);
01289 W.zero();
01290 IW.resize(6, 6);
01291 IW.zero();
01292 #ifdef USE_DCA
01293 Vhat.resize(6,6);
01294 Vhat.zero();
01295 SVS.resize(n_dof, n_dof);
01296 SVS.zero();
01297 #else
01298 Gamma.resize(n_const, n_const);
01299 Gamma.zero();
01300 Gamma_inv.resize(n_const, n_const);
01301 Gamma_inv.zero();
01302 #endif
01303 f_temp.resize(n_const);
01304 f_temp.zero();
01305 colf_temp.resize(n_const);
01306 colf_temp.zero();
01307 tau.resize(n_dof);
01308 tau.zero();
01309 acc_final.resize(n_dof);
01310 acc_final.zero();
01311 if(n_outer_joints > 0)
01312 {
01313 int i, j;
01314 Lambda = new fMat* [n_outer_joints];
01315 acc_temp = new fVec [n_outer_joints];
01316 vel_temp = new fVec [n_outer_joints];
01317 for(i=0; i<n_outer_joints; i++)
01318 {
01319 Lambda[i] = new fMat [n_outer_joints];
01320 for(j=0; j<n_outer_joints; j++)
01321 {
01322 Lambda[i][j].resize(6, 6);
01323 Lambda[i][j].zero();
01324 }
01325 acc_temp[i].resize(6);
01326 acc_temp[i].zero();
01327 vel_temp[i].resize(6);
01328 vel_temp[i].zero();
01329 }
01330 }
01331 }
01332
01336 int pSim::Schedule(Joint** joints)
01337 {
01338 if(subchains) delete subchains;
01339 subchains = 0;
01340 subchain_list buf;
01341 build_subchain_tree(n_joint, joints, buf);
01342 if(buf.size() == 1)
01343 {
01344 subchains = *(buf.begin());
01345 subchains->init();
01346 return 0;
01347 }
01348 cerr << "pSim::Schedule(joints): error- invalid joint order" << endl;
01349 return -1;
01350 }
01351
01352 int pSim::build_subchain_tree(int _n_joints, Joint** joints, subchain_list& buf)
01353 {
01354 int i;
01355 for(i=0; i<_n_joints; i++)
01356 {
01357 cerr << "build_subchain_tree " << joints[i]->name << endl;
01358 build_subchain_tree(joints[i], buf);
01359 }
01360 return 0;
01361 }
01362
01363 void pSim::build_subchain_tree(Joint* cur_joint, subchain_list& buf)
01364 {
01365 JointInfo& jinfo = joint_info[cur_joint->i_joint];
01366 pJoint* pj0 = jinfo.pjoints[0];
01367 pJoint* pj1 = jinfo.pjoints[1];
01368 int pj0_done = false;
01369 int pj1_done = false;
01370 pSubChain* to_remove[2] = {0, 0};
01371 pSubChain* myp = new pSubChain(this, 0, pj0, pj1);
01372 subchain_list::iterator i;
01373 for(i=buf.begin(); i!=buf.end(); i++)
01374 {
01375 if(!pj0_done && in_subchain(*i, pj0->plink))
01376 {
01377 myp->children[0] = *i;
01378 (*i)->parent = myp;
01379 pj0_done = true;
01380 if(!to_remove[0]) to_remove[0] = *i;
01381 else if(!to_remove[1]) to_remove[1] = *i;
01382 }
01383 if(!pj1_done && in_subchain(*i, pj1->plink))
01384 {
01385 myp->children[1] = *i;
01386 (*i)->parent = myp;
01387 pj1_done = true;
01388 if(!to_remove[0]) to_remove[0] = *i;
01389 else if(!to_remove[1]) to_remove[1] = *i;
01390 }
01391 }
01392 if(!pj0_done && pj0->plink)
01393 {
01394 pSubChain* newp = new pSubChain(this, myp, pj0->plink);
01395 myp->children[0] = newp;
01396 }
01397 if(!pj1_done && pj1->plink)
01398 {
01399 pSubChain* newp = new pSubChain(this, myp, pj1->plink);
01400 myp->children[1] = newp;
01401 }
01402 if(to_remove[0]) buf.remove(to_remove[0]);
01403 if(to_remove[1]) buf.remove(to_remove[1]);
01404 buf.push_front(myp);
01405 }
01406
01407 int pSim::in_subchain(pSubChain* sc, pLink* pl)
01408 {
01409 if(!sc) return 0;
01410 if(sc->links && sc->links[0] == pl) return 1;
01411 if(in_subchain(sc->children[0], pl)) return 1;
01412 if(sc->children[0] != sc->children[1] &&
01413 in_subchain(sc->children[1], pl)) return 1;
01414 return 0;
01415 }