Found error in kd-tree construction. Partly fixed, but doesn't look quite right yet.