Подобные алгоритмы уже давно описаны.

В первой попавшейся толковой книге по разработке игр.