reimplement GetDistance function (need testing)

Signed-off-by: Konctantin <gawrilyako@gmail.com>