appli/TempAirwaysAppli/AirwaysLib/airwaysNode.cxx

   1 /*
   2  * airwaysNode.cxx
   3  *
   4  *  Created on: May 12, 2014
   5  *  Author: Diego Cáceres
   6  *
   7  *  Modified by: Alfredo Morales Pinzón
   8  */
   9
  10 #include <iostream>
  11
  12 #include "airwaysNode.h"
  13
  14 namespace airways
  15 {
  16
  17 Node::Node() : m_id(0), m_children(), m_edge(NULL), m_coords(0, 0, 0), m_level(-1), m_mark(false)
  18 {
  19 }
  20
  21 Node::Node(const Vec3& coord) : m_id(0), m_children(), m_edge(NULL), m_level(-1), m_mark(false)
  22 {
  23         this->m_coords = coord;
  24         this->m_level = 0;
  25 }
  26
  27 Node::Node(Node* node) : m_id(0), m_children(), m_mark(false)
  28 {
  29         this->m_coords = node->m_coords;
  30         this->m_level = node->m_level;
  31         this->m_edge = new Edge(node->m_edge);
  32 }
  33
  34 Node::~Node()
  35 {
  36 }
  37
  38 //Getters
  39
  40 const std::vector<Node*>& Node::GetChildren() const
  41 {
  42         return this->m_children;
  43 }
  44
  45 const unsigned int Node::GetId() const
  46 {
  47         return this->m_id;
  48 }
  49
  50 const Vec3& Node::GetCoords() const
  51 {
  52         return this->m_coords;
  53 }
  54
  55 Edge* Node::GetEdge() const
  56 {
  57         return this->m_edge;
  58 }
  59
  60 Node* Node::GetFather() const
  61 {
  62         return this->father;
  63 }
  64
  65 unsigned int Node::GetLevel() const
  66 {
  67         return this->m_level;
  68 }
  69
  70 int Node::GetDepthById(unsigned int idNode) const
  71 {
  72         if( this->m_id == idNode)
  73                 return 0;
  74
  75         vec_nodes::const_iterator it = this->m_children.begin();
  76         for( ; it != this->m_children.end(); ++it)
  77         {
  78                 if((*it)->HasNodeId(idNode))
  79                         return 1 + (*it)->GetDepthById(idNode);
  80         }
  81
  82         std::cout << "GetDepthById - The idNode: " << idNode <<  "does not exist" << std::endl;
  83
  84         return -1;
  85 }
  86
  87 unsigned int Node::GetWeight() const
  88 {
  89         if(this->IsLeaf())
  90                 return 1;
  91
  92         unsigned int weight = 0;
  93
  94         vec_nodes::const_iterator it = this->m_children.begin();
  95         for( ; it != this->m_children.end(); ++it)
  96                 weight += (*it)->GetWeight();
  97
  98         return 1 + weight;
  99 }
 100
 101 unsigned int Node::GetHeight() const
 102 {
 103         unsigned int heightChildren = 0;
 104
 105         vec_nodes::const_iterator it=this->m_children.begin();
 106         for( ; it != this->m_children.end(); ++it)
 107         {
 108                 unsigned int actualHeight = (*it)->GetHeight();
 109                 if(actualHeight > heightChildren)
 110                         heightChildren = actualHeight;
 111         }
 112
 113         // Return my heigh plus my children height
 114         return 1 + heightChildren;
 115 }
 116
 117 unsigned int Node::GetNumberOfChildren() const
 118 {
 119         return this->m_children.size();
 120 }
 121
 122 unsigned int Node::GetNumberOfNonMarkedChildren( unsigned int depth ) const
 123 {
 124         if(depth > 0)
 125         {
 126                 unsigned int nonMarked = 0;
 127
 128                 for(vec_nodes::const_iterator it = this->m_children.begin(); it != this->m_children.end(); ++it)
 129                 {
 130                         if( !(*it)->m_mark )
 131                                 ++nonMarked;
 132                         nonMarked += (*it)->GetNumberOfNonMarkedChildren( depth - 1 );
 133                 }
 134                 return nonMarked;
 135         }
 136         return 0;
 137 }
 138
 139 unsigned int Node::GetNumberOfLeafs() const
 140 {
 141         if(this->IsLeaf())
 142                 return 1;
 143
 144         unsigned int leafs = 0;
 145         vec_nodes::const_iterator it = this->m_children.begin();
 146         for( ; it != this->m_children.end(); ++it)
 147                 leafs += (*it)->GetNumberOfLeafs();
 148
 149         return leafs;
 150 }
 151
 152 Node* Node::GetNodeById(unsigned int nId)
 153 {
 154         // Base case
 155         if( m_id == nId)
 156                 return this;
 157
 158         Node* node_searched = NULL;
 159
 160         // Search in the children
 161         vec_nodes::iterator it_children = m_children.begin();
 162         for( ; it_children != m_children.end() && node_searched == NULL; ++it_children)
 163                 node_searched = (*it_children)->GetNodeById(nId);
 164
 165         return node_searched;
 166 }
 167
 168 Node* Node::GetClosestBranchNodeToPoint(float point_x, float point_y, float point_z, float &distance)
 169 {
 170         // Get the actual distance
 171         distance = GetBranchDistanceToPoint(point_x, point_y, point_z);
 172
 173         // Make as closest "this" node
 174         Node* node_closest = this;
 175
 176         // Iterate over the children
 177         vec_nodes::iterator it_children = m_children.begin();
 178         for( ; it_children != m_children.end(); ++it_children)
 179         {
 180                 // Get actual child distance
 181                 float distance_actual = 0;
 182                 Node* bestNodeChild = (*it_children)->GetClosestBranchNodeToPoint(point_x, point_y, point_z, distance_actual);
 183
 184                 // If the child distance is smaller then switch
 185                 if(distance_actual < distance)
 186                 {
 187                         node_closest = bestNodeChild;
 188                         distance = distance_actual;
 189                 }
 190         }
 191
 192         return node_closest;
 193 }
 194
 195 Node* Node::GetClosestNodeToPoint(Vec3 position, double& distance)
 196 {
 197         // Calculate actual distance to point
 198         Vec3 vector_diff = position - this->m_coords;
 199         distance = vector_diff.Norm();
 200
 201         if(this->IsLeaf())
 202                 return this;
 203
 204         // Variables
 205         double actualDistance = -1;
 206         Node* node_Best = this;
 207         Node* node_Actual = NULL;
 208
 209         // Check the children and get the best
 210         for(vec_nodes::const_iterator it = this->m_children.begin(); it != this->m_children.end(); ++it)
 211         {
 212                 node_Actual = (*it)->GetClosestNodeToPoint(position, actualDistance);
 213
 214                 if(actualDistance < distance)
 215                 {
 216                         node_Best = node_Actual;
 217                         distance = actualDistance;
 218                 }
 219         }
 220
 221         if(actualDistance == -1)
 222                 std::cout << "GetClosestNodeToPoint NOT FOUND!" << std::endl;
 223
 224         return node_Best;
 225 }
 226
 227 float Node::GetBranchDistanceToPoint(int voxel_x, int voxel_y, int voxel_z)
 228 {
 229         Vec3 voxel(voxel_x,voxel_y, voxel_z);
 230         if(this->m_edge != NULL)
 231                 return this->m_edge->GetSmallestDistanceToPoint(voxel);
 232         else
 233         {
 234                 Vec3 vector_diff = this->m_coords - voxel;
 235                 return vector_diff.Norm();
 236         }
 237 }
 238
 239 bool Node::HasMinimumLevels(int levels)
 240 {
 241         if(levels == 1)
 242                 return true;
 243
 244         bool minimum = false;
 245
 246         vec_nodes::iterator it=this->m_children.begin();
 247         for( ; it != this->m_children.end() && !minimum; ++it)
 248                 if((*it)->HasMinimumLevels(levels-1))
 249                         minimum = true;
 250
 251         return minimum;
 252 }
 253
 254 bool Node::IsLeaf() const
 255 {
 256         return this->m_children.empty();
 257 }
 258
 259 bool Node::IsMarked() const
 260 {
 261         return this->m_mark;
 262 }
 263
 264 bool Node::HasMarkedChildren() const
 265 {
 266         if(this->IsLeaf())
 267                 return false;
 268
 269         // Check the children
 270         vec_nodes::const_iterator it = this->m_children.begin();
 271         for( ; it != this->m_children.end(); ++it)
 272                 if( (*it)->IsMarked() || (*it)->HasMarkedChildren())
 273                         return true;
 274
 275         return false;
 276 }
 277
 278 bool Node::HasNode(Node* node_searched) const
 279 {
 280         if(this->m_id == node_searched->m_id)
 281                 return true;
 282
 283         bool found = false;
 284
 285         vec_nodes::const_iterator it = this->m_children.begin();
 286         for( ; it != this->m_children.end() && !found; ++it)
 287                 if( (*it)->HasNode(node_searched) )
 288                         found = true;
 289
 290         return found;
 291 }
 292
 293 bool Node::HasNodeId(unsigned int id_node_searched) const
 294 {
 295         // If this is the node then return true
 296         if(this->m_id == id_node_searched)
 297                 return true;
 298
 299         // Search in the children
 300         bool found = false;
 301         vec_nodes::const_iterator it = this->m_children.begin();
 302         for( ; it != this->m_children.end() && !found; ++it)
 303                 if( (*it)->HasNodeId(id_node_searched) )
 304                         found = true;
 305
 306         return found;
 307 }
 308
 309 bool Node::IsPointInfluencedByNode(float point_x, float point_y, float point_z) const
 310 {
 311         if(this->m_edge)
 312                 return m_edge->IsPointInfluencedByEdge(point_x, point_y, point_z);
 313
 314         return false;
 315 }
 316
 317 void Node::GetBrancheNodesInfluencingPoint(float point_x, float point_y, float point_z, vec_nodes& vec_nodes_influence)
 318 {
 319         if(this->IsPointInfluencedByNode(point_x, point_y, point_z))
 320                 vec_nodes_influence.push_back(this);
 321
 322         vec_nodes::const_iterator it = this->m_children.begin();
 323         for( ; it != this->m_children.end(); ++it)
 324                 (*it)->GetBrancheNodesInfluencingPoint(point_x, point_y, point_z, vec_nodes_influence);
 325 }
 326
 327 void Node::GetChildrenUptoDepth(unsigned int Q, vec_nodes& fathers)
 328 {
 329         if(Q > 0)
 330         {
 331                 vec_nodes::const_iterator it = this->m_children.begin();
 332                 for( ; it != this->m_children.end(); ++it)
 333                 {
 334                         fathers.push_back((*it));
 335                         (*it)->GetChildrenUptoDepth(Q-1, fathers);
 336                 }
 337                 /*// Add this node
 338                 fathers.push_back(this);
 339
 340                 // If the depth is greater that 1 then add the children of this node
 341                 if(Q > 1)
 342                 {
 343                         vec_nodes::const_iterator it = this->m_children.begin();
 344                         for( ; it != this->m_children.end(); ++it)
 345                                 (*it)->GetChildrenUptoDepth(Q-1, fathers);
 346                 }*/
 347         }
 348 }
 349
 350 void Node::CompareSubTreesOrkiszMorales(Node* nodeB, unsigned int Q, unsigned int F, vec_nodes& commonA, vec_nodes& commonB, vec_nodes& nonCommonA, vec_nodes& nonCommonB, map_id_vecnodes& map_A_to_B, map_id_vecnodes& map_B_to_A, std::vector< std::pair< pair_nodes, pair_nodes > >& vector_pair_edges)
 351 {
 352         //std::cout << "---------------------------------------------------------" << std::endl;
 353         //std::cout << "Comparing [A,B]: " << this->m_id << "," << nodeB->m_id << std::endl;
 354
 355         // Variables
 356         double distanceAtoB = 0;
 357         double distanceBtoA = 0;
 358         Node* node_bestA = NULL;
 359         Node* node_bestB = NULL;
 360
 361         // Algorithm
 362         // The algorithm is recursive it is implemented the node class. The idea is to overlap the initial nodes of the subtrees
 363         // and find the best branch correspondence for the first level of the trees.
 364         // Hypothesis: It is supposed that the roots of the trees are common, so the algorithm does not run in the root nodes
 365         // 1. Translate one of the subtree so that the initial nodes overlap.
 366         // 2. While one of the trees has non-marked child
 367         //   2.1 Compare each child to all the branches is the other tree.
 368         //   2.2 Select the pair, child and branch in the other tree, that has the lowest distance function.
 369         //   2.3 Add the pair and the distance to the final result.
 370         //   2.4 Mark the nodes
 371         //   2.5 Find and mark the omitted nodes in the similar branch. If the selected branch has more that 2 nodes it means that intermediary nodes do not have correspondence.
 372         //   2.6 Run the process (First step) using the found pair of nodes.
 373
 374         //1. Translate one of the subtree so that the initial nodes overlap.
 375         // The translation must be applied later
 376
 377         Vec3 vector_trans_B_to_A = this->m_coords - nodeB->m_coords;
 378
 379         // ------------------------
 380         // ------------------------
 381         // Making the matching faster
 382         std::multimap<double, pair_nodes> map_dist_pairNodes_A_to_B = this->CalculateDistanceToAllBranches_FatherAndFamily(nodeB, Q, F);
 383         std::multimap<double, pair_nodes> map_dist_pairNodes_B_to_A = nodeB->CalculateDistanceToAllBranches_FatherAndFamily(this, Q, F);
 384
 385         // ------------------------
 386         // ------------------------
 387
 388         while( this->GetNumberOfNonMarkedChildren( Q ) > 0 && nodeB->GetNumberOfNonMarkedChildren( Q ) > 0 )
 389         {
 390                 //   2.1 Compare each child to all the branches is the other tree.
 391
 392                 //     2.1.1 Get best translated branch from A to B
 393                 //std::pair<pair_nodes,double> pair_A_to_B = this->GetClosestBranch_FatherAndFamily(nodeB);
 394                 //     2.1.2 Get best translated branch from B to A
 395                 //std::pair<pair_nodes,double> pair_B_to_A = nodeB->GetClosestBranch_FatherAndFamily(this);
 396
 397                 // ------------------------
 398                 // ------------------------
 399                 // Making the matching faster
 400                 // Get the next best pair
 401                 std::pair<double,pair_nodes> pair_A_to_B = GetPairWithClosestDistance_FirstNodeNonMarked(map_dist_pairNodes_A_to_B);
 402                 std::pair<double,pair_nodes> pair_B_to_A = GetPairWithClosestDistance_FirstNodeNonMarked(map_dist_pairNodes_B_to_A);
 403
 404                 if( ! pair_A_to_B.second.first || ! pair_A_to_B.second.second )
 405                         std::cout << "There is no closest-non-marked branch!" << std::endl;
 406
 407                 distanceAtoB = pair_A_to_B.first;
 408                 distanceBtoA = pair_B_to_A.first;
 409
 410                 //std::map<double, pair_nodes> map_dist_pairNodes_A_to_B = this->CalculateDistanceToAllBranches_FatherAndFamily(nodeB);
 411                 //std::map<double, pair_nodes> map_dist_pairNodes_B_to_A = nodeB->CalculateDistanceToAllBranches_FatherAndFamily(this);
 412                 // ------------------------
 413                 // ------------------------
 414
 415
 416                 // Assign the distances
 417                 //distanceAtoB = pair_A_to_B.second;
 418                 //distanceBtoA = pair_B_to_A.second;
 419
 420                 //   2.2 Select the pair, child and branch in the other tree, that has the lowest distance function.
 421                 if(distanceAtoB < distanceBtoA)
 422                 {
 423                         //node_bestA = pair_A_to_B.first.first;
 424                         //node_bestB = pair_A_to_B.first.second;
 425                         node_bestA = pair_A_to_B.second.first;
 426                         node_bestB = pair_A_to_B.second.second;
 427
 428                         //std::cout << "Best Pair [A',B]: " << node_bestA->m_id << "," << node_bestB->m_id << std::endl;
 429
 430                         // 2.2.1 Check that there are no topological problems before adding the pair
 431                         // Search all the nodes in the tree A linked to the found best node in B
 432                         vec_nodes nodesA_linkedToBestB = map_B_to_A[node_bestB->m_id];
 433
 434                         // Look for topological problems
 435                         bool topo_problem = false;
 436                         for(vec_nodes::iterator it = nodesA_linkedToBestB.begin(); it != nodesA_linkedToBestB.end() && !topo_problem; ++it)
 437                         {
 438                                 if( ! ( (*it)->HasNode( node_bestA ) || node_bestA->HasNode( (*it) ) ) )
 439                                 {
 440                                         topo_problem = true;
 441                                         //std::cout << "Topological problem adding node:" << node_bestA->m_id << std::endl;
 442                                 }
 443                         }
 444
 445                         if(! topo_problem)
 446                         {
 447                                 // 2.3 Add the pair and the distance to the final result.
 448                                 commonA.push_back(node_bestA);
 449                                 commonB.push_back(node_bestB);
 450                                 map_A_to_B[node_bestA->m_id].push_back(node_bestB);
 451                                 map_B_to_A[node_bestB->m_id].push_back(node_bestA);
 452
 453                                 // Add the edge link
 454                                 pair_nodes pair_edgeA(this, node_bestA);
 455                                 pair_nodes pair_edgeB(nodeB, node_bestB);
 456                                 std::pair< pair_nodes, pair_nodes > pair_edge(pair_edgeA, pair_edgeB);
 457                                 vector_pair_edges.push_back(pair_edge);
 458
 459
 460                                 // 2.4 Mark the nodes
 461                                 node_bestA->Mark();
 462                                 node_bestB->Mark();
 463
 464                                 // 2.5 Find and mark the omitted nodes in the similar branch. If the selected branch has more that 2 nodes it means that intermediary nodes do not have correspondence.
 465                                 Node* node_nextA = this->GetNextNodeInPathToNode(node_bestA);
 466                                 //bool markedA = node_nextA->MarkNodesUntilNode(node_bestA);
 467                                 //if(!markedA)
 468                                 //      std::cout << "NodeA not found for marking [A]:" << node_bestA->m_id << std::endl;
 469
 470                                 Node* node_nextB = nodeB->GetNextNodeInPathToNode(node_bestB);
 471                                 //bool markedB = node_nextB->MarkNodesUntilNode(node_bestB);
 472                                 //if(!markedB)
 473                                 //      std::cout << "NodeB not found for marking [B]:" << node_bestB->m_id << std::endl;
 474
 475                                 // 2.6 Run the process (First step) using the found pair of nodes.
 476                                 node_bestA->CompareSubTreesOrkiszMorales(node_bestB, Q, F, commonA, commonB, nonCommonA, nonCommonB, map_A_to_B, map_B_to_A, vector_pair_edges);
 477                         }
 478                         else
 479                         {
 480                                 // 2.4 Mark the wrong node
 481                                 node_bestA->Mark();
 482                                 nonCommonA.push_back(node_bestA);
 483                         }
 484                 }
 485                 else
 486                 {
 487                         //node_bestA = pair_B_to_A.first.second;
 488                         //node_bestB = pair_B_to_A.first.first;
 489                         node_bestA = pair_B_to_A.second.second;
 490                         node_bestB = pair_B_to_A.second.first;
 491
 492                         //std::cout << "Best Pair [A,B']: " << node_bestA->m_id << "," << node_bestB->m_id << std::endl;
 493
 494                         // 2.29 Check that there are no topological problems before adding the pair
 495                         // 2.291 Search all the nodes in the A tree linked to the found best node in B
 496                         vec_nodes nodesB_linkedToBestA = map_A_to_B[node_bestA->m_id];
 497
 498                         // 2.292 Look for topological problems
 499                         bool topo_problem = false;
 500                         for(vec_nodes::iterator it = nodesB_linkedToBestA.begin(); it != nodesB_linkedToBestA.end() && !topo_problem; ++it)
 501                         {
 502                                 if( ! ( (*it)->HasNode(node_bestB) || node_bestB->HasNode( (*it) ) ) )
 503                                 {
 504                                         topo_problem = true;
 505                                         //std::cout << "Topological problem adding node:" << node_bestB->m_id << std::endl;
 506                                 }
 507                         }
 508
 509                         if(! topo_problem)
 510                         {
 511                                 // 2.3 Add the pair and the distance to the final result.
 512                                 commonA.push_back(node_bestA);
 513                                 commonB.push_back(node_bestB);
 514                                 map_A_to_B[node_bestA->m_id].push_back(node_bestB);
 515                                 map_B_to_A[node_bestB->m_id].push_back(node_bestA);
 516
 517                                 // Add the edge link
 518                                 pair_nodes pair_edgeA(this, node_bestA);
 519                                 pair_nodes pair_edgeB(nodeB, node_bestB);
 520                                 std::pair< pair_nodes, pair_nodes > pair_edge(pair_edgeA, pair_edgeB);
 521                                 vector_pair_edges.push_back(pair_edge);
 522
 523                                 // 2.4 Mark the nodes
 524                                 node_bestA->Mark();
 525                                 node_bestB->Mark();
 526
 527                                 // 2.5 Find and mark the omitted nodes in the similar branch. If the selected branch has more that 2 nodes it means that intermediary nodes do not have correspondence.
 528                                 Node* node_nextA = this->GetNextNodeInPathToNode(node_bestA);
 529                                 //bool markedA = node_nextA->MarkNodesUntilNode(node_bestA);
 530                                 //if(!markedA)
 531                                 //      std::cout << "NodeA not found for marking [A]:" << node_bestA->m_id << std::endl;
 532
 533                                 Node* node_nextB = nodeB->GetNextNodeInPathToNode(node_bestB);
 534                                 //bool markedB = node_nextB->MarkNodesUntilNode(node_bestB);
 535                                 //if(!markedB)
 536                                 //      std::cout << "NodeB not found for marking [B]:" << node_bestB->m_id << std::endl;
 537
 538                                 // 2.6 Run the process (First step) using the found pair of nodes.
 539                                 //node_bestB->CompareSubTreesOrkiszMorales(node_bestA, commonB, commonA, nonCommonB, nonCommonA, map_B_to_A, map_A_to_B, vector_pair_edges);
 540                                 node_bestA->CompareSubTreesOrkiszMorales(node_bestB, Q, F, commonA, commonB, nonCommonA, nonCommonB, map_A_to_B, map_B_to_A, vector_pair_edges);
 541                         }
 542                         else
 543                         {
 544                                 // 2.4 Mark the wrong node
 545                                 node_bestB->Mark();
 546                                 nonCommonB.push_back(node_bestB);
 547                         }
 548                 }
 549
 550         }
 551         //std::cout << "OUT Comparing [A,B]: " << this->m_id << "," << nodeB->m_id << std::endl;
 552         //std::cout << "---------------------------------------------------------" << std::endl;
 553 }
 554
 555 std::pair< std::pair<Node*, Node*>, double> Node::GetBestBranches_ActualTranslatedOneLevelBranches_To_AllPossibleBranches(Node* nodeB)
 556 {
 557         // Variables
 558         double distance = -1;
 559         double distance_bestFinal = -1;
 560         Node* node_A_bestFinal = NULL;
 561         Node* node_B_bestFinal = NULL;
 562         bool first = true;
 563
 564         // Get the translation vector for root ovelapping
 565         Vec3 vector_trans_A_to_B = nodeB->m_coords - this->m_coords;
 566
 567         // Iterate over my children
 568         for(vec_nodes::iterator it_A = this->m_children.begin(); it_A != this->m_children.end(); ++it_A)
 569         {
 570                 if( !(*it_A)->IsMarked() )
 571                 {
 572                         // Get the position of the actual child of this node
 573                         Vec3 position_actual = (*it_A)->m_coords;
 574
 575                         // Translate the position
 576                         Vec3 position_translated = position_actual + vector_trans_A_to_B;
 577
 578                         // Find the "closest" node in the subtree nodeB using a distance function
 579                         // a. Distance to ending point
 580                         //Node* bestNode_actual = nodeB->GetClosestRelativeToPoint(position_translated, distance);
 581                         // b. Distance point to point
 582                         Node* bestNode_actual = nodeB->GetClosestRelativeToBranch( (*it_A)->m_edge, vector_trans_A_to_B, distance);
 583
 584                         if(first)
 585                         {
 586                                 first = false;
 587                                 node_A_bestFinal = (*it_A);
 588                                 node_B_bestFinal = bestNode_actual;
 589                                 distance_bestFinal = distance;
 590                         }
 591                         else if(distance < distance_bestFinal)
 592                         {
 593                                 node_A_bestFinal = (*it_A);
 594                                 node_B_bestFinal = bestNode_actual;
 595                                 distance_bestFinal = distance;
 596                         }
 597                 }
 598         }
 599
 600         if(first)
 601                 std::cout << "GetBestTranslatedBranchFromOtherTree NOT FOUND!" << std::endl;
 602
 603         pair_nodes pair_best(node_A_bestFinal, node_B_bestFinal);
 604         std::pair<pair_nodes,double> best_pair_score(pair_best, distance_bestFinal);
 605         return best_pair_score;
 606 }
 607
 608 std::multimap< double, std::pair<Node*, Node*> > Node::CalculateDistanceToAllBranches_FatherAndFamily(Node* node_gradfather, unsigned int Q, unsigned int F)
 609 {
 610         // "this" is a grandfather node
 611         // Variables
 612         std::multimap< double, pair_nodes > map_dist_pairNodeNode;
 613         double distance_actual = -1;
 614
 615         // Get the translation vector for root overlapping
 616         Vec3 vector_trans_A_to_B_grandfathers = node_gradfather->m_coords - this->m_coords;
 617
 618         // Get all the possible "fathers" based on Q
 619         vec_nodes fathers;
 620         this->GetChildrenUptoDepth(Q, fathers);
 621         //fathers = this->m_children;
 622
 623         // Iterate over my children which actually are fathers
 624
 625         //vec_nodes::iterator it_father = this->m_children.begin();
 626         vec_nodes::iterator it_father = fathers.begin();
 627         //for( ; it_father != this->m_children.end(); ++it_father)
 628         for( ; it_father != fathers.end(); ++it_father)
 629         {
 630                 if( ! (*it_father)->IsMarked() )
 631                 {
 632
 633                         Node* node_actualBest = node_gradfather->GetClosestRelativeFatherAndFamily( this, (*it_father), F, vector_trans_A_to_B_grandfathers, distance_actual);
 634
 635                         pair_nodes pair_actual((*it_father), node_actualBest);
 636
 637                         std::pair<double,pair_nodes> actual_dist_pair(distance_actual,pair_actual);
 638                         map_dist_pairNodeNode.insert(actual_dist_pair);
 639                 }
 640         }
 641
 642         return map_dist_pairNodeNode;
 643 }
 644
 645
 646 /*std::pair< std::pair<Node*, Node*>, double> Node::GetClosestBranch_FatherAndFamily(Node* node_gradfather)
 647 {
 648         // "this" is a grandfather node
 649         // Variables
 650         double distance_actual = -1;
 651         double distance_bestFinal = -1;
 652         Node* node_A_bestFinal = NULL;
 653         Node* node_B_bestFinal = NULL;
 654         bool first = true;
 655
 656         // Get the translation vector for root overlapping
 657         Vec3 vector_trans_A_to_B_grandfathers = node_gradfather->m_coords - this->m_coords;
 658
 659         // Iterate over my children which actually are fathers
 660         vec_nodes::iterator it_father = this->m_children.begin();
 661         for( ; it_father != this->m_children.end(); ++it_father)
 662         {
 663                 if( !(*it_father)->IsMarked() )
 664                 {
 665                         // Get the position of the actual child of this node
 666                         //Vec3 position_actualFather = (*it_father)->m_coords;
 667
 668                         // Find the "closest" node in the subtree nodeB using a distance function
 669                         // a. Distance to ending point
 670                         // Translate the position
 671                         //Vec3 position_translated = position_actualFather + vector_trans_A_to_B_grandfathers;
 672                         //Node* bestNode_actual = nodeB->GetClosestRelativeToPoint(position_translated, distance);
 673                         // b. Distance point to point
 674                         Node* node_actualBest = node_gradfather->GetClosestRelativeFatherAndFamily( (*it_father), vector_trans_A_to_B_grandfathers, distance_actual);
 675
 676                         if(first)
 677                         {
 678                                 first = false;
 679                                 node_A_bestFinal = (*it_father);
 680                                 node_B_bestFinal = node_actualBest;
 681                                 distance_bestFinal = distance_actual;
 682                         }
 683                         else if(distance_actual < distance_bestFinal)
 684                         {
 685                                 node_A_bestFinal = (*it_father);
 686                                 node_B_bestFinal = node_actualBest;
 687                                 distance_bestFinal = distance_actual;
 688                         }
 689                 }
 690         }
 691
 692         if(first)
 693                 std::cout << "GetBestTranslatedBranchFromOtherTree NOT FOUND!" << std::endl;
 694
 695         pair_nodes pair_best(node_A_bestFinal, node_B_bestFinal);
 696         std::pair<pair_nodes,double> best_pair_score(pair_best, distance_bestFinal);
 697         return best_pair_score;
 698 }
 699 */
 700
 701 Node* Node::GetClosestRelativeToPoint(Vec3 position, double& distance)
 702 {
 703         if(this->IsLeaf())
 704         {
 705                 distance = -1;
 706                 return NULL;
 707         }
 708
 709         distance = -1;
 710         bool first = true;
 711         double actualDistance = -1;
 712         Node* node_Best = NULL;
 713         Node* node_Actual = NULL;
 714
 715         for(vec_nodes::const_iterator it = this->m_children.begin(); it != this->m_children.end(); ++it)
 716         {
 717                 node_Actual = (*it)->GetClosestNodeToPoint(position, actualDistance);
 718
 719                 if(first)
 720                 {
 721                         first = false;
 722                         node_Best = node_Actual;
 723                         distance = actualDistance;
 724                 }
 725                 else if(actualDistance < distance)
 726                 {
 727                         node_Best = node_Actual;
 728                         distance = actualDistance;
 729                 }
 730         }
 731
 732         if(first)
 733                 std::cout << "GetClosestRelativeToPoint NOT FOUND!" << std::endl;
 734
 735         if(!node_Best)
 736                 std::cout << "GetClosestRelativeToPoint NULL!" << std::endl;
 737
 738         return node_Best;
 739 }
 740
 741 Node* Node::GetClosestRelativeToBranch(Edge* branch, Vec3 vector_translation, double& distance)
 742 {
 743         if(this->IsLeaf())
 744         {
 745                 distance = -1;
 746                 //std::cout << "GetClosestRelativeToBranch -- This node is a leaf" << std::endl;
 747                 return NULL;
 748         }
 749
 750         distance = -1;
 751         bool first = true;
 752         double actualDistance = -1;
 753         Node* node_Best = NULL;
 754         Node* node_Actual = NULL;
 755
 756         for(vec_nodes::const_iterator it = this->m_children.begin(); it != this->m_children.end(); ++it)
 757         {
 758                 node_Actual = (*it)->GetClosestBranchToTranslatedBranch(branch,vector_translation, actualDistance, NULL);
 759
 760                 if(first)
 761                 {
 762                         first = false;
 763                         node_Best = node_Actual;
 764                         distance = actualDistance;
 765                 }
 766                 else if(actualDistance < distance)
 767                 {
 768                         node_Best = node_Actual;
 769                         distance = actualDistance;
 770                 }
 771         }
 772
 773         if(first)
 774                 std::cout << "GetClosestRelativeToPoint NOT FOUND!" << std::endl;
 775
 776         if(!node_Best)
 777                 std::cout << "GetClosestRelativeToPoint NULL!" << std::endl;
 778
 779         return node_Best;
 780 }
 781
 782 Node* Node::GetClosestRelativeFatherAndFamily(Node* node_grandfather, Node* node_father, unsigned int F, Vec3 vector_translation, double& distance)
 783 {
 784         // "this" is a grandfather node
 785
 786         distance = -1;
 787         if(this->IsLeaf())
 788                 return NULL;
 789
 790         bool first = true;
 791         double actualDistance = -1;
 792         Node* node_Best = NULL;
 793         Node* node_Actual = NULL;
 794
 795         // Iterate over the children of this granfather which means
 796         // iterate over the fathers
 797         vec_nodes::const_iterator it_father = this->m_children.begin();
 798
 799         for( ; it_father != this->m_children.end(); ++it_father)
 800         {
 801                 node_Actual = (*it_father)->GetClosestFatherAndFamilyToTranslatedFatherAndFamily(node_grandfather, node_father, F, vector_translation, actualDistance, NULL);
 802
 803                 if(first)
 804                 {
 805                         first = false;
 806                         node_Best = node_Actual;
 807                         distance = actualDistance;
 808                 }
 809                 else if(actualDistance < distance)
 810                 {
 811                         node_Best = node_Actual;
 812                         distance = actualDistance;
 813                 }
 814         }
 815
 816         if(first)
 817                 std::cout << "GetClosestRelativeToPoint NOT FOUND!" << std::endl;
 818
 819         if(!node_Best)
 820                 std::cout << "GetClosestRelativeToPoint NULL!" << std::endl;
 821
 822         return node_Best;
 823 }
 824
 825 Node* Node::GetClosestBranchToTranslatedBranch(Edge* branch, Vec3 vector_translation, double& distance, Edge* edgeSuperior)
 826 {
 827         Node* node_best = this;
 828         Node* node_Actual = NULL;
 829         Edge* edge_composed = NULL;
 830
 831         // Joint the superior edge with this edge
 832         edge_composed = this->ConcatenateEdgeToSuperiorEdge(edgeSuperior);
 833
 834         distance = edge_composed->GetDistanceToTranslatedEdge(branch, vector_translation);
 835         distance += branch->GetDistanceToTranslatedEdge(edge_composed, -vector_translation);
 836         //distance = edge_composed->GetDistanceWeigthedToTranslatedEdge(branch, vector_translation);
 837         //distance += branch->GetDistanceWeigthedToTranslatedEdge(edge_composed, -vector_translation);
 838         //std::cout << "DW[thisId,toBranchTargetId,Dist]:[" << this->m_id << "," << branch->GetTarget()->GetId() << "," << distance << "]";
 839
 840         // Lance the recursion and the the minumum distance and node
 841         double distance_actual = -1;
 842
 843         // Check the children and get the best
 844         for(vec_nodes::iterator it = this->m_children.begin(); it != this->m_children.end(); ++it)
 845         {
 846                 Edge* edge_forChild = new Edge(edge_composed);
 847                 node_Actual = (*it)->GetClosestBranchToTranslatedBranch(branch, vector_translation, distance_actual, edge_forChild);
 848
 849                 if(distance_actual < distance)
 850                 {
 851                         node_best = node_Actual;
 852                         distance = distance_actual;
 853                 }
 854                 delete(edge_forChild);
 855         }
 856
 857         return node_best;
 858 }
 859
 860 Node* Node::GetClosestFatherAndFamilyToTranslatedFatherAndFamily(Node* node_grandfather, Node* node_father, unsigned int F, Vec3 vector_translation, double& distance, Edge* edgeSuperior)
 861 {
 862         // "this" is a father node_father
 863         Node* node_best = this;         // Best matching node is "this" in the beginnig.
 864         Node* node_Actual = NULL;
 865         Edge* edge_composed = NULL;
 866
 867         // Joint the superior edge with this edge
 868         edge_composed = this->ConcatenateEdgeToSuperiorEdge(edgeSuperior);
 869
 870         //distance = edge_composed->GetDistanceToTranslatedEdge(branch, vector_translation);
 871         //distance += branch->GetDistanceToTranslatedEdge(edge_composed, -vector_translation);
 872
 873         //Edge* branch_father = node_father->m_edge;
 874         Edge* branch_father = node_father->GetEdgeToAncestor(node_grandfather, NULL);
 875         if( branch_father == NULL )
 876                 std::cout << "No branch father" << std::endl;
 877
 878
 879         distance = edge_composed->GetDistanceWeigthedToTranslatedEdge(branch_father, vector_translation);
 880         distance += branch_father->GetDistanceWeigthedToTranslatedEdge(edge_composed, -vector_translation);
 881
 882         // Find the family distance from the node_father family to this family
 883         double distance_family_A_to_B = 0;
 884         // Add the distances to the family, up to one generation by the moment
 885         Vec3 vector_translation_child_A_to_B = this->m_coords - node_father->m_coords; // Vector that align the fathers
 886
 887         // Get the family up to depth F
 888         vec_nodes family;
 889         node_father->GetChildrenUptoDepth(F, family);
 890         //vec_nodes family = node_father->m_children;
 891
 892         vec_nodes::iterator it_child_other = family.begin();
 893         for(; it_child_other != family.end(); ++it_child_other)
 894         {
 895                 // Create the "child" edge
 896                 Edge* childEdge = (*it_child_other)->GetEdgeToAncestor(node_father, NULL);
 897                 //Edge* childEdge = (*it_child_other)->m_edge;
 898
 899                 double distance_child = 0;
 900                 if(this->IsLeaf())
 901                 {
 902                         // The distance of the each point edge to the root point because the fathers are aligned
 903                         distance_child = childEdge->GetDistanceToPoint(this->m_coords);
 904                 }
 905                 else
 906                 {
 907                         this->GetClosestRelativeToBranch(childEdge,vector_translation_child_A_to_B, distance_child);
 908                 }
 909                 distance_family_A_to_B += distance_child;
 910         }
 911
 912         distance += distance_family_A_to_B;
 913
 914         // Find the family distance from this family to node_father family
 915         double distance_family_B_to_A = 0;
 916
 917         // Add the distances to the family, up to one generation by the moment
 918         Vec3 vector_translation_child_B_to_A = node_father->m_coords - this->m_coords; // Vector that align the fathers
 919
 920         // Get the family up to depth F
 921         vec_nodes my_family;
 922         this->GetChildrenUptoDepth(F, my_family);
 923         //vec_nodes my_family = this->m_children;
 924
 925         vec_nodes::iterator it_child_my = my_family.begin();
 926         for(; it_child_my != my_family.end(); ++it_child_my)
 927         {
 928                 // Create the "child" edge
 929                 Edge* my_childEdge = (*it_child_my)->GetEdgeToAncestor(this, NULL);
 930                 //Edge* my_childEdge = (*it_child_my)->m_edge;
 931
 932                 double distance_child = 0;
 933                 if(node_father->IsLeaf())
 934                 {
 935                         // The distance of the each point edge to the root point because the fathers are aligned
 936                         distance_child = my_childEdge->GetDistanceToPoint(node_father->m_coords);
 937                 }
 938                 else
 939                 {
 940                         node_father->GetClosestRelativeToBranch(my_childEdge, vector_translation_child_B_to_A, distance_child);
 941                 }
 942                 distance_family_B_to_A += distance_child;
 943         }
 944
 945         distance += distance_family_B_to_A;
 946
 947         //std::cout << "DW[thisId,toBranchTargetId,Dist]:[" << this->m_id << "," << branch->GetTarget()->GetId() << "," << distance << "]";
 948
 949         // Lance the recursion and the minimum distance and node_father
 950         double distance_actual = -1;
 951
 952         // Check the children and get the best
 953         vec_nodes::iterator it = this->m_children.begin();
 954         for( ; it != this->m_children.end(); ++it)
 955         {
 956                 Edge* edge_forChild = new Edge(edge_composed);
 957                 node_Actual = (*it)->GetClosestFatherAndFamilyToTranslatedFatherAndFamily(node_grandfather, node_father, F, vector_translation, distance_actual, edge_forChild);
 958
 959                 if(distance_actual < distance)
 960                 {
 961                         node_best = node_Actual;
 962                         distance = distance_actual;
 963                 }
 964                 delete(edge_forChild);
 965         }
 966
 967         return node_best;
 968 }
 969
 970 const Node* Node::GetClosestCommonAscendingNode() const
 971 {
 972         Node* node_closestCommonAscendingNode = NULL;
 973
 974         if(this->m_edge)
 975         {
 976                 if(this->m_edge->GetSource()->IsMarked() || this->m_edge->GetSource()->HasMarkedChildren())
 977                         node_closestCommonAscendingNode =  this->m_edge->GetSource();
 978                 else
 979                         return this->m_edge->GetSource()->GetClosestCommonAscendingNode();
 980         }
 981
 982         return node_closestCommonAscendingNode;
 983 }
 984
 985 bool Node::MarkNodesUntilNode(Node* nodeB)
 986 {
 987         this->Mark();
 988
 989         if(this->m_id == nodeB->m_id)
 990                 return true;
 991
 992         bool found = false;
 993         bool actualFound = false;
 994         for(vec_nodes::const_iterator it = this->m_children.begin(); it != this->m_children.end(); ++it)
 995         {
 996                 actualFound = (*it)->MarkNodesUntilNode(nodeB);
 997
 998                 if(actualFound)
 999                         found = true;
1000         }
1001
1002         return found;
1003 }
1004
1005 Node* Node::GetNextNodeInPathToNode(Node* node_searched)
1006 {
1007         Node* nextNode = NULL;
1008         for(vec_nodes::const_iterator it = this->m_children.begin(); it != this->m_children.end() && !nextNode; ++it)
1009         {
1010                 if( (*it)->HasNode(node_searched) )
1011                         nextNode = (*it);
1012         }
1013
1014         return nextNode;
1015 }
1016
1017 void Node::GetPathToNode(Node* node_end, vec_nodes& vector_pathNodes)
1018 {
1019         if(!this->HasNode(node_end))
1020                 return;
1021
1022         vector_pathNodes.push_back(this);
1023         for(vec_nodes::const_iterator it = this->m_children.begin(); it != this->m_children.end(); ++it)
1024                 (*it)->GetPathToNode(node_end, vector_pathNodes);
1025 }
1026
1027 void Node::MarkPathFromNodeToNode(Node* node_begin, Node* node_end)
1028 {
1029         if(this->m_id == node_begin->m_id)
1030         {
1031                 if(this->HasNode(node_end))
1032                         this->MarkPathNodesToNode(node_end);
1033         }
1034         else
1035                 for(vec_nodes::const_iterator it = this->m_children.begin(); it != this->m_children.end(); ++it)
1036                         (*it)->MarkPathFromNodeToNode(node_begin, node_end);
1037 }
1038
1039 void Node::MarkPathNodesToNode(Node* node_end)
1040 {
1041         if(this->HasNode(node_end))
1042                 this->Mark();
1043
1044         for(vec_nodes::const_iterator it = this->m_children.begin(); it != this->m_children.end(); ++it)
1045                 (*it)->MarkPathNodesToNode(node_end);
1046 }
1047
1048 Node* Node::GetDetachedRootCopy()
1049 {
1050         Node* node_copy = new Node(this);
1051
1052         // Change properties for root node
1053         Node* root_detached = new Node();
1054         root_detached->m_coords = node_copy->m_edge->GetSource()->GetCoords();
1055         root_detached->m_edge = NULL;
1056         vec_nodes vector_new;
1057         vector_new.push_back(node_copy);
1058         root_detached->m_children = vector_new;
1059
1060         // Change properties for the copy node
1061         node_copy->m_edge->SetSource(root_detached);
1062
1063         return root_detached;
1064 }
1065
1066 bool Node::CompareNodes(Node* nodeTreeB)
1067 {
1068         if (this == NULL && nodeTreeB == NULL)
1069                 return true;
1070         else if (this == NULL || nodeTreeB == NULL)
1071                 return false;
1072         vec_nodes::iterator it = this->m_children.begin();
1073         for (; it != this->m_children.end(); ++it)
1074         {
1075                 if ((*it)->m_mark)
1076                         continue;
1077                 bool suc = false;
1078                 vec_nodes::iterator it2 = nodeTreeB->m_children.begin();
1079                 for (; it2 != nodeTreeB->m_children.end(); ++it2)
1080                         suc = (*it)->CompareNodes(*it2);
1081                 if (!suc)
1082                         return false;
1083         } //for
1084         if (*this != *nodeTreeB)
1085                 return false;
1086         this->m_mark = true;
1087         nodeTreeB->m_mark = true;
1088         return true;
1089 }
1090
1091 Edge* Node::GetComposedEdge(unsigned int idNode)
1092 {
1093         // If this is the node, then a copy edge (to the parent) is returned
1094         if(this->m_id == idNode)
1095         {
1096                 //std::cout << "This is the searched idNode: " << this->m_id << std::endl;
1097                 Edge* nEdge = new Edge(this->m_edge);
1098                 return nEdge;
1099         }
1100
1101
1102         Edge* composedEdge = NULL;
1103
1104         for(std::vector<Node*>::iterator it = this->m_children.begin(); it != this->m_children.end(); ++it)
1105         {
1106                 // If one of my children has the composed node, then my edge must be added
1107                 // and the composed edge is returned
1108                 Edge* childEdge = (*it)->GetComposedEdge(idNode);
1109                 if(childEdge)
1110                 {
1111                         //std::cout << "My id: " << this->m_id << ", Looking for: " << idNode << std::endl;
1112                         return AddChildEdgeInformation(childEdge);
1113                 }
1114         }
1115
1116         return composedEdge;
1117 }
1118
1119 Edge* Node::AddChildEdgeInformation(Edge* nEdge)
1120 {
1121         // If this is the root then the edge to be compose is null, so the same edge must be returned
1122         if(this->m_edge == NULL)
1123         {
1124                 //std::cout << "This is the root node" << std::endl;
1125                 return nEdge;
1126         }
1127
1128         // New information vector
1129         vec_pair_posVox_rad vector_newInformation;
1130         vec_pair_posVox_rad actualEdgeInformation = this->m_edge->GetEdgeInfo();
1131         vec_pair_posVox_rad composedInformation = nEdge->GetEdgeInfo();
1132
1133         // Check that last and initial information, from this and the other edge respectively, are equal.
1134         if(actualEdgeInformation[actualEdgeInformation.size()-1].first != composedInformation[0].first)
1135         {
1136
1137                 std::cout << "actualEdgeInformation[0;end]: [" << actualEdgeInformation[0].first << ";" << actualEdgeInformation[actualEdgeInformation.size()-1].first << "], Points: " << actualEdgeInformation.size() << std::endl;
1138                 std::cout << "ActualEdge[source;target]: [" << this->m_edge->GetSource()->GetCoords() << "];[" << this->m_edge->GetTarget()->GetCoords() << "]" << std::endl;
1139                 std::cout << "-- -- -- " << std::endl;
1140                 std::cout << "composedInformation[0;end]: [" << composedInformation[0].first << ";" << composedInformation[composedInformation.size()-1].first << "], Points: " << composedInformation.size() << std::endl;
1141                 std::cout << "AddChildEdge[source;target]: [" << nEdge->GetSource()->GetCoords() << "];[" << nEdge->GetTarget()->GetCoords() << "]" << std::endl;
1142                 std::cout << "End and initial information are not equal! : " << actualEdgeInformation[actualEdgeInformation.size()-1].first << " , " << composedInformation[0].first << std::endl;
1143
1144
1145                 std::cout << "Print composed edge information" << std::endl;
1146                 std::cout << "Composed [source, target] " << nEdge->GetSource()->GetCoords() << "];[" << nEdge->GetTarget()->GetCoords() << "]" << std::endl;
1147                 std::cout << "Composed information using iterator:" << std::endl;
1148
1149                 for(vec_pair_posVox_rad::const_iterator it_composed = nEdge->GetEdgeInfo().begin(); it_composed != nEdge->GetEdgeInfo().end(); ++it_composed)
1150                         std::cout << "[" << (*it_composed).first << "], ";
1151         }
1152
1153         // Change the source
1154         nEdge->SetSource(this->m_edge->GetSource());
1155
1156         // Change properties
1157         nEdge->SetLength(nEdge->GetLength()+this->m_edge->GetLength());
1158         nEdge->SetEDistance(nEdge->GetEDistance()+this->m_edge->GetEDistance());
1159
1160         // Create the new information vector and calculate the new radii attributes
1161         for(vec_pair_posVox_rad::iterator it_actual = actualEdgeInformation.begin(); it_actual != actualEdgeInformation.end(); ++it_actual)
1162                 vector_newInformation.push_back((*it_actual));
1163
1164         bool connectionVoxel = true;
1165         for(vec_pair_posVox_rad::iterator it_composed = composedInformation.begin(); it_composed != composedInformation.end(); ++it_composed)
1166         {
1167                 if(connectionVoxel)
1168                         connectionVoxel = false;
1169                 else
1170                         vector_newInformation.push_back((*it_composed));
1171         }
1172
1173         // Set the new information
1174         nEdge->SetSkeletonPairVector(vector_newInformation);
1175
1176         return nEdge;
1177 }
1178
1179 Edge* Node::GetEdgeToAncestor(Node* node_ancestor, Edge* edge)
1180 {
1181         Edge* edge_composed = NULL;
1182
1183         // Compose the edge
1184         if(edge == NULL)
1185                 edge_composed = new Edge(this->m_edge);
1186         else
1187                 edge_composed = edge->ConcatenateToSuperior(new Edge(this->m_edge));
1188
1189         // Check if the father is the ancestor
1190         if(this->m_edge->GetSource()->GetId() == node_ancestor->GetId())
1191                 return edge_composed;
1192
1193         return this->m_edge->GetSource()->GetEdgeToAncestor(node_ancestor, edge_composed);
1194 }
1195
1196 Edge* Node::ConcatenateEdgeToSuperiorEdge(Edge* superiorEdge)
1197 {
1198         if(superiorEdge == NULL)
1199                 return new Edge(this->m_edge);
1200
1201         // Check concatenation condition in the nodes
1202         if(superiorEdge->GetTarget()->GetCoords() != this->m_edge->GetSource()->GetCoords())
1203         {
1204                 std::cout << "Node::ConcatenateEdgeToSuperiorEdge - superiorEdge.target != actualEdge.source. Actual idNode:" << this->m_id << std::endl;
1205                 std::cout << "[superiorEdge.target;actualEdge.source]: [" << superiorEdge->GetTarget()->GetCoords() << ";" << this->m_edge->GetSource()->GetCoords() << "]" << std::endl;
1206                 return NULL;
1207         }
1208
1209         // Check concatenation condition in the edge information
1210         vec_pair_posVox_rad vectorInfo_thisEdge = this->m_edge->GetEdgeInfo(); // Actual edge information
1211         vec_pair_posVox_rad vectorInfo_superiorEdge = superiorEdge->GetEdgeInfo(); // Superior edge information
1212
1213         // Check that last superior edge position is equal to first initial actual edge position
1214         if(vectorInfo_superiorEdge[vectorInfo_superiorEdge.size()-1].first != vectorInfo_thisEdge[0].first)
1215         {
1216                 std::cout << "Node::ConcatenateEdgeToSuperiorEdge - superiorEdge.info[end] != actualEdge.info[begin]. Actual idNode:" << this->m_id << std::endl;
1217                 return NULL;
1218         }
1219
1220         // Change the superior edge target, the superior length, and the euclidian distance
1221         superiorEdge->SetTarget(this->m_edge->GetTarget());
1222         superiorEdge->SetLength(superiorEdge->GetLength()+this->m_edge->GetLength()-1);
1223         superiorEdge->SetEDistance(superiorEdge->GetEDistance()+this->m_edge->GetEDistance());
1224
1225         // Add the position information
1226         vec_pair_posVox_rad::iterator it_actualInfo = vectorInfo_thisEdge.begin();
1227         ++it_actualInfo; // Skip the first position because is it shared between both edges
1228         for(; it_actualInfo != vectorInfo_thisEdge.end(); ++it_actualInfo)
1229                 vectorInfo_superiorEdge.push_back((*it_actualInfo));
1230
1231         // Set the new information
1232         superiorEdge->SetSkeletonPairVector(vectorInfo_superiorEdge);
1233
1234         return superiorEdge;
1235 }
1236
1237 bool Node::FindSimilarEdgeByAccumulation(Node* nodeB, Node* nodeBAcc)
1238 {
1239         bool firstTime = false;
1240         if (nodeBAcc == NULL)
1241         {
1242                 nodeBAcc = new Node();
1243                 nodeBAcc->m_edge = new Edge();
1244                 firstTime = true;
1245         } //if
1246         nodeBAcc->m_coords = nodeB->m_coords;
1247         nodeBAcc->m_level = this->m_level;
1248         Edge* edgeBAcc = nodeBAcc->m_edge;
1249         Edge* edgeB = nodeB->m_edge;
1250         edgeBAcc->m_angle = edgeB->m_angle;
1251         edgeBAcc->m_eDistance = edgeB->m_eDistance;
1252         if (!firstTime)
1253         {
1254                 edgeBAcc->m_length += edgeB->m_length;
1255                 //edgeBAcc->m_aRadius = (edgeB->m_aRadius + edgeBAcc->m_aRadius) / 2.0;
1256                 edgeBAcc->m_maxRadius = edgeB->m_maxRadius > edgeBAcc->m_maxRadius ? edgeB->m_maxRadius : edgeBAcc->m_maxRadius;
1257                 edgeBAcc->m_minRadius = edgeB->m_minRadius < edgeBAcc->m_minRadius ? edgeB->m_minRadius : edgeBAcc->m_minRadius;
1258         } //fi
1259         else
1260         {
1261                 edgeBAcc->m_length = edgeB->m_length;
1262                 edgeBAcc->m_aRadius = edgeB->m_aRadius;
1263                 edgeBAcc->m_maxRadius = edgeB->m_maxRadius;
1264                 edgeBAcc->m_minRadius = edgeB->m_minRadius;
1265         } //else
1266
1267         if (*this == *nodeBAcc)
1268                 return true;
1269         Node* aux = NULL;
1270         bool suc = false;
1271         vec_nodes::iterator it = nodeB->m_children.begin();
1272         for (; it != nodeB->m_children.end(); ++it)
1273         {
1274                 aux = *it;
1275                 if (this->FindSimilarEdgeByAccumulation(aux, nodeBAcc))
1276                 {
1277                         suc = true;
1278                         break;
1279                 } //if
1280         } //for
1281         if (suc)
1282         {
1283                 nodeB = aux;
1284                 return true;
1285         }//if
1286         return false;
1287 }
1288
1289 bool Node::FindMarked(Node* result)
1290 {
1291         vec_nodes::iterator it = this->m_children.begin();
1292         for (; it != this->m_children.end(); ++it)
1293                 if ((*it)->m_mark)
1294                 {
1295                         result = *it;
1296                         return true;
1297                 } //if
1298                 else if ((*it)->FindMarked(result))
1299                         return true;
1300         result = NULL;
1301         return false;
1302 }
1303
1304 bool Node::MarkPath(Node* nodeB)
1305 {
1306         if (this == NULL || nodeB == NULL)
1307                 return false;
1308         //comparing addresses
1309         if (this == nodeB)
1310         {
1311                 nodeB->m_mark = true;
1312                 return true;
1313         } //if
1314         vec_nodes::iterator it = this->m_children.begin();
1315         for (; it != this->m_children.end(); ++it)
1316                 if ((*it)->MarkPath(nodeB))
1317                 {
1318                         (*it)->m_mark = true;
1319                         return true;
1320                 } //if
1321         return false;
1322
1323 }
1324
1325 unsigned int Node::GetCost(Node* nodeB)
1326 {
1327         if (this == NULL || nodeB == NULL)
1328                 return 0;
1329         unsigned int maxCost = 0;
1330         if (*this == *nodeB)
1331         {
1332                 vec_nodes::iterator it = this->m_children.begin();
1333                 for (; it != this->m_children.end(); ++it)
1334                 {
1335                         vec_nodes::iterator it2 = nodeB->m_children.begin();
1336                         for (; it2 != nodeB->m_children.end(); ++it2)
1337                         {
1338                                 unsigned int currentCost = (*it)->GetCost(*it2);
1339                                 if (currentCost > maxCost)
1340                                         maxCost = currentCost;
1341                         } //for
1342                 } //for
1343                 maxCost += 1;
1344         } //if
1345         return maxCost;
1346 }
1347
1348
1349 //Setters and Modifiers
1350
1351 void Node::SetId(unsigned int nM_id)
1352 {
1353         m_id = nM_id;
1354 }
1355
1356
1357 void Node::AddChild(Node* node)
1358 {
1359         this->m_children.push_back(node);
1360 }
1361
1362 void Node::SetChildren(std::vector<Node*>& children)
1363 {
1364         this->m_children = children;
1365 }
1366
1367 void Node::SetFather(Node* nFather)
1368 {
1369         father = nFather;
1370 }
1371
1372 void Node::SetEdge(Edge* edge)
1373 {
1374         this->m_edge = edge;
1375 }
1376
1377 void Node::SetCoords(const Vec3& coords)
1378 {
1379         this->m_coords = coords;
1380 }
1381
1382 void Node::SetLevel(const int& level)
1383 {
1384         this->m_level = level;
1385 }
1386
1387 bool Node::DeleteNodeById(unsigned int nId)
1388 {
1389         if(this->IsLeaf())
1390                 return false;
1391
1392         bool deleted = false;
1393
1394         vec_nodes::iterator it = this->m_children.begin();
1395         while(it != this->m_children.end() && !deleted)
1396         {
1397                 if((*it)->m_id == nId)
1398                 {
1399                         it = m_children.erase(it);
1400                         deleted = true;
1401                 }
1402                 else
1403                         ++it;
1404         }
1405
1406         if(!deleted)
1407         {
1408                 for(vec_nodes::const_iterator it = this->m_children.begin(); it != this->m_children.end() && !deleted; ++it)
1409                 {
1410                         deleted = (*it)->DeleteNodeById(nId);
1411                 }
1412         }
1413
1414         return deleted;
1415 }
1416
1417 void Node::MergeNodes(Node* nodeB)
1418 {
1419         if (nodeB == NULL)
1420                 return;
1421         this->m_coords = nodeB->m_coords;
1422         Edge* edgeA = this->m_edge;
1423         Edge* edgeB = nodeB->m_edge;
1424         edgeA->m_angle = edgeB->m_angle;
1425         edgeA->m_eDistance = edgeB->m_eDistance;
1426         edgeA->m_length += edgeB->m_length;
1427         edgeA->m_aRadius = (edgeB->m_aRadius + edgeA->m_aRadius) / 2.0;
1428         edgeA->m_maxRadius = edgeB->m_maxRadius > edgeA->m_maxRadius ? edgeB->m_maxRadius : edgeA->m_maxRadius;
1429         edgeA->m_minRadius = edgeB->m_minRadius < edgeA->m_minRadius ? edgeB->m_minRadius : edgeA->m_minRadius;
1430         edgeA->m_vec_pair_posVox_rad.insert(edgeA->m_vec_pair_posVox_rad.end(), edgeB->m_vec_pair_posVox_rad.begin(), edgeB->m_vec_pair_posVox_rad.end());
1431 }
1432
1433 void Node::Mark()
1434 {
1435         this->m_mark = true;
1436 }
1437
1438 void Node::UnMark()
1439 {
1440         this->m_mark = false;
1441 }
1442
1443 void Node::UpdateLevels(unsigned int level)
1444 {
1445         this->m_level = level;
1446         level++;
1447         vec_nodes::iterator it = this->m_children.begin();
1448         for (; it != this->m_children.end(); ++it)
1449                 (*it)->UpdateLevels(level);
1450 }
1451
1452 void Node::printUpToLevel(int level)
1453 {
1454         // Print myself
1455         cout << "level" << level << " " << m_coords << " || " << endl;
1456         if(level == 1)
1457                 return;
1458
1459         // Print my children
1460         for (vec_nodes::iterator it = this->m_children.begin(); it != this->m_children.end(); ++it)
1461         {
1462                 (*it)->printUpToLevel(level-1);
1463         }
1464 }
1465
1466 void Node::printNodeAndChildrenIds( )
1467 {
1468         std::cout << "------------------------------------------" << std::endl;
1469         std::cout << "Id: " << this->m_id << std::endl;
1470
1471         // Print children
1472         for(vec_nodes::const_iterator it = this->m_children.begin(); it != this->m_children.end(); ++it)
1473                 std::cout << (*it)->m_id << ";";
1474
1475         // Recursion
1476         for(vec_nodes::const_iterator it = this->m_children.begin(); it != this->m_children.end(); ++it)
1477                 (*it)->printNodeAndChildrenIds( );
1478 }
1479
1480 void Node::printChildrenIds( )
1481 {
1482         std::cout << "------------------------------------------" << std::endl;
1483         std::cout << "Id: " << this->m_id << std::endl;
1484
1485         // Print children
1486         for(vec_nodes::const_iterator it = this->m_children.begin(); it != this->m_children.end(); ++it)
1487                 std::cout << (*it)->m_id << ";";
1488 }
1489
1490 void Node::createQuaternionsUpToLevel(int level, int requiredLevel)
1491 {
1492         //cout << "Level:" << requiredLevel-level <<endl;
1493
1494         if(level >= 2 && this->HasMinimumLevels(2))
1495         {
1496                 // 1. Take the vector to each son (SP - The vector that points from the son to the parent [P-S]).
1497                 // 2. For each son take each vector from son to grandson (SG - the vector that points from the son to the grandson [G-S])
1498                 // 3. For each combination SP-SG calculate the quaternion
1499                 for(vec_nodes::iterator it = this->m_children.begin(); it != this->m_children.end(); ++it)
1500                 {
1501                         // Create vector from son to parent (SP = [P - S])
1502                         Vec3 sp(this->m_coords - (*it)->m_coords);
1503
1504                         // Get the mean radius for the parent-son branch
1505                         double radMeanSP=(*it)->m_edge->GetARadius();
1506
1507                         // Get distance son to parent
1508                         double distSonParent = (*it)->m_edge->GetEDistance();
1509
1510                         // Get the vectors from son to each grandson (SG = [G - S] )
1511                         for(vec_nodes::iterator itps = (*it)->m_children.begin(); itps != (*it)->m_children.end(); ++itps)
1512                         {
1513                                 // Build the sg vector and get the quaternion
1514                                 Vec3 sg((*itps)->m_coords - (*it)->m_coords);
1515                                 Quaternion q(sp, sg);
1516
1517                                 // Get distance son to grandson
1518                                 double distSonGrandson = (*itps)->m_edge->GetEDistance();
1519
1520                                 // Get the mean radius for the son-grandson branch
1521                                 double radMeanSG=(*itps)->m_edge->GetARadius();
1522
1523                                 //cout << "Vectors:" << ps << sg <<endl;
1524                                 //cout << "Q:" << q << ", Distance=" << (*itps)->m_edge->GetEDistance() << endl << endl;
1525                                 // Printing
1526                                 // Level | PositionSon | SPvector | SGvector | Quaternion | distanceSonParent | distanceSonGrandSon | meanRadiusSP | meanRadiusSG
1527                                 cout << requiredLevel-level << " " << (*it)->m_coords << " " << sp << " " << sg << " " << q << " " << distSonParent << " " << distSonGrandson << " " << radMeanSP << " " << radMeanSG << endl;
1528                         }
1529                 }
1530
1531                 for(vec_nodes::iterator it = this->m_children.begin(); it != this->m_children.end(); ++it)
1532                 {
1533                         (*it)->createQuaternionsUpToLevel(level-1, requiredLevel);
1534                 }
1535         }
1536 }
1537
1538 std::pair<double, std::pair<Node*, Node*> > Node::GetPairWithClosestDistance_FirstNodeNonMarked(std::multimap<double, pair_nodes> map_dist_pairNodes)
1539 {
1540         // Variables
1541         bool pairFound = false;
1542         std::pair<Node*, Node*> pairNodes_null (NULL, NULL);
1543         std::pair<double, std::pair<Node*, Node*> > pair_closest_dist_pairNodes (-1, pairNodes_null);
1544
1545         std::multimap<double, pair_nodes>::iterator it = map_dist_pairNodes.begin();
1546
1547         for( ; it != map_dist_pairNodes.end() && !pairFound; ++it)
1548         {
1549                 if( ! (*it).second.first->IsMarked() )
1550                 {
1551                         pair_closest_dist_pairNodes.first=(*it).first;
1552                         pair_closest_dist_pairNodes.second=(*it).second;
1553                         pairFound = true;
1554                 }
1555         }
1556
1557         return pair_closest_dist_pairNodes;
1558 }
1559
1560 void Node::getStasticsBifurcations(int* p)
1561 {
1562         int numBif = this->m_children.size();
1563         if(numBif>0 && numBif<=6)
1564                 p[numBif-1]=p[numBif-1]+1;
1565
1566         for(vec_nodes::iterator it = this->m_children.begin(); it != this->m_children.end(); ++it)
1567         {
1568                 (*it)->getStasticsBifurcations(p);
1569         }
1570 }
1571
1572 //Operator Overloads
1573
1574 Node &Node::operator=(Node & node)
1575 {
1576         this->m_children = node.m_children;
1577         this->m_edge = node.m_edge;
1578         this->m_coords = node.m_coords;
1579         this->m_level = node.m_level;
1580         this->m_mark = node.m_mark;
1581         return *this;
1582 }
1583
1584 const Node&Node::operator=(const Node& node)
1585 {
1586         this->m_children = node.m_children;
1587         this->m_edge = node.m_edge;
1588         this->m_coords = node.m_coords;
1589         this->m_level = node.m_level;
1590         this->m_mark = node.m_mark;
1591         return *this;
1592 }
1593
1594 bool Node::operator ==(const Node& node)
1595 {
1596         bool comp = false;
1597         if (this->m_edge == NULL && this->m_edge != NULL)
1598                 return false;
1599         else if (this->m_edge != NULL && this->m_edge == NULL)
1600                 return false;
1601         else if (this->m_edge == NULL && this->m_edge == NULL)
1602                 comp = true;
1603         else
1604                 comp = ((*this->m_edge) == (*node.m_edge));
1605         bool ret = (/*(this->m_coords == node.m_coords)
1606      && (this->m_children.size() == node.m_children.size())
1607      && */(this->m_level == node.m_level) && comp);
1608         return ret;
1609 }
1610
1611 bool Node::operator !=(const Node& node)
1612 {
1613         bool comp = false;
1614         if (this->m_edge == NULL && this->m_edge != NULL)
1615                 return false;
1616         else if (this->m_edge != NULL && this->m_edge == NULL)
1617                 return false;
1618         else if (this->m_edge == NULL && this->m_edge == NULL)
1619                 comp = true;
1620         else
1621                 comp = ((*this->m_edge) == (*node.m_edge));
1622         return (/*(this->m_coords != node.m_coords)
1623      && (this->m_children.size() != node.m_children.size())
1624      && */(this->m_level != node.m_level) && !comp);
1625 }
1626
1627 bool Node::operator <(const Node& node)
1628 {
1629         bool comp = false;
1630         if (this->m_edge == NULL && this->m_edge != NULL)
1631                 return false;
1632         else if (this->m_edge != NULL && this->m_edge == NULL)
1633                 return false;
1634         else if (this->m_edge == NULL && this->m_edge == NULL)
1635                 comp = false;
1636         else
1637                 comp = ((*this->m_edge) < (*node.m_edge));
1638         return comp;
1639 }
1640
1641 bool Node::operator >(const Node& node)
1642 {
1643         bool comp = false;
1644         if (this->m_edge == NULL && this->m_edge != NULL)
1645                 return false;
1646         else if (this->m_edge != NULL && this->m_edge == NULL)
1647                 return false;
1648         else if (this->m_edge == NULL && this->m_edge == NULL)
1649                 comp = false;
1650         else
1651                 comp = ((*this->m_edge) > (*node.m_edge));
1652         return comp;
1653 }
1654
1655 } /* namespace airways */