int main()
{
Tree tree;
Tree* t = &tree;
Node node1;
node1.SetKey(5);
Node* n1 = &node1;
tree.TreeInsert(t, n1);
Node node2;
node2.SetKey(2);
Node* n2 = &node2;
tree.TreeInsert(t, n2);
Node node3;
node3.SetKey(7);
Node* n3 = &node3;
tree.TreeInsert(t, n3);
Node node4;
node4.SetKey(1);
Node* n4 = &node4;
tree.TreeInsert(t, n4);
Node node5;
node5.SetKey(3);
Node* n5 = &node5;
tree.TreeInsert(t, n5);
Node* n = tree.CommonAncestor(tree.GetRoot()->GetLeft()->GetLeft(), tree.GetRoot()->GetRight());
return 0;
}
//------------------------------------------------------------------------------
// Copy constructor
Tree::Tree (const Tree &t)
{
if (t.GetRoot() == NULL)
{
Root = NULL;
CurNode = NULL;
Leaves = 0;
Internals = 0;
Error = 0;
InternalLabels = false;
EdgeLengths = false;
Nodes = NULL;
Name = "";
Rooted = false;
Weight = 1.0;
}
else
{
CurNode = t.GetRoot();
NodePtr placeHolder;
t.copyTraverse (CurNode, placeHolder );
Root = placeHolder;
Leaves = t.GetNumLeaves ();
Internals = t.GetNumInternals ();
Name = t.GetName ();
CurNode = NULL;
Error = 0;
InternalLabels = t.GetHasInternalLabels ();;
EdgeLengths = t.GetHasEdgeLengths ();
Nodes = NULL;
Rooted = t.IsRooted();
Weight = t.GetWeight();
}
}
int main()
{
Tree tree;
Tree* t = &tree;
std::vector<int> arr;
for(int i = 0; i < 10; i++) {
arr.push_back(i);
}
CreateBST(t, arr, 0, 9); // problem 4.3
Node* successor = tree.FindSuccessor(tree.GetRoot()); // problem 4.6
DeleteBST(tree.GetRoot()); // free the tree nodes in heap
return 0;
}
TaxaParametersBis::TaxaParametersBis(string filename){
Tree* tree = new Tree(filename);
Ntaxa = tree->GetSize();
int i = 0;
RecursiveAddTaxaName(tree->GetRoot(), i);
delete tree;
}
void Jacobian::ComputeJacobian(const Tree& tree)
{
Invalidate();
jacobian_ = MatrixX(3,tree.GetNumJoint()-1); // à cause de son incrémentation débile
// Traverse this to find all end effectors
Vector3 temp;
Joint* n = tree.GetRoot();
while (n) {
if (n->IsEffector())
{
int i = n->GetEffectorNum();
// Find all ancestors (they will usually all be joints)
// Set the corresponding entries in the Jacobian J
Joint* m = tree.GetParent(n);
while (m) {
int j = m->GetJointNum();
assert (0 <=i && i<tree.GetNumEffector() && 0<=j && j<tree.GetNumJoint());
temp = m->GetS(); // joint pos.
temp -= n->GetS(); // -(end effector pos. - joint pos.)
Vector3 tmp2 = temp.cross(m->GetW()); // cross product with joint rotation axis
jacobian_.col(j-1) = tmp2;
m = tree.GetParent(m);
}
}
n = (n->IsEffector() ? 0 : tree.GetSuccessor(n));
}
}
int main()
{
int iArr[] = {1, 2, 5, 7, 10, 13, 15, 21, 22};
int low = 0, high = sizeof(iArr) / sizeof(int) - 1;
Tree<int> *ptree = new Tree<int>();
TreeNode<int> *proot = ptree->InitializeTree(iArr, low, high, ptree->GetRoot());
ptree = new Tree<int>(proot);
int iArr1[] = {13, 15, 21, 22};
//int iArr1[] = {13, 15, 221, 223};
low = 0, high = sizeof(iArr1) / sizeof(int) - 1;
Tree<int> *ptreeSub = new Tree<int>();
proot = ptree->InitializeTree(iArr1, low, high, ptreeSub->GetRoot());
ptreeSub = new Tree<int>(proot);
cout << ptree->IsSubtree(ptree->GetRoot(), ptreeSub->GetRoot()) << endl;
system("pause");
return 0;
}
Sample::Sample(Tree& tree)
{
Joint * j = tree.GetRoot();
while(j)
{
angles_.push_back(j->GetTheta());
position_ = j->GetS();
j = j->pChild_;
}
position_ -= tree.GetPosition();
jacobianProd_ = tree.GetJacobian()->GetJacobianProduct();
jacobianProdInverse_ = tree.GetJacobian()->GetJacobianProductInverse();
}
void Sample::LoadIntoTree(Tree& tree) const
{
Joint * j = tree.GetRoot();
assert(angles_.size() == tree.GetNumJoint());
{
for(Sample::LAngles::const_iterator it = angles_.begin(); it < angles_.end() && (j != 0); ++it)
{
j->SetTheta(*it);
j = j->pChild_;
}
tree.Compute();
}
}
//=================================================================================
/*virtual*/ Cornucopia::Node* PluginManager::PluginAssistant::GetTreeFromPage( wxWindow* page )
{
Cornucopia::Node* root = 0;
Panel* panel = wxDynamicCast( page, Panel );
if( panel )
{
Tree* tree = panel->GetTree();
root = tree->GetRoot();
}
return root;
}
void Tree::swap (Tree & srcTree)
{
if (srcTree.GetRoot () != 0)
{
std::auto_ptr<Node> srcRoot = srcTree.PopRoot ();
if (GetRoot () != 0)
{
std::auto_ptr<Node> myRoot = _top.PopChild ();
srcTree.SetRoot (myRoot);
}
SetRoot (srcRoot);
}
}
//测试
int main()
{
cout<<"树的双亲表示法"<<endl;
Tree<char> newtree;
int n;
cout << "输入树节点数目：";
cin >> n;
newtree.CreatK(n);
cout << "当没有对应数据输出，表示没有" << endl;
cout << "第三个节点的树的值：" << newtree.GetData(3) << endl;
cout << "第三个节点的最左子树根节点值：" << newtree.GetLeftMostChild(3, n) << endl;
cout << "第三个节点的最右子树根节点值：" << newtree.GetRightMostChild(3, n) << endl;
cout << "第三个节点的父亲节点值：" << newtree.GetParent(3) << endl;
cout << "根节点值：" << newtree.GetRoot() << endl;
getchar();
return 0;
}
int main()
{
// Balanced
int inArr[] = {4, 2, 5, 1, 6, 3};
int preArr[] = {1, 2, 4, 5, 3, 6};
// Unbalanced
//int inArr[] = {4, 3, 5, 2, 1, 6};
//int preArr[] = {1, 2, 3, 4, 5, 6};
Tree<int> *root = new Tree<int>();
TreeNode<int> *tree = new TreeNode<int>();
tree = root->BuildTreeUsingPreInorder(tree, preArr, inArr,
0, sizeof(inArr)/sizeof(int) - 1, 0, sizeof(preArr)/sizeof(int) - 1);
root = new Tree<int>(tree);
int h;
cout << root->IsBalance(root->GetRoot(), h) << endl;
system("pause");
return 0;
}
//=================================================================================
bool UndoRedo::TakeSnapShot( Panel* panel )
{
// When a snap-shot is taken, the user forfeits the ability
// to redo anything that is currently redo-able.
while( current != end )
{
SnapShot* doomed = end;
end = end->prev;
end->next = 0;
delete doomed;
count--;
}
Tree* tree = panel->GetTree();
Cornucopia::Node* root = tree->GetRoot();
Cornucopia::Node* clone = 0;
if( root )
{
Cornucopia::Node::CopyParameters copyParameters;
clone = root->Clone( wxGetApp().GetContext(), copyParameters );
}
current = new SnapShot( clone );
end->next = current;
current->prev = end;
end = current;
count++;
if( count > HISTORY_LIMIT )
{
SnapShot* doomed = beginning;
beginning = beginning->next;
beginning->prev = 0;
delete doomed;
count--;
}
return true;
}
//=================================================================================
bool UndoRedo::MoveCurrent( Panel* panel, Direction dir )
{
if( ( dir == FORWARD && !CanRedo() ) || ( dir == BACKWARD && !CanUndo() ) )
return false;
Cornucopia::Node* forgottenTree = current->root;
if( dir == FORWARD )
current = current->next;
else if( dir == BACKWARD )
current = current->prev;
Tree* tree = panel->GetTree();
Cornucopia::Node* oldRoot = tree->GetRoot();
Cornucopia::Node* newRoot = current->root;
if( newRoot )
{
Cornucopia::Node::CopyParameters copyParameters;
newRoot = newRoot->Clone( wxGetApp().GetContext(), copyParameters );
if( !newRoot )
return false;
}
panel->PreUndoRedoNotice( newRoot, forgottenTree );
tree->SetRoot(0);
tree->SetRoot( newRoot );
panel->PostUndoRedoNotice( oldRoot, forgottenTree );
if( oldRoot )
delete oldRoot;
return true;
}
NUMBER JointConstraint::Evaluate(const Robot& robot, const Tree& tree, const int joint, Jacobian& jacobianMinus, Jacobian& jacobianPlus, float epsilon, const Vector3& direction)
{
Sample * target = tree.targetSample_;
if(true )
{
if(target)
{
const Joint* j = tree.GetRoot(); int i =1;
while (j)
{
if(i == joint)
{
double angleRef = j->GetRestAngle() / 2;
while (angleRef > 360 * DegreesToRadians )
{
angleRef -= 360 * DegreesToRadians;
}
while (angleRef < -360 * DegreesToRadians )
{
angleRef += 360 * DegreesToRadians;
}
if(angleRef < 0)
{
angleRef = (360 * DegreesToRadians + angleRef);
}
double angle = j->GetAngle();
double angleRefOpposed = angleRef > 180 * DegreesToRadians ? (angleRef - 180 * DegreesToRadians ) : (angleRef + 180 * DegreesToRadians);
if(angleRefOpposed == 360 * DegreesToRadians)
{
angleRefOpposed = 0;
}
int sign = 1;
if(angleRefOpposed < angleRef && angle < angleRefOpposed)
{
sign = -1;
}
else if(angleRefOpposed > angleRef && angle < angleRef)
{
sign = -1;
}
double res = sign * abs((angleRef-angle)) / (j->GetMaxTheta() - j->GetMinTheta());
if( abs(angleRef-angle) < 5 * DegreesToRadians )
{
return 0;
}
else
{
res = res * 0.01;
res = res > 1 ? 1 : res;
res = res < -1 ? -1 : res;
return res;
}
/*std::cout << "joint " << i << " target :" << angleRef << std::endl;
std::cout << "joint " << i << " current :" << angle << std::endl;
std::cout << "joint " << i << " distance :" << ((angleRef - (angle + epsilon)) * (angleRef - (angle + epsilon)) - (angleRef - (angle - epsilon)) * (angleRef - (angle - epsilon))) /(epsilon * 2) << std::endl;
*///return ((angleRef - (angle + epsilon)) * (angleRef - (angle + epsilon)) - (angleRef - (angle - epsilon)) * (angleRef - (angle - epsilon))) /(epsilon * 2);
}
else
{
j = j->pChild_;
}
++i;
}
}
}
return 0;
}
