Obstacle Shadowing Bug

[manglemix]

Describe the bug
When the depth camera is observing an obstacle without being able to see the area behind it, the next valid vertices to the top boundary of the obstacle actually lie some distance behind the obstacle. As a result, the triangle formed is extremely elongated and leads the heightmap builder to think the obstacle stretches out much further than expected.

To Reproduce
Steps to reproduce the behavior:

1. Run Lunasim
2. Pilot the robot towards a medium sized rock
3. Look out for floating points

Expected behavior
The heightmap should correctly approximate the shape of rocks despite their size.

Screenshots

As visible here, there are magenta points above the ground because the depth camera cannot see the area behind the rock, which naturally leads it to assume the rock is bigger than it actually is.

This is an inaccurate visualization of how the depth map is tessellated, but it does get the point across. The vertices along the top edge of the rock have valid neighbors above them, but in reality they are much further out than is apparent. As such, those vertices should be considered invalid as neighbors as they would make the obstacle seem much larger than it actually is, when really the algorithm should just not know what is on the other side until the robot can actually see the other side.

Additional context
One possible solution is to update the checks performed on each triangle before the heightmap is interpolated to fit the triangle. Right now, it only checks that each vertex is a valid vertex (in that it is a vertex observed by the depth camera). However, we can set a limit for how skewed a triangle can be, as very skewed triangles are correlated with this bug. That solution would simply be an amendment to the code here.