sdls/Tree.h at master · MirkoFerrati/sdls · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68

// #include "LinearR3.h"
#include "Node.h"
#include <kdl/frames.hpp>

#ifndef _CLASS_TREE
#define _CLASS_TREE

class Tree {

public:
	Tree();

	int GetNumNode() const { return nNode; }
	int GetNumEffector() const { return nEffector; }
	int GetNumJoint() const { return nJoint; }
	void InsertRoot(Node*);
	void InsertLeftChild(Node* parent, Node* child);
	void InsertRightSibling(Node* parent, Node* child);

	// Accessors based on node numbers
	Node* GetJoint(int);
	Node* GetEffector(int);
	const KDL::Vector& GetEffectorPosition(int index);

	// Accessors for tree traversal
	Node* GetRoot() const { return root; }
	Node* GetSuccessor ( const Node* ) const;
	Node* GetParent( const Node* node ) const { return node->realparent; }

	void Compute();
	void Draw();
	void Print();
	void Init();
	void UnFreeze();

private:
	Node* root;
	int nNode;			// nNode = nEffector + nJoint
	int nEffector;
	int nJoint;
	void SetSeqNum(Node*);
	Node* SearchJoint(Node*, int);
	Node* SearchEffector(Node*, int);
	void ComputeTree(Node*);
	void DrawTree(Node*);
	void PrintTree(Node*);
	void InitTree(Node*);
	void UnFreezeTree(Node*);
};

inline Node* Tree::GetSuccessor ( const Node* node ) const
{
	if ( node->left ) {
		return node->left;
	}
	while ( true ) {
		if ( node->right ) {
			return ( node->right );
		}
		node = node->realparent;
		if ( !node ) {
			return 0;		// Back to root, finished traversal
		}
	}
}

#endif