Optimized some code that used to iterate through the whole occupancy grid and was a bottleneck.

Removed some unused fields/methods.

Refactored OccupancyGrid class.
Fixed and tested optimizations in previous commit
Fixed some bottlenecks (for calculating relayedCells - now no longer iterate through the whole OccGrid)