View source for Drive trough a corridor

Solution:

Algorithm:
1) Sort vector of Walls such that theta1<theta2<...<thetaN
2) Select the first entry of vector as right wall and last entry of vector as left wall.
3) Define a fixed frame (x0,y0,z0) on the left wall. x0 at left wall pointed in driving direction, y0 perpedicular to wall, pointed inwards to Pico, z0 along right hand rule.
4) Define position Pico (p1) in (x0,y0,z0) coordinates
5) Define angle Pico in (x0,y0,z0) coordinates
6) Define reference point (p2) in (x0,y0,z0) coordinates
7) Definieer referenceangle phi from (p1) t0 (p2) in (x0,y0,z0) coordinates
Assumptions:
1) Pico starts with his “face” pointed inside the corridor.
2) The referencepoint (p2) is positioned at the middle of the corridor, 1 meter ahead of Pico’s current position.