Посмотри ЭТОТ ПРОЕКТ, загляни по ссылкам. Там алгоритм попроще, но ИМХО интуитивно понятней.