Variable driveable bounds

[Agustin Alba Chicar]

Parent issue: #142

Given a set of lanes, we need to create a variable driveable bounds based on a on the position s of the lane and the other lanes.

Here is a picture to show the problem:

Blue lanes are the baselines of the lanes and the blue dots are the positions of the s values on each lane. We want to the get the driveable bounds of L_1 for s = s_1. So, we need to get the values of s_0 and s_2 to then determine the distance from the GeoPosition of s_1 to s_0 and s_2 to s_1. It must be said that s_2 or s_0 are not the values that minimizes the distance to their respective lanes but the values of s that match the cut of the r direction for s_1 on each lane (as the arrows show).

[Agustin Alba Chicar]

As we don't have an API to interpolate the normal of the tangent vector and get the intersection point over l_0 and l_2, I'm going to test a brief approximation of the splines as straight lines given the first and last point of each lane. Then intersect that line approximation with the normal of the s_1 and find the point which corresponds to s_0 / s_2.

[Agustin Alba Chicar]

Got a partial solution, which is based on the description of the issue. It works for multiple lanes and headings. However, I still got the problem of the orientation of the lanes (e.g. a road can be have two lanes one that goes from South to North and the other from North to South). This makes very complex the algorithm and should be treated in another issue (#147)

[Agustin Alba Chicar]

Some pictures of segments with multiple lanes that go in the same direction:

[Agustin Alba Chicar]

And a piece of Darpa file: