Make some variables const to enable better compiler optimizations.
Pre-compute some common values (cos and sine) used in scan matching.
Reorganize the array2d.h and harray2d.h to use a single array instead of nested arrays. This avoids an additional pointer indirection when looking up cell information.
Remove a layer of pointer indirection in autoptr, which is only used as a layer in harray2d.h. This avoids an additional pointer indirection when looking up cell information.
Remove some of the API surface on harray2d.h to avoid creating and destroying IntPoint objects when doing cell lookups.
Overall, when built in RelWithDebInfo mode, the time for each call to scanMatch goes from ~730ms to ~500ms.
Speed up the scan matching step of gmapping by about 50% through the following optimizations and refactoring:
Overall, when built in RelWithDebInfo mode, the time for each call to scanMatch goes from ~730ms to ~500ms.