More robust start/finish of laps

[Thonners]

Resetting the vehicle's position results in a new lap being triggered (and a lap being 'completed'

Lap checking could potentially be improved with a few vector based checks of the starting line coords (fail fast to keep runtime down):

• If magnitude of distance of vehicle from start line vector > distance travelled in one timestep, assume we won't cross the line on this timestep
  • Compare distances squared to save on sqrt calculations. Fail fast - this should be the furthest we get in 99.9% of the time
  • Or could just check distance to one of the points (but then need to compare to [distance travelled per timestep + track width] as unless we're driving down the edge of the track the distance will be greater)
• if dot product of vectors from car to inner & outer start line > 0, then they're both in the same direction, so we're not going through the gate (could be on the 'back straight', so going through the (infinite) start line, but not the finite version).
• Should be able to use sign of cross product to check whether we're driving towards/away from start line (need to define if track is L/R handed as this will switch sign change we're looking for). If driving away from it, then no need to check! (when sign switches, then we've gone through the start line!)

[Thonners]

On second thoughts, much simpler to check for intersection between the start line and the line between p(t-1), p(t), where p(t) is the position of the vehicle at time t.

Still need some logic to deal with vehicle resets (perhaps just setting lapcount to 0 on reset)

[steviet91]

Method two sounds simpler to implement and should exit quickly thanks to the improvements you made for the lidar intersections. Resetting the lapcounter sound reasonable, if someone has needed to reset then their data should be voided