Added many optimizations.

- Inlined tons of inner loop functions.
- Got rid of virtual funciton calls in inner loop.
- Use std::vector and custom sorted array instead of
  std::set and std::multiset.
- Stop using std::iterators, these were not inlined.
- Added some temp vars.

All optimizations had a > 5% speed increase on their own or they
weren't made.  There are more optimizations that can be made
but I think we are good enough for now.  Tests run faster than
reported in the paper now and the machine in the paper is clocked
faster (not that this means a lot).

Tested using Clang on OSX although the changes should help other
compilers too.