Используй А*.
http://en.wikipedia.org/wiki/A*_search_algorithm
Там есть эвристика - она быстрая! Используй ту, которая диагонально-прямая, не помню как она называется - самая хорошая.
А чтобы всё было ещё быстрее - попробуй использовать порталы в помещения. Т. е., если есть дверь, можно улучшить поиск кэшируя прошлые результаты.