Implemented a rather ugly way for testing the correctness of kd-trees (Since it is just for testing I hope it is okay).