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