#include "SurgSim/DataStructures/AabbTreeData.h"
#include "SurgSim/Framework/Assert.h"
#include <algorithm>