Алгоритм, в кратце, такой: обход в ширину по графу состояний. Представьте, что каждому положению всех машинок на поле соответствует вершина в некотором графе. Каждому возможному ходу - ориентированное ребро в графе. Начальное заданное состояние - это начальная вершина. Любая вершина, где нужная машинка стоит впритык к выходу с поля - считается конечной. Чтобы найти кратчайший путь - нужно запустить в этом графе обход вширину.
Теперь детали. Сам граф хранить не надо. Надо только уметь как-то хранить, что некоторое состояние поля уже было просмотрено (если нужно еще и сами ходы решения найти, то нужно еще хранить для каждого состояния предшествующее ему в пути). Тут подойдет какой-нибуть hash map или map.
Для обхода в ширину понадобится еще и очередь. Осталось только научиться по заданному состоянию получать все, куда можно из него перейти за один ход. Для этого состоянием удобно считать не матрицу прямоугольные - а набор чисел: для каждой машинки где в своей строке/столбце она находится. По такому состоянию очень просто восстановить саму матрицу. Потом просто перебираем каждую машинку и все возможные ее сдвиги. Смотрим в матрице, что все клетки нового положения ничем другим не заняты. Если все хорошо - вот новое состояние.
На практике это будет работать довольно быстро. Если машинок много - то допустимых состояний будет мало. Если же машинок мало, то ответ найдется довольно быстро, а значит просмотрено будет не так много состояний. Проблема только, если решения не существует, а машинок не слишком много. Тогда этот алгоритм будет работать медленно. Но, оценка сверху - N^K состояний, где N - размер поля, а K - количество машинок. Для небольших полей (N=6) будет решать все мгновенно.
