t_float median_calculate(t_float * array, int begin, int end){
median_sort(array + begin, end - begin + 1);
int qtd = end - begin + 1;
t_float median = 0;
median = (qtd%2 == 1)?
array[begin + ((qtd - 1)/2)] :
(array[begin + qtd / 2 - 1] + array[begin + qtd / 2])/2.0f;
return median;
}
void median_sort (t_float *a, int n) {
if (n < 2)
return;
t_float p = a[n / 2];
t_float *l = a;
t_float *r = a + n - 1;
while (l <= r) {
while (*l < p)
l++;
while (*r > p)
r--;
if (l <= r) {
t_float t = *l;
*l++ = *r;
*r-- = t;
}
}
median_sort(a, r - a + 1);
median_sort(l, a + n - l);
}
void KDTree_CreateTree(TKDTree* t)
{
unsigned int i;
int next_node, stack_ptr, min, mid, max, parent, cut_direction;
double width, max_width;
int* stack;
int* idx;
assert(t);
/* If the tree is already built, we don't have to do anything */
if(t->tree_built)
return;
/* If there are no elements in the tree, we don't have to do anything */
if(t->elements_num > 0) {
/* Allocate the k-D tree memory */
t->tree_size = 2 * t->elements_num;
t->tree_safety_boxes = MALLOC(t->tree_size, TBounds);
t->tree_link = MALLOC(t->tree_size, int);
/* Create and initialize temporary arrays */
next_node = 0;
stack_ptr = 0;
stack = MALLOC(3 * t->tree_size, int);
idx = MALLOC(t->elements_num, int);
for (i = 0; (int)i < t->elements_num; i++) {
idx[i] = i;
}
/* Setup the root node of the tree and put it on the stack */
stack[stack_ptr++] = 0; /* Node Number in the Tree */
stack[stack_ptr++] = 0; /* Element Span Minumum */
stack[stack_ptr++] = t->elements_num - 1; /* Element Span Maximum */
Bounds_Copy(&(t->extent), &(t->tree_safety_boxes[0]));
next_node++;
/* Construct k-D tree by setting up each pair of child nodes */
while (stack_ptr) {
/* Pop the top entry off the stack */
max = stack[--stack_ptr];
min = stack[--stack_ptr];
parent = stack[--stack_ptr];
/* If the current node should be a leaf node, make it one */
if ((max - min) == 0) {
Bounds_Copy(&(t->elements[idx[min]]), &(t->tree_safety_boxes[parent]));
t->tree_link[parent] = - idx[min];
continue;
}
/* Select optimum cutting direction for the parent node's safety box */
cut_direction = -1;
max_width = NEGATIVE_INFINITY;
for (i = 0; i < 2; i++) {
width = Bounds_WidthAxis(&(t->tree_safety_boxes[parent]), i);
if(width > max_width) {
max_width = width;
cut_direction = i;
}
}
assert(cut_direction >= 0);
/* Do a median sort of the elements under the parent node. The sort key
is the center point of the element bounding boxes along the selected
cutting direction. */
mid = (min + max) /2;
median_sort(t, (unsigned int)cut_direction, mid - min, max - min + 1, &(idx[min]));
/* Give the parent a reference to its two children */
t->tree_link[parent] = next_node;
/* Add the "left" child to the tree and the stack */
stack[stack_ptr++] = next_node; /* Node Number in the Tree */
stack[stack_ptr++] = min; /* Element Span Minimum */
stack[stack_ptr++] = mid; /* Element Span Maximum */
Bounds_Infinite(&(t->tree_safety_boxes[next_node]));
for (i = min; (int)i <= mid; i++) {
Bounds_AddBounds(&(t->tree_safety_boxes[next_node]),
&(t->elements[idx[i]]));
}
next_node++;
/* Add the "right" child to the tree and the stack */
stack[stack_ptr++] = next_node; /* Node Number in the Tree */
stack[stack_ptr++] = mid + 1; /* Element Span Minimum */
stack[stack_ptr++] = max; /* Element Span Maximum */
Bounds_Infinite(&(t->tree_safety_boxes[next_node]));
for (i = min + 1; (int)i <= max; i++) {
Bounds_AddBounds(&(t->tree_safety_boxes[next_node]),
&(t->elements[idx[i]]));
}
next_node++;
}
/* Destroy the temporary arrays */
FREE(stack);
FREE(idx);
}
