Rough Idea for iteration over hierarchy

main.rs

use nalgebra::*;
use std::hash::Hash;

struct Node {
    similarity: Similarity3<f32>,
    children: u32,
}

fn main() {   
    let mut hierarchy: Vec<Node> = vec![
        Node { similarity: Similarity3::from_parts(Translation3::identity(), UnitQuaternion::identity(), 1.0), children: 5},
          Node { similarity: Similarity3::from_parts(Translation3::new(4.0, 0.0, 0.0), UnitQuaternion::identity(), 1.0), children: 2},
            Node { similarity: Similarity3::from_parts(Translation3::new(0.0, 0.0, 0.0), UnitQuaternion::identity(), 1.0), children: 0},
            Node { similarity: Similarity3::from_parts(Translation3::new(2.0, 0.0, 0.0), UnitQuaternion::identity(), 1.0), children: 0},
          Node { similarity: Similarity3::from_parts(Translation3::new(0.0, 6.0, 0.0), UnitQuaternion::identity(), 1.0), children: 3},
            Node { similarity: Similarity3::from_parts(Translation3::new(-1.0, 0.0, 0.0), UnitQuaternion::identity(), 1.0), children: 0},
            Node { similarity: Similarity3::from_parts(Translation3::new(-1.0, 0.0, 0.0), UnitQuaternion::identity(), 1.0), children: 0},
            Node { similarity: Similarity3::from_parts(Translation3::new(0.0, 6.0, 0.0), UnitQuaternion::identity(), 1.0), children: 3},
              Node { similarity: Similarity3::from_parts(Translation3::new(-1.0, 0.0, 0.0), UnitQuaternion::identity(), 1.0), children: 0},
              Node { similarity: Similarity3::from_parts(Translation3::new(-1.0, 0.0, 0.0), UnitQuaternion::identity(), 1.0), children: 0},
              Node { similarity: Similarity3::from_parts(Translation3::new(0.0, 6.0, 0.0), UnitQuaternion::identity(), 1.0), children: 3},
                Node { similarity: Similarity3::from_parts(Translation3::new(-1.0, 0.0, 0.0), UnitQuaternion::identity(), 1.0), children: 0},
                Node { similarity: Similarity3::from_parts(Translation3::new(-1.0, 0.0, 0.0), UnitQuaternion::identity(), 1.0), children: 0},
                Node { similarity: Similarity3::from_parts(Translation3::new(0.0, 6.0, 0.0), UnitQuaternion::identity(), 1.0), children: 3},
                  Node { similarity: Similarity3::from_parts(Translation3::new(-1.0, 0.0, 0.0), UnitQuaternion::identity(), 1.0), children: 0},
                  Node { similarity: Similarity3::from_parts(Translation3::new(-1.0, 0.0, 0.0), UnitQuaternion::identity(), 1.0), children: 0},
                  Node { similarity: Similarity3::from_parts(Translation3::new(-1.0, 0.0, 0.0), UnitQuaternion::identity(), 1.0), children: 0},
          Node { similarity: Similarity3::from_parts(Translation3::new(4.0, 0.0, 0.0), UnitQuaternion::identity(), 1.0), children: 1},
            Node { similarity: Similarity3::from_parts(Translation3::new(4.0, 0.0, 0.0), UnitQuaternion::identity(), 1.0), children: 0},
          Node { similarity: Similarity3::from_parts(Translation3::new(4.0, 0.0, 0.0), UnitQuaternion::identity(), 1.0), children: 2},
            Node { similarity: Similarity3::from_parts(Translation3::new(4.0, 0.0, 0.0), UnitQuaternion::identity(), 1.0), children: 0},
            Node { similarity: Similarity3::from_parts(Translation3::new(4.0, 0.0, 0.0), UnitQuaternion::identity(), 1.0), children: 0},
          Node { similarity: Similarity3::from_parts(Translation3::new(4.0, 0.0, 0.0), UnitQuaternion::identity(), 1.0), children: 0},
    ];    

    process_hierarchy(&mut hierarchy);
}

fn process_hierarchy(hierarchy: &mut Vec<Node>) {
    let mut parents: Vec<(u32, u32, Transform3<f32>)> = Vec::new();
    let mut parent_transform: Transform3<f32> = Transform3::identity();
    let mut node_id = 0;
    let mut current_child = 0;
    let mut parent_num_children = 0; 
    let mut level = String::from(""); // used to for a visual of the heirarchy
    for node in hierarchy.iter() {
        println!("{} {}Processing child {}", node_id, level, current_child);
        
        // Could be pushing into a Query here <(Virtual<ParentTransform>, Read<Similarity3>, Write<LocalToWorld>)>::query()
        let local_transform: Transform3<f32> = nalgebra::convert(node.similarity);
        let local_to_world = parent_transform * local_transform;
        println!("{}", local_to_world * Point3::new(0.0, 0.0, 0.0));
        current_child += 1;
        if node.children > 0 {
            // go deeper into hierarchy
            level.push_str("--");
            parents.push((current_child, parent_num_children, parent_transform));
            current_child = 0;
            parent_num_children = node.children;
            parent_transform = local_to_world;
        } else {
            while current_child >=  parent_num_children {
              // go back up hierarchy
                match parents.pop() {
                    Some((cc, pnc, pt)) => {
                        current_child = cc;
                        parent_num_children = pnc;
                        parent_transform = pt;
                    },
                    _ => break,
                };

                level.pop();
                level.pop();
            }
        } 
        node_id += 1;
    }
    ()
}