Add unit tests using boost test

Added UnitTests for a few classes, as well as fixing some minor bugs and cleaning up a little bit in the process.
The min\_dist value is an upper bound to the minimum distance between the input location and the nearest point contained in the MBR. It is used to sort the elements in the queue by which they are explored.