Collision: fix partition algorithm

- Partition sort had a degenerate case where 1 element in the array would always be assigned to the disabled bucket, regardless of if it passed the partitioning test.
- The fix also makes the sort logic generally simpler as a side benefit.

src/collider/BVHTree.cpp

@@ -305,17 +305,17 @@ uint32_t SingleBVHTree::Partition(SortKey *keys, uint32_t numKeys, const AABBd &
 
 	// Simple O(n) sort algorithm to sort all objects according to side of pivot
 	uint32_t startIdx = 0;
-	uint32_t endIdx = numKeys - 1;
+	uint32_t endIdx = numKeys;
 
 	// It is ~10% faster to sort the indices than to sort the whole AABB array
 	// (cache hit rate vs. memory bandwidth)
 	// Sorting in general is extremely fast.
-	while (startIdx <= endIdx && endIdx) {
+	while (startIdx < endIdx) {
 		if (keys[startIdx].center[axis] < pivot) {
 			startIdx++;
 		} else {
-			std::swap(keys[startIdx], keys[endIdx]);
 			endIdx--;
+			std::swap(keys[startIdx], keys[endIdx]);
 		}
 	}
 

src/collider/CollisionSpace.cpp

@@ -236,14 +236,14 @@ uint32_t CollisionSpace::SortEnabledGeoms(std::vector<Geom *> &geoms)
 	// Simple O(n) sort algorithm
 	// Sort geoms according to enabled state (group all enabled geoms at start of array)
 	uint32_t startIdx = 0;
-	uint32_t endIdx = geoms.size() - 1;
+	uint32_t endIdx = geoms.size();
 
-	while (startIdx <= endIdx && endIdx) {
+	while (startIdx < endIdx) {
 		if (geoms[startIdx]->IsEnabled()) {
 			startIdx++;
 		} else {
-			std::swap(geoms[startIdx], geoms[endIdx]);
 			endIdx--;
+			std::swap(geoms[startIdx], geoms[endIdx]);
 		}
 	}