This logic doesn't guarantee that the last point is equal to `p2`.  It might even be past it.  I'm not sure if the Insight3D code had the same issue, but it's easy to fix just by stopping shy of numPoints and doing `result[result.length-1] = p2`.