Fixed object addition to kd-tree bug, still not fully usable
Fixed object addition functions...
fixed kdTree, still not optimal, but 2x speedup