Fixed Jordy's bug when visualizing the occupied_voxels. It was a silly ol' thang. Still need to fix worldToGrid and gridToWorld to warn the user properly.