Fixed the SBA* algorithm. It works awesome so far!