Оптимизация внедрения быстрой сортировки

Поэтому я реализовал версию для быстрой сортировки, но я не уверен, что сделал это оптимальным образом. Элементы в векторе, который я сортирую, являются трехмерными точками. Вы заметите, что при разбиении массива он говорит «if (level == 1), if (level == 2) и т. Д.» Это потому, что в зависимости от того, когда я вызываю быструю сортировку, координаты x, y или z из точек является основой сортировки.

Мой код ниже. Я проверил свой алгоритм, и он, кажется, правильно сортирует точки, однако я чувствую, что у меня могут быть большие издержки или что-то в этом роде, потому что он работает не так быстро, как я надеялся, он будет работать, особенно потому, что мне нужно вызвать Эта функция много раз сортирует массивы размером до 500 000 элементов.

Что я могу сделать, чтобы улучшить / очистить / ускорить мой алгоритм сортировки?

std::vector<Point> SortPoints(std::vector<Point> points, int level)
{
if(points.size() <= 1) return points;
std::vector<Point> s1, s2;
size_t index = ((float)rand()/RAND_MAX)*(points.size()-1);
Point pivot = points[index];
points.erase(points.begin() + index);
size_t size = points.size();
for(int i = 0; i < size; i++)
{
if(level == 1)
{
if(points[i].pos.x <= pivot.pos.x) s1.push_back(points[i]);
else s2.push_back(photons[i]);
} else if(level == 2)
{
if(points[i].pos.y <= pivot.pos.y) s1.push_back(points[i]);
else s2.push_back(photons[i]);
} else if(level == 3)
{
if(points[i].pos.z <= pivot.pos.z) s1.push_back(points[i]);
else s2.push_back(photons[i]);
}
}
if(s1.size() == 0) return Concatenate(s1, pivot, s2);
else if(s2.size() == 0) return Concatenate(s1, pivot, s2);
else return Concatenate(SortPoints(s1, level), pivot, SortPoints(s2, level));
}

std::vector<Point> Concatenate(std::vector<Point> s1,
Point p, std::vector<Point> s2)
{
std::vector<Point> result;
result.reserve(s1.size()+s2.size()+1);
result.insert(result.end(), s1.begin(), s1.end());
result.push_back(p);
result.insert(result.end(), s2.begin(), s2.end());
return result;
}