Replace Straight Line constraint with Lane constraint

[shueja]

A Lane constraint is two line constraints on the center of the robot, offset perpendicular to the line between two adjacent waypoints.

-------- between this line
a-----b
-------- and this line

[calcmogul]

The line constraint should be something like this:

ax + by > c

m = (y₁ - y₀)/(x₁ - x₀)
y - y₀ = m(x - x₀)

y - y₀ = mx - mx₀
y = mx - mx₀ + y₀
-mx + y = -mx₀ + y₀
-(y₁ - y₀)/(x₁ - x₀)x + y = -(y₁ - y₀)/(x₁ - x₀)x₀ + y₀
-(y₁ - y₀)x + (x₁ - x₀)y = -(y₁ - y₀)x₀ + (x₁ - x₀)y₀
(y₀ - y₁)x + (x₁ - x₀)y = (y₀ - y₁)x₀ + (x₁ - x₀)y₀

(y₀ - y₁)x + (x₁ - x₀)y > (y₀ - y₁)x₀ + (x₁ - x₀)y₀

We'd use a pair of them with flipped inequalities to define the lane.