← Back to branch summary

~robot3d-team/robot3d/trunk

« back to all changes in this revision

Viewing changes to src/srCore/organismTree.cpp

  • Committer: Anne van Rossum
  • Date: 2010-08-10 15:58:55 UTC
  • Revision ID: anne@gamix-20100810155855-kve7x2vwouagdij9
Initial import

Show diffs side-by-side

added added

removed removed

Lines of Context:
src/srCore/organismTree.cpp
 
1
/*
 
2
 * organismTree.cpp
 
3
 *
 
4
 * Tree structure of organism
 
5
 *
 
6
 *  Created on: May 25, 2010
 
7
 *      Author: Friebel
 
8
 */
 
9
 
 
10
#include <algorithm>
 
11
#include <map>
 
12
#include <math.h>
 
13
#include <srCore/organismTree.h>
 
14
 
 
15
 
 
16
treeNode::treeNode(string id, string rob, string con, osg::Vec3f pos, osg::Quat quat)
 
17
{
 
18
        data.terminalID         = id;
 
19
        data.robotName          = rob;
 
20
        data.controllerName = con;
 
21
        data.pos                        = pos;
 
22
        data.quat                       = quat;
 
23
 
 
24
        sides[0] = NULL;
 
25
        sides[1] = NULL;
 
26
        sides[2] = NULL;
 
27
        sides[3] = NULL;
 
28
 
 
29
        nextSibling = NULL;
 
30
        prevSibling = NULL;
 
31
 
 
32
        parent = -1;
 
33
}
 
34
 
 
35
treeNode::treeNode(string id, string rob, string con, osg::Vec3f pos, osg::Quat quat, short sideParent)
 
36
{
 
37
        data.terminalID         = id;
 
38
        data.robotName          = rob;
 
39
        data.controllerName = con;
 
40
        data.pos                        = pos;
 
41
        data.quat                       = quat;
 
42
 
 
43
        sides[0] = NULL;
 
44
        sides[1] = NULL;
 
45
        sides[2] = NULL;
 
46
        sides[3] = NULL;
 
47
 
 
48
        nextSibling = NULL;
 
49
        prevSibling = NULL;
 
50
 
 
51
        parent = sideParent;
 
52
}
 
53
 
 
54
treeNode::~treeNode()
 
55
{
 
56
        nextSibling = NULL;
 
57
        prevSibling = NULL;
 
58
 
 
59
        if(parent != -1)
 
60
        {
 
61
                treeNode* parentNode = sides[parent]->parentNode;
 
62
 
 
63
                parentNode->sides[ sides[parent]->sideParent ] = NULL;
 
64
 
 
65
                std::vector<short>::iterator iter(parentNode->children.begin());
 
66
                int i=0;
 
67
                while(parentNode->children[i] != sides[parent]->sideParent)
 
68
                {
 
69
                        ++iter;
 
70
                        i++;
 
71
                }
 
72
                iter+=i;
 
73
 
 
74
                parentNode->children.erase(iter);
 
75
                delete sides[parent];
 
76
 
 
77
                cout << "organismTree: " << data.robotName << " deleted" << endl;
 
78
        }
 
79
}
 
80
 
 
81
void treeNode::addEdge(treeNode* node, short sideChild, short sideParent, short rot)
 
82
{
 
83
        sides[sideParent] = new treeEdge(this, node, sideChild, sideParent, rot);
 
84
 
 
85
        children.push_back(sideParent);
 
86
 
 
87
        node->sides[sideChild] = sides[sideParent];
 
88
 
 
89
        std::sort(children.begin(), children.end());
 
90
 
 
91
        sides[children[0]]->childNode->prevSibling = NULL;
 
92
        sides[children[children.size()-1]]->childNode->nextSibling = NULL;
 
93
 
 
94
        for(unsigned int i=0; i<children.size(); i++)
 
95
        {
 
96
                if(i>0)
 
97
                        sides[children[i]]->childNode->prevSibling = sides[children[i-1]]->childNode;
 
98
                if(i<children.size()-1)
 
99
                        sides[children[i]]->childNode->nextSibling = sides[children[i+1]]->childNode;
 
100
        }
 
101
}
 
102
 
 
103
void treeNode::delEdge(int i)
 
