Finished modification of kd tree initialization

may need debugging later