Optimize Geometry2D

[kylestach]

Description

Many components in our Geometry2d are horifically unoptimized. As part of the motion planning rewrite I've had to fix a few of them. Some examples:

• ShapeSet collision detection: first called hitSet to construct a set of all obstacles, then returned whether that set was empty (huge allocation overhead, could be fixed just by checking if each shape hits in sequence)
• Rectcollision: constructs a vector of four Segments and checks collision for each one independently (can be calculated very easily).

In general, there are likely many other cases of similar garbage in the Geometry2d library. Considering that it's basically used everywhere in our code, we should really put in the effort to vet it with a fine-toothed comb to make sure we're not throwing away CPU cycles and allocating for no reason.

Requirements

• It should be possible to construct a non-owning shape set, to avoid shared_ptr overhead
  • Idea: template it on the contained type
• Each algorithm in Geometry2d should be checked to make sure that it is at least of reasonable complexity. Any known naïve algorithms in Geometry2d should be marked as such.

Design

N/A