104
{
 
105
        std::vector<short>::iterator iter(children.begin());
 
106
        iter+=i;
 
107
 
 
108
        treeNode* child = sides[children[i]]->childNode;
 
109
 
 
110
        child->sides[child->parent] = NULL;                                                                                     //loescht verweis auf kante des kindknotens
 
111
        child->parent = -1;                                                                                                                     //setzt den index der kante des kindknotens
 
112
 
 
113
        delete sides[children[i]];                                                                                                      //loescht kante
 
114
        children.erase(iter);                                                                                                           //loescht index der kante
 
115
 
 
116
        for(unsigned int i=0; i<children.size(); i++)
 
117
        {
 
118
                if(i>0)
 
119
                        sides[children[i]]->childNode->prevSibling = sides[children[i-1]]->childNode;
 
120
                else
 
121
                        sides[children[0]]->childNode->prevSibling = NULL;
 
122
 
 
123
                if(i<children.size()-1)
 
124
                        sides[children[i]]->childNode->nextSibling = sides[children[i+1]]->childNode;
 
125
                else
 
126
                        sides[children[children.size()-1]]->childNode->nextSibling = NULL;
 
127
        }
 
128
}
 
129
 
 
130
struct treeNodeComp
 
131
{
 
132
  bool operator() (treeNode* const &lhs, treeNode* const &rhs ) const
 
133
  {return *lhs<*rhs;}
 
134
};
 
135
 
 
136
bool treeNode::operator<(const treeNode& other) const
 
137
{
 
138
        short numChild[2];
 
139
        short maxDepth[2];
 
140
 
 
141
        if(parent==-1)
 
142
                numChild[0] = children.size();
 
143
        else
 
144
                numChild[0] = children.size() + 1;
 
145
 
 
146
        if(other.parent==-1)
 
147
                numChild[1] = other.children.size();
 
148
        else
 
149
                numChild[1] = other.children.size() + 1;
 
150
 
 
151
 
 
152
        if(numChild[0] == numChild[1])
 
153
        {
 
154
                maxDepth[0] = getMaxDepthAsRoot();
 
155
                maxDepth[1] = other.getMaxDepthAsRoot();
 
156
 
 
157
                if(maxDepth[0] == maxDepth[1])  return true;
 
158
                else return (maxDepth[0] < maxDepth[1]);
 
159
 
 
160
        }
 
161
        else return (numChild[0] > numChild[1]);
 
162
}
 
163
 
 
164
bool treeNode::isEqual(const treeNode* other) const
 
165
{
 
166
        short numChild[2];
 
167
        short maxDepth[2];
 
168
 
 
169
        if(parent==-1)
 
170
                numChild[0] = children.size();
 
171
        else
 
172
                numChild[0] = children.size() + 1;
 
173
 
 
174
        if(other->parent==-1)
 
175
                numChild[1] = other->children.size();
 
176
        else
 
177
                numChild[1] = other->children.size() + 1;
 
178
 
 
179
 
 
180
        if(numChild[0] == numChild[1])
 
181
        {
 
182
                maxDepth[0] = getMaxDepthAsRoot();
 
183
                maxDepth[1] = other->getMaxDepthAsRoot();
 
184
 
 
185
                return maxDepth[0] == maxDepth[1];
 
186
        }
 
187
        return false;
 
188
}
 
189
 
 
190
int treeNode::getMaxDepthAsRoot() const
 
