Как грамотно остановить объект при столкновении?
Ваша проблема в том, что вы допускаете залезание объекта в препятствие, а потом не знаете, как его оттуда вынуть. 

На каждой итерации, вам нужно проверять отрезок прямой, по которому ваш объект шагает и искать внутри этого отрезка такой параметр времени theta, при котором объект начинает касаться стенки. 

После этого вам остается зафиксировать ваш объект в этом положении.

Поиск пересечения нужно делать аналитически, это быстрее.
Сдвигать по нормали статичной поверхности. Во "взрослых" движках нельзя просто сдинуть, потому что это ломает всю непрерывную физику, поэтому вместо сдвига добавляется ускорение в ту сторону, куда надо сдвинуть. Этим объясняются прыгающие ящики и трупы и прочее - из-за погрешностей вычисления объекты постоянно проникают, выстреливют наружу и обратно проникают при падении.