LinkPath.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  * National Institute of Advanced Industrial Science and Technology (AIST)
00008  */
00009 
00016 #include "LinkPath.h"
00017 #include "Link.h"
00018 #include <algorithm>
00019 
00020 using namespace std;
00021 using namespace hrp;
00022 
00023 
00024 LinkPath::LinkPath()
00025 {
00026 
00027 }
00028 
00029 
00030 LinkPath::LinkPath(Link* base, Link* end)
00031 {
00032     find(base, end);
00033 }
00034 
00035 
00037 LinkPath::LinkPath(Link* base)
00038 {
00039     find(base);
00040 }
00041 
00042 
00044 void LinkPath::find(Link* root, bool doUpward, bool doDownward)
00045 {
00046     throw "The find method for LinkTraverse cannot be used in LinkPath";
00047 }
00048 
00049 
00050 bool LinkPath::find(Link* base, Link* end)
00051 {
00052     links.clear();
00053     numUpwardConnections = 0;
00054     bool found = findPathSub(base, 0, end, false);
00055     if(!found){
00056         links.clear();
00057     }
00058     return found;
00059 }
00060 
00061 
00062 bool LinkPath::findPathSub(Link* link, Link* prev, Link* end, bool isUpward)
00063 {
00064     links.push_back(link);
00065     if(isUpward){
00066         ++numUpwardConnections;
00067     }
00068     
00069     if(link == end){
00070         return true;
00071     }
00072 
00073     for(Link* child = link->child; child; child = child->sibling){
00074         if(child != prev){
00075             if(findPathSub(child, link, end, false)){
00076                 return true;
00077             }
00078         }
00079     }
00080 
00081     Link* parent = link->parent;
00082     if(parent && parent != prev){
00083         if(findPathSub(parent, link, end, true)){
00084             return true;
00085         }
00086     }
00087 
00088     links.pop_back();
00089     if(isUpward){
00090         --numUpwardConnections;
00091     }
00092 
00093     return false;
00094 }
00095 
00096 
00098 void LinkPath::find(Link* end)
00099 {
00100     links.clear();
00101     numUpwardConnections = 0;
00102     findPathFromRootSub(end);
00103     std::reverse(links.begin(), links.end());
00104 }
00105 
00106 
00107 void LinkPath::findPathFromRootSub(Link* link)
00108 {
00109     links.push_back(link);
00110     if(link->parent){
00111         findPathFromRootSub(link->parent);
00112     }
00113 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Apr 11 2019 03:30:17