191
{
 
192
        vector<short> maxDepthRootSides;
 
193
 
 
194
        if(parent==-1)
 
195
                maxDepthRootSides.push_back(maxDepth);
 
196
        else
 
197
        {
 
198
                short depth = 1;
 
199
                const treeNode* tmp = this;
 
200
                while(tmp->sides[tmp->parent]->parentNode->parent != -1)
 
201
                {
 
202
                        tmp = tmp->sides[tmp->parent]->parentNode;
 
203
                        depth++;
 
204
                }
 
205
 
 
206
                short side_root = tmp->sides[tmp->parent]->sideParent;
 
207
                tmp = tmp->sides[tmp->parent]->parentNode;
 
208
 
 
209
                if(tmp->children.size()==1)
 
210
                        maxDepthRootSides.push_back(depth);
 
211
                else
 
212
                        for(unsigned int i=0; i<tmp->children.size(); i++)
 
213
                                if(tmp->children[i]!=side_root)
 
214
                                        maxDepthRootSides.push_back(tmp->sides[tmp->children[i]]->childNode->maxDepth + depth);
 
215
                maxDepthRootSides.push_back(maxDepth-depth);
 
216
        }
 
217
        std::sort(maxDepthRootSides.begin(), maxDepthRootSides.end());
 
218
 
 
219
//      for(unsigned int i=0; i<maxDepthRootSides.size(); i++)
 
220
//              cout << "organismTree: " << data.robotName << " maxDepthRootSides[" << i << "]=" << maxDepthRootSides[i] << endl;
 
221
 
 
222
        return maxDepthRootSides[maxDepthRootSides.size()-1];
 
223
}
 
224
 
 
225
 
 
226
treeNode::treeEdge::treeEdge(treeNode* parent, treeNode* child, short sideChild, short sideParent, short rot)
 
227
{
 
228
        parentNode = parent;
 
229
        childNode  = child;
 
230
 
 
231
        this->sideChild  = sideChild;
 
232
        this->sideParent = sideParent;
 
233
        this->rot        = rot;
 
234
}
 
235
 
 
236
treeNode::treeEdge::~treeEdge()
 
237
{
 
238
        parentNode = NULL;
 
239
        childNode  = NULL;
 
240
}
 
241
 
 
242
 
 
243
organismTree::organismTree(string id, string rob, string con, osg::Vec3f pos, osg::Quat quat)
 
244
{
 
245
        treeNode* node = new treeNode(id, rob, con, pos, quat);
 
246
        node->maxDepth = 0;
 
247
 
 
248
        root = node;
 
249
        feet = node;
 
250
}
 
251
 
 
252
organismTree::~organismTree()
 
253
{
 
254
        erase(begin());
 
255
}
 
256
 
 
257
organismTree::iterator organismTree::seed(string id, string rob, string con, osg::Vec3f pos, osg::Quat quat)
 
258
{
 
259
        treeNode* node = new treeNode(id, rob, con, pos, quat);
 
260
        node->maxDepth = 0;
 
261
 
 
262
        root = node;
 
263
        feet = node;
 
264
 
 
265
        return iterator(root);
 
266
}
 
267
 
 
268
organismTree::iterator organismTree::append_child(iterator parentNode, string id, string rob, string con, osg::Vec3f pos, osg::Quat quat, short sideChild, short sideParent, short rot)
 
269
{
 
270
        treeNode* node = new treeNode(id, rob, con, pos, quat, sideChild);
 
271
 
 
272
        parentNode.node->addEdge(node, sideChild, sideParent, rot);
 
273
 
 
274
        node->maxDepth = getDepth(parentNode) + 1;
 
275
        treeNode* tmp = node;
 
276
        while(tmp->parent != -1)
 
277
        {
 
278
                if(tmp->sides[tmp->parent]->parentNode->maxDepth < tmp->maxDepth)
 
279
                {
 
280
                        tmp->sides[tmp->parent]->parentNode->maxDepth = tmp->maxDepth;
 
281
                        tmp = tmp->sides[tmp->parent]->parentNode;
 
282
                }
 
283
                else break;
 
284
        }
 
285
 
 
286
        feet = root;
 
287
        while(feet->children.size()>0)
 
288
                feet = feet->sides[feet->children[feet->children.size()-1]]->childNode;
 
289
 
 
290
        return iterator(node);
 
291
}
 
292
 
 
293
void organismTree::erase(iterator iter)
 
