Vectorized raycast for lidar

[Zartris]

Currently, the lidar is a MAJOR bottleneck. It loops over all angles and all entities, so if you want a lidar with a remotely similar amount of rays as a true lidar this takes seconds for each sensor you use in the environment.

I have optimized it so now we are not looping over each angle and it gives me num_rays x improvement

Here I included a test with each landmark/ shape type. I have a timer in the measurement function (needs to be removed if you agree to accept the PR) The measurement function is doing both the loop and the vectorized methods to test if they are truly similar. I left these in so you can test for a bit yourself. Then I will clean it up and we can merge.

With 100 rays, a single agent, 3 objects:

LOOP TIME: 0.06827758200051903
VEC TIME: 0.000823776999823167