Move optimize routines and bound tree updater code