private int FindIndexAt(Vector3d vector) { var minIndex = 0; var minDistance = _p1.DistanceSquaredTo(vector); if (minDistance < _tolerance) { return(minIndex); } // have to loop through all points as this is a curve, O(n) for (var i = 0; i < _steps.Length; i++) { var distance = _points[i].DistanceSquaredTo(vector); if (distance.LessOrEqual(minDistance, _tolerance)) { minDistance = distance; minIndex = i; } } return(minIndex); }