| 535 | } |
| 536 | |
| 537 | void KD_TREE::BuildTree(KD_TREE_NODE ** root, int l, int r, PointVector & Storage){ |
| 538 | if (l>r) return; |
| 539 | *root = new KD_TREE_NODE; |
| 540 | InitTreeNode(*root); |
| 541 | int mid = (l+r)>>1; |
| 542 | int div_axis = 0; |
| 543 | int i; |
| 544 | // Find the best division Axis |
| 545 | // float average[3] = {0,0,0}; |
| 546 | // float covariance[3] = {0,0,0}; |
| 547 | // for (i=l;i<=r;i++){ |
| 548 | // average[0] += Storage[i].x; |
| 549 | // average[1] += Storage[i].y; |
| 550 | // average[2] += Storage[i].z; |
| 551 | // } |
| 552 | // for (i=0;i<3;i++) average[i] = average[i]/(r-l+1); |
| 553 | // for (i=l;i<=r;i++){ |
| 554 | // covariance[0] += (Storage[i].x - average[0]) * (Storage[i].x - average[0]); |
| 555 | // covariance[1] += (Storage[i].y - average[1]) * (Storage[i].y - average[1]); |
| 556 | // covariance[2] += (Storage[i].z - average[2]) * (Storage[i].z - average[2]); |
| 557 | // } |
| 558 | // for (i=0;i<3;i++) covariance[i] = covariance[i]/(r-l+1); |
| 559 | // for (i = 1;i<3;i++){ |
| 560 | // if (covariance[i] > covariance[div_axis]) div_axis = i; |
| 561 | // } |
| 562 | // Select the longest dimension as division axis |
| 563 | float min_value[3] = {INFINITY, INFINITY, INFINITY}; |
| 564 | float max_value[3] = {-INFINITY, -INFINITY, -INFINITY}; |
| 565 | float dim_range[3] = {0,0,0}; |
| 566 | for (i=l;i<=r;i++){ |
| 567 | min_value[0] = min(min_value[0], Storage[i].x); |
| 568 | min_value[1] = min(min_value[1], Storage[i].y); |
| 569 | min_value[2] = min(min_value[2], Storage[i].z); |
| 570 | max_value[0] = max(max_value[0], Storage[i].x); |
| 571 | max_value[1] = max(max_value[1], Storage[i].y); |
| 572 | max_value[2] = max(max_value[2], Storage[i].z); |
| 573 | } |
| 574 | for (i=0;i<3;i++) dim_range[i] = max_value[i] - min_value[i]; |
| 575 | for (i=1;i<3;i++) if (dim_range[i] > dim_range[div_axis]) div_axis = i; |
| 576 | // Divide by the division axis and recursively build. |
| 577 | |
| 578 | (*root)->division_axis = div_axis; |
| 579 | switch (div_axis) |
| 580 | { |
| 581 | case 0: |
| 582 | nth_element(begin(Storage)+l, begin(Storage)+mid, begin(Storage)+r+1, point_cmp_x); |
| 583 | break; |
| 584 | case 1: |
| 585 | nth_element(begin(Storage)+l, begin(Storage)+mid, begin(Storage)+r+1, point_cmp_y); |
| 586 | break; |
| 587 | case 2: |
| 588 | nth_element(begin(Storage)+l, begin(Storage)+mid, begin(Storage)+r+1, point_cmp_z); |
| 589 | break; |
| 590 | default: |
| 591 | nth_element(begin(Storage)+l, begin(Storage)+mid, begin(Storage)+r+1, point_cmp_x); |
| 592 | break; |
| 593 | } |
| 594 | (*root)->point = Storage[mid]; |