Heuristic debugging colours

* Also add jitter to heuristic target points