modified sweep-prune to correctly remove bodies from tracking