for the high-level search state-by-state collision checking
for the low-level search simple dynamics (all except car with trailer) are considered for the simplicity to verify that the low-level heuristic works. If correct, then adding car with trailer will be not difficult.
I've deleted some lines, please ignore them. Please review only newly added two functions: highLevelfocalHeuristicState() and lowLevelfocalHeuristicState(). It might be slow, but the logic should be correct.
Simple cases are considered:
I've deleted some lines, please ignore them. Please review only newly added two functions: highLevelfocalHeuristicState() and lowLevelfocalHeuristicState(). It might be slow, but the logic should be correct.