Added the heuristics Search Tests - SearchTest2 to the CMakeLists
I changed the EqualMP function a bit. Explanations are provided inside the file
Decided on a heuristics. Commented the others out and explained when/ how/ why they should/ should not be used. I am not sure on whether saving the first part of the old path - the one before hitting the first obstacle is worth it. The tests in the Search Test seem do indicate that it is, the tests in the Search Test 2 seem to indicate that it is not. I will look more into it