fixed diagonal splitting, my bad.
Merge branch 'master' into collision_filter