LinkPath.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
3  * All rights reserved. This program is made available under the terms of the
4  * Eclipse Public License v1.0 which accompanies this distribution, and is
5  * available at http://www.eclipse.org/legal/epl-v10.html
6  * Contributors:
7  * National Institute of Advanced Industrial Science and Technology (AIST)
8  */
9 
16 #include "LinkPath.h"
17 #include "Link.h"
18 #include <algorithm>
19 
20 using namespace std;
21 using namespace hrp;
22 
23 
24 LinkPath::LinkPath()
25 {
26 
27 }
28 
29 
30 LinkPath::LinkPath(Link* base, Link* end)
31 {
32  find(base, end);
33 }
34 
35 
37 LinkPath::LinkPath(Link* base)
38 {
39  find(base);
40 }
41 
42 
44 void LinkPath::find(Link* root, bool doUpward, bool doDownward)
45 {
46  throw "The find method for LinkTraverse cannot be used in LinkPath";
47 }
48 
49 
50 bool LinkPath::find(Link* base, Link* end)
51 {
52  links.clear();
53  numUpwardConnections = 0;
54  bool found = findPathSub(base, 0, end, false);
55  if(!found){
56  links.clear();
57  }
58  return found;
59 }
60 
61 
62 bool LinkPath::findPathSub(Link* link, Link* prev, Link* end, bool isUpward)
63 {
64  links.push_back(link);
65  if(isUpward){
66  ++numUpwardConnections;
67  }
68 
69  if(link == end){
70  return true;
71  }
72 
73  for(Link* child = link->child; child; child = child->sibling){
74  if(child != prev){
75  if(findPathSub(child, link, end, false)){
76  return true;
77  }
78  }
79  }
80 
81  Link* parent = link->parent;
82  if(parent && parent != prev){
83  if(findPathSub(parent, link, end, true)){
84  return true;
85  }
86  }
87 
88  links.pop_back();
89  if(isUpward){
90  --numUpwardConnections;
91  }
92 
93  return false;
94 }
95 
96 
98 void LinkPath::find(Link* end)
99 {
100  links.clear();
101  numUpwardConnections = 0;
102  findPathFromRootSub(end);
103  std::reverse(links.begin(), links.end());
104 }
105 
106 
107 void LinkPath::findPathFromRootSub(Link* link)
108 {
109  links.push_back(link);
110  if(link->parent){
111  findPathFromRootSub(link->parent);
112  }
113 }
hrp
Definition: ColdetModel.h:28
LinkPath.h


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Wed Sep 7 2022 02:51:03