for (int i = 0; i < startPos.size(); i++) {
VectorXYZ start = startPos.get(i);
VectorXYZ end = endPos.get(i);
double lenToEnd = end.distanceToXZ(start);
double heightDiff = end.y - start.y;
double stepSize = lenToEnd / INTERPOLATION_STEPS;
VectorXZ dir = end.xz().subtract(start.xz()).normalize();