Avoicdance.pde: implemented calc_dist and unify_trees