f117e54959bbc0cd8ab82ce25c65cd31b031e685
[blender.git] / intern / itasc / kdl / tree.cpp
1 // Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
2
3 // Version: 1.0
4 // Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
5 // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
6 // URL: http://www.orocos.org/kdl
7
8 // This library is free software; you can redistribute it and/or
9 // modify it under the terms of the GNU Lesser General Public
10 // License as published by the Free Software Foundation; either
11 // version 2.1 of the License, or (at your option) any later version.
12
13 // This library is distributed in the hope that it will be useful,
14 // but WITHOUT ANY WARRANTY; without even the implied warranty of
15 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
16 // Lesser General Public License for more details.
17
18 // You should have received a copy of the GNU Lesser General Public
19 // License along with this library; if not, write to the Free Software
20 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
21
22 #include "tree.hpp"
23 #include <sstream>
24 namespace KDL {
25 using namespace std;
26
27 Tree::Tree() :
28     nrOfJoints(0), nrOfSegments(0) {
29     segments.insert(make_pair("root", TreeElement::Root()));
30 }
31
32 Tree::Tree(const Tree& in) {
33     segments.clear();
34     nrOfSegments = 0;
35     nrOfJoints = 0;
36
37     segments.insert(make_pair("root", TreeElement::Root()));
38     this->addTree(in, "", "root");
39
40 }
41
42 Tree& Tree::operator=(const Tree& in) {
43     segments.clear();
44     nrOfSegments = 0;
45     nrOfJoints = 0;
46
47     segments.insert(make_pair("root", TreeElement::Root()));
48     this->addTree(in, "", "root");
49     return *this;
50 }
51
52 bool Tree::addSegment(const Segment& segment, const std::string& segment_name,
53         const std::string& hook_name) {
54     SegmentMap::iterator parent = segments.find(hook_name);
55     //check if parent exists
56     if (parent == segments.end())
57         return false;
58     pair<SegmentMap::iterator, bool> retval;
59     //insert new element
60     retval = segments.insert(make_pair(segment_name, TreeElement(segment,
61             parent, nrOfJoints)));
62     //check if insertion succeeded
63     if (!retval.second)
64         return false;
65     //add iterator to new element in parents children list
66     parent->second.children.push_back(retval.first);
67     //increase number of segments
68     nrOfSegments++;
69     //increase number of joints
70         nrOfJoints += segment.getJoint().getNDof();
71     return true;
72 }
73
74 bool Tree::addChain(const Chain& chain, const std::string& chain_name,
75         const std::string& hook_name) {
76     string parent_name = hook_name;
77     for (unsigned int i = 0; i < chain.getNrOfSegments(); i++) {
78         ostringstream segment_name;
79         segment_name << chain_name << "Segment" << i;
80         if (this->addSegment(chain.getSegment(i), segment_name.str(),
81                 parent_name))
82             parent_name = segment_name.str();
83         else
84             return false;
85     }
86     return true;
87 }
88
89 bool Tree::addTree(const Tree& tree, const std::string& tree_name,
90         const std::string& hook_name) {
91     return this->addTreeRecursive(tree.getSegment("root"), tree_name, hook_name);
92 }
93
94 bool Tree::addTreeRecursive(SegmentMap::const_iterator root,
95         const std::string& tree_name, const std::string& hook_name) {
96     //get iterator for root-segment
97     SegmentMap::const_iterator child;
98     //try to add all of root's children
99     for (unsigned int i = 0; i < root->second.children.size(); i++) {
100         child = root->second.children[i];
101         //Try to add the child
102         if (this->addSegment(child->second.segment, tree_name + child->first,
103                 hook_name)) {
104             //if child is added, add all the child's children
105             if (!(this->addTreeRecursive(child, tree_name, tree_name
106                     + child->first)))
107                 //if it didn't work, return false
108                 return false;
109         } else
110             //If the child could not be added, return false
111             return false;
112     }
113     return true;
114 }
115
116 }
117