294
{
 
295
        vector<short> children_copy;
 
296
        bool need_new_feet = false;
 
297
 
 
298
        for(unsigned int i=0; i<iter.node->children.size(); i++)
 
299
                children_copy.push_back(iter.node->children[i]);
 
300
 
 
301
        for(unsigned int i=0; i<children_copy.size(); i++)
 
302
                erase(iterator(iter.node->sides[children_copy[i]]->childNode));
 
303
 
 
304
        if(iter.node->nextSibling) iter.node->nextSibling->prevSibling = iter.node->prevSibling;
 
305
        if(iter.node->prevSibling) iter.node->prevSibling->nextSibling = iter.node->nextSibling;
 
306
 
 
307
        if(iter.node == root) root = NULL;
 
308
        if(iter.node == feet) need_new_feet = true;
 
309
        delete iter.node;
 
310
 
 
311
        if(need_new_feet && root != NULL)
 
312
        {
 
313
                feet = root;
 
314
                while(feet->children.size()>0)
 
315
                        feet = feet->sides[feet->children[feet->children.size()-1]]->childNode;
 
316
        }
 
317
}
 
318
 
 
319
organismTree::iterator organismTree::begin()
 
320
{
 
321
        return iterator(root);
 
322
}
 
323
 
 
324
organismTree::iterator organismTree::end()
 
325
{
 
326
        return iterator(feet);
 
327
}
 
328
 
 
329
osg::Vec3f organismTree::getOrgPos()
 
330
{
 
331
        return root->data.pos;
 
332
}
 
333
 
 
334
osg::Quat organismTree::getOrgQuat()
 
335
{
 
336
        return root->data.quat;
 
337
}
 
338
 
 
339
int organismTree::getDepth(iterator iter)
 
340
{
 
341
        int i = 0;
 
342
        treeNode* tr = iter.node;
 
343
 
 
344
        while(tr != root)
 
345
        {
 
346
                tr = tr->sides[tr->parent]->parentNode;
 
347
                i++;
 
348
        }
 
349
        return i;
 
350
}
 
351
 
 
352
void organismTree::rotate(iterator par, iterator child)
 
353
{
 
354
        bool done = false;
 
355
 
 
356
        if(par.node != root)
 
357
                rotate(iterator(par.node->sides[par.node->parent]->parentNode), par);
 
358
 
 
359
        for(unsigned int i=0; i<par.node->children.size(); i++)
 
360
        {
 
361
                int t = par.node->children[i];
 
362
                if(par.node->sides[t]->childNode == child.node)
 
363
                {
 
364
                        short sideParent = par.node->sides[t]->sideParent;
 
365
                        short sideChild  = par.node->sides[t]->sideChild;
 
366
                        short rot        = par.node->sides[t]->rot;
 
367
 
 
368
                        par.node->delEdge(i);
 
369
                        child.node->addEdge(par.node, sideParent, sideChild, rot);
 
370
                        par.node->parent = sideParent;
 
371
                        child.node->nextSibling = NULL;
 
372
                        child.node->prevSibling = NULL;
 
373
 
 
374
                        if(root == par.node)   root = child.node;
 
375
                        if(feet == child.node) feet = par.node;
 
376
 
 
377
                        done = true;
 
378
                }
 
379
        }
 
380
 
 
381
        if(!done) {
 
382
                cout << "Error: <organismTree.cpp>   ::rotate(iter arg1, iter arg2)  Error, arg1 is not parent of arg2:" << endl;
 
383
        }
 
384
}
 
385
 
 
386
void organismTree::printTree()
 
387
{
 
388
        iterator iter = begin();
 
389
        bool feet_touched = false;
 
390
 
 
391
        while(!feet_touched)
 
392
        {
 
393
                cout << "organismTree: ";
 
394
                if(iter.node->children.size()==0)
 
395
                        cout << iter->robotName << "(depth= " << getDepth(iter) << "), (maxDepth= " << iter.node->maxDepth << ") is leaf node. " << endl;
 
396
                else
 
397
                {
 
398
                        cout << iter->robotName << "(depth= " << getDepth(iter) << "), (maxDepth= " << iter.node->maxDepth << ") is parent of: ";
 
399
                        for(unsigned int i=0; i<iter.node->children.size(); i++)
 
400
                        {
 
401
                                cout << iter.node->sides[iter.node->children[i]]->childNode->data.robotName;
 
402
                                cout << " (" << iter.node->sides[iter.node->children[i]]->sideParent << ", ";
 
403
                                cout << iter.node->sides[iter.node->children[i]]->sideChild << "), ";
 
404
                        }
 
405
                        cout << endl;
 
406
                }
 
407
 
 
408
                if(iter == feet)        feet_touched = true;
 
409
                else                            ++iter;
 
410
        }
 
411
}
 
