WIP: Improve implementation of Kd-tree

Use double instead float for storing coordinate location and
reduce number of casts betweeen floats and doubles in parts
of Kd tree implementation.