Done: Fix 'knnSearch doesn't return espected nearest point' bug. It caused by addPoints function add a reference to index. Solution is dynamicly allocate pt_new in loop, which means declare the pt_new in the loop. Every loop instance, pt_new will be freshly allocated. Changing the pt_new laster doesn't affect previous values; Fix 'tree doesn't look good' bug. It's caused by poorly generated random number. Solution include two parts, change integer random number to float number and adjust the boundary of the random number generator according to the initial and final joint values