412
 
 
413
string organismTree::generateString()
 
414
{
 
415
        string unambig;
 
416
        bool feet_touched = false;
 
417
        iterator iter = begin();
 
418
        map<int, string> side;
 
419
        map<int, string> rot;
 
420
 
 
421
        side[0] = "A";  rot[0] = "";
 
422
        side[1] = "B";  rot[1] = "1";
 
423
        side[2] = "C";  rot[2] = "2";
 
424
        side[3] = "D";  rot[3] = "3";
 
425
 
 
426
        unambig = iter->terminalID;
 
427
//      cout << "organismTree: " << unambig <<endl;
 
428
 
 
429
        if(iter == this->end()) feet_touched = true;
 
430
 
 
431
        while( !(iter==root && feet_touched) ) {
 
432
                if(iter.node->children.size() != 0) {
 
433
                        iter.node = iter.node->sides[iter.node->children[0]]->childNode;
 
434
 
 
435
                        unambig = unambig + "[" + side[iter.node->sides[iter.node->parent]->sideParent] + side[iter.node->sides[iter.node->parent]->sideChild];
 
436
                        unambig = unambig + rot[iter.node->sides[iter.node->parent]->rot];
 
437
                        unambig = unambig + iter->terminalID;
 
438
//                      cout << "organismTree: " << unambig <<endl;
 
439
                }
 
440
                else {
 
441
                        while(iter.node->nextSibling == NULL) {
 
442
                                iter.node = iter.node->sides[iter.node->parent]->parentNode;
 
443
                                unambig = unambig + "]";
 
444
//                              cout << "organismTree: " << unambig <<endl;
 
445
 
 
446
                                if(root==iter.node && feet_touched)
 
447
                                        return unambig;
 
448
                        }
 
449
                        iter.node = iter.node->nextSibling;
 
450
 
 
451
                        unambig = unambig + "]" + "[" + side[iter.node->sides[iter.node->parent]->sideParent] + side[iter.node->sides[iter.node->parent]->sideChild];
 
452
                        unambig = unambig + rot[iter.node->sides[iter.node->parent]->rot];
 
453
                        unambig = unambig + iter->terminalID;
 
454
//                      cout << "organismTree: " << unambig <<endl;
 
455
                }
 
456
 
 
457
                if(iter == this->end()) feet_touched = true;
 
458
        }
 
459
 
 
460
        return unambig;
 
461
}
 
462
 
 
463
void organismTree::generUnambigTree()
 
464
{
 
465
        typedef map<treeNode*, int, treeNodeComp> mapType1;                     //container for the nodes of the tree and their index
 
466
        mapType1 nodes;
 
467
        typedef map<string, treeNode*> mapType2;                                                //container for the best strings and the pointer to their root nodes
 
468
        mapType2 candidates;
 
469
 
 
470
        iterator iter = begin();
 
471
        treeNode* root_backup = iter.node;                                                              //pointer to the root node
 
472
        int      i    = 0;
 
473
        bool feet_touched = false;
 
474
 
 
475
        while(!feet_touched)                                                                                    //fill the nodes and their index in the container
 
476
        {
 
477
 
 
478
                nodes.insert(std::pair<treeNode*, int>(iter.node, i));
 
479
 
 
480
                if(iter == feet) feet_touched = true;
 
481
                else {  ++iter; ++i; }
 
482
        }
 
483
        std::cout << "org:feet = " << feet->data.robotName << std::endl;
 
484
        std::cout << "org:map size = " << nodes.size() << std::endl;
 
485
 
 
486
        for(mapType1::iterator it = nodes.begin(); it != nodes.end(); it++)             //print the nodes in order
 
487
                cout << "organismTree: map[" << it->first->data.robotName << ", " << it->second << "];" << endl;
 
488
 
 
489
        mapType1::const_iterator it_first = nodes.begin();
 
490
        for(mapType1::const_iterator it = nodes.begin(); it != nodes.end(); ++it)               //go through the nodes, starting with the one with hightes order
 
491
        {
 
492
                if(it_first->first->isEqual(it->first))                                                                         //if the order of a node equals the highest order node
 
493
                {
 
494
                        if(it->second != 0)                                                                                                             //and its not the root node
 
495
                        {
 
496
                                rotate(iterator(it->first->sides[it->first->parent]->parentNode), iterator(it->first));                 //rotate the node as root
 
497
 
 
498
                                //save the corresponding organismString and the node
 
499
                                candidates.insert(std::pair<string, treeNode*>(generateString(), it->first) );
 
500
                                rotate(iterator(root_backup->sides[root_backup->parent]->parentNode), iterator(root_backup));   //rotate back in primal state
 
501
                        }
 
502
                        else                                                                                                                                    //if it is the root node no rotation needed
 
503
//                              candidates[generateString()] = it->first;
 
504
                                candidates.insert(std::pair<string, treeNode*>(generateString(), it->first) );
 
505
                }
 
506
                else                                                                                                                                            //the first node with lesser order quits the loop
 
507
                        break;
 
508
        }
 
509
 
 
510
        for(mapType2::const_iterator it = candidates.begin(); it != candidates.end(); ++it)             //print the strings with highest order nodes as root
 
511
                cout << "organismTree: candidate = " << it->first << endl;
 
512
 
 
513
        treeNode* best = candidates.begin()->second;                                                                    //generate the final highest order tree
 
514
        if(best->parent != -1) rotate(iterator(best->sides[best->parent]->parentNode), iterator(best));
 
515
}
 
