Я не отлаживал / не читал весь код, но это похоже на классическую проблему.Как только вы обнаружите столкновение, вы разрешите его с одним направлением и проникновением.
В результате получается, что после перемещения и столкновения, разрешение по одной оси может оставить другую с сохранением столкновения.И оттуда все рушится.
Или может случиться так, что вы столкнетесь с двумя объектами, если разрешить второй, вы вернетесь к тому, из которого вы уже настроены.
По крайней мере,после корректировки положения необходимо проверить, все ли чисто, для всех объектов.