Fix incorrect coordinate transformation in spline

[Zhen-Ni]

The coordinate transformation from Cartesian to Frenet in spline seems to have some incorrect output when the point is exactly on the spline. Here's the code for reproducing this issue:

spline = LinearSpline2D([(0, 0), (1, 0), (2, 0.5)])

# Find two very close points, the first is exactly lying on the
# spline, and the second is slightly offset by 0.0001
coor1 = spline.cartesian_to_frenet((1.5, 0.25))
coor2 = spline.cartesian_to_frenet((1.5, 0.25+0.0001))

# The following code prints (np.float64(1.5), np.float64(0.25)),
# which is incorrect as the lateral distance should be 0 (got 0.25 instead), because this point lies on the spline.
print(coor1)

# prints (np.float64(1.5590617157344975), np.float64(8.944271909994139e-05))
# A small deviation should not have such large error for lateral distance.
print(coor2)

This PR provides fix and optimization of the coordinate transform to solve the issue.