Institut de Robòtica i Informàtica Industrial

InsertInTree

PURPOSE ^

Inserts an element in a BTree.

SYNOPSIS ^

function T=InsertInTree(T,element)

DESCRIPTION ^

 Inserts an element in a BTree.

 Adds an element to the rigth-most extrem of a BTree and then it
 rebalances the tree. Moreover, the information of the tree nodes from the
 new leaf to the root are updated with the information of the new element.
 
 See also BTree.

CROSS-REFERENCE INFORMATION ^

This function calls:
  • get Get function for robots.
  • get Get function for trajectories.
  • get Get function for EKF filters.
  • get Generic get function for filters.
  • get Get function for TRO filters.
  • get Get function for BTrees.
  • Union Union of two PoseData objects.
  • get Get function for PoseData objects.
  • get Get function for Gaussians.
  • Union Iout=Union(d1,d2)
  • Union Hull of two interval matrices.
  • get Get function for poses.
  • get Generic get for relative positioning sensors
  • get Generic get for sensors
This function is called by:
  • Simulation Simulates a robot performing Pose SLAM.
  • BTree Balanced tree constructor.

SOURCE CODE ^

0001 function T=InsertInTree(T,element)
0002 % Inserts an element in a BTree.
0003 %
0004 % Adds an element to the rigth-most extrem of a BTree and then it
0005 % rebalances the tree. Moreover, the information of the tree nodes from the
0006 % new leaf to the root are updated with the information of the new element.
0007 %
0008 % See also BTree.
0009 
0010   if T.last==0
0011     % The tree is empty. Insert as a first element
0012     T.last=1;
0013     T.root=1;
0014     T.leaves(get(element,'id'))=T.last;
0015     T.data{1}.element=element;
0016     T.data{1}.left=0;
0017     T.data{1}.right=0;
0018     T.data{1}.heigth=1;
0019     T.data{1}.parent=0;
0020   else
0021     % The tree already hass element. Just add a new one.
0022     
0023     n=T.last; % last element in the tree up to now
0024     p=T.data{n}.parent; 
0025     
0026     n2=n+1; % place for the root of the last element up to now and
0027             % the element we are adding to the tree now
0028                 
0029     n1=n2+1; % Place for element we are adding to the tree right now
0030     
0031     T.leaves(get(element,'id'))=n1;
0032     T.data{n1}.element=element;
0033     T.data{n1}.left=0;
0034     T.data{n1}.right=0;
0035     T.data{n1}.heigth=1;
0036     T.data{n1}.parent=n2;
0037 
0038     T.data{n}.parent=n2;
0039 
0040     T.data{n2}.element=Union(element,T.data{n}.element);
0041     T.data{n2}.left=n;
0042     T.data{n2}.right=n1;
0043     T.data{n2}.heigth=2;
0044     T.data{n2}.parent=p;
0045     
0046     T.last=n1;
0047     
0048     if p>0
0049       T.data{p}.right=n2;
0050     end
0051     
0052     if T.root==n
0053       T.root=n2;
0054     end
0055     
0056     % now navigate back to the root, updating the internal tree
0057     % nodes and re-balancing the tree
0058     while p~=0
0059       l=T.data{p}.left;
0060       r=T.data{p}.right;
0061       
0062       hl=T.data{l}.heigth;
0063       hr=T.data{r}.heigth;
0064       
0065       if hr-hl>1
0066         % Re-balance current node
0067         rr=T.data{r}.right;
0068         rl=T.data{r}.left;
0069         
0070         T.data{p}.left=r;
0071         T.data{p}.right=rr;
0072         
0073         T.data{r}.left=l;
0074         T.data{r}.right=rl;
0075         
0076         T.data{rr}.parent=p;
0077         T.data{l}.parent=r;
0078         
0079         T.data{r}.element=Union(T.data{l}.element,T.data{rl}.element);
0080         T.data{r}.heigth=max(T.data{l}.heigth,T.data{rl}.heigth)+1;
0081         
0082       else
0083         if hl-hr>1
0084           error('not implemented');
0085         end
0086       end
0087       
0088       l=T.data{p}.left;
0089       r=T.data{p}.right;
0090        
0091       T.data{p}.element=Union(T.data{l}.element,T.data{r}.element);
0092       T.data{p}.heigth=max(T.data{l}.heigth,T.data{r}.heigth)+1;
0093 
0094       % proceed up to the root
0095       p=T.data{p}.parent;
0096     end
0097     
0098   end
0099 
0100   
0101


Institut de Robòtica i Informàtica Industrial

Generated on Fri 24-Jul-2009 12:32:50 by m2html © 2003