Theta Star implementation in C++