Code extracted from https://github.com/stonier/cost_map - View it on GitHub