516
 
 
517
 
 
518
organismTree::iterator::iterator()
 
519
{
 
520
        node = NULL;
 
521
}
 
522
 
 
523
organismTree::iterator::iterator(treeNode* treeNode)
 
524
{
 
525
        node = treeNode;
 
526
}
 
527
 
 
528
Robot  organismTree::iterator::operator*() const
 
529
{
 
530
        return node->data;
 
531
}
 
532
 
 
533
Robot* organismTree::iterator::operator->() const
 
534
{
 
535
        return &(node->data);
 
536
}
 
537
 
 
538
organismTree::iterator& organismTree::iterator::operator++()
 
539
{
 
540
        if(this->node->children.size() != 0)
 
541
                this->node = this->node->sides[this->node->children[0]]->childNode;
 
542
 
 
543
        else {
 
544
                while(this->node->nextSibling == NULL) {
 
545
                        this->node = this->node->sides[this->node->parent]->parentNode;
 
546
                        if(this->node == 0)
 
547
                                return *this;
 
548
                }
 
549
                this->node = this->node->nextSibling;
 
550
        }
 
551
 
 
552
        return *this;
 
553
}
 
554
 
 
555
organismTree::iterator& organismTree::iterator::operator--()
 
556
{
 
557
        if(this->node->prevSibling) {
 
558
                this->node = this->node->prevSibling;
 
559
                if(this->node->children.size()>0)
 
560
                        while(this->node->children.size()>0 && this->node->sides[this->node->children[this->node->children.size()-1]]->childNode)
 
561
                                this->node = this->node->sides[this->node->children[this->node->children.size()-1]]->childNode;
 
562
        }
 
563
        else {
 
564
                this->node = this->node->sides[this->node->parent]->parentNode;
 
565
                if(this->node==0)
 
566
                        return *this;
 
567
        }
 
568
        return *this;
 
569
}
 
570
 
 
571
organismTree::iterator& organismTree::iterator::operator+=(unsigned int num)
 
572
{
 
573
        while(num>0) {
 
574
                ++(*this);
 
575
                --num;
 
576
        }
 
577
        return (*this);
 
578
}
 
579
 
 
580
organismTree::iterator& organismTree::iterator::operator-=(unsigned int num)
 
581
{
 
582
        while(num>0) {
 
583
                --(*this);
 
584
                --num;
 
585
        }
 
586
        return (*this);
 
587
}
 
588
 
 
589
bool organismTree::iterator::operator==(const iterator& other) const
 
590
{
 
591
        if(this->node == other.node)    return true;
 
592
        else                                                    return false;
 
593
}
 
594
 
 
595
bool organismTree::iterator::operator!=(const iterator& other) const
 
596
{
 
597
        if(this->node != other.node)    return true;
 
598
        else                                                    return false;
 
599
}
 
600
 
 
601
 
 
602
 
 
603