Fixed some template compile time errors that showed themselves after I instantiated the code with find_nearest_intersection(). It compiles now and seems to sort of work but still looking into parts where it breaks.