128 lines
3.1 KiB
C
128 lines
3.1 KiB
C
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
#include <sys/time.h>
|
|
#include <stdint.h>
|
|
#include <errno.h>
|
|
#include <assert.h>
|
|
#include <stdbool.h>
|
|
|
|
#include "record.h"
|
|
#include "euclidean_distance.h"
|
|
#include "coord_query.h"
|
|
|
|
struct record* closest_point;
|
|
double closest_point_distance;
|
|
|
|
int lon_comp_func(const void* a, const void* b) {
|
|
return (
|
|
((struct record*)a)->lon - ((struct record*)b)->lon
|
|
);
|
|
}
|
|
|
|
int lat_comp_func(const void* a, const void* b) {
|
|
return (
|
|
((struct record*)a)->lat - ((struct record*)b)->lat
|
|
);
|
|
}
|
|
|
|
struct node {
|
|
struct record* record;
|
|
double point[2];
|
|
int axis;
|
|
struct node* left;
|
|
struct node* right;
|
|
};
|
|
|
|
struct node* kdtree(struct record* points, int depth, int n) {
|
|
int axis = depth % 2;
|
|
|
|
if (axis == 0) {
|
|
qsort(points, n, sizeof(struct record), lon_comp_func);
|
|
} else {
|
|
qsort(points, n, sizeof(struct record), lat_comp_func);
|
|
}
|
|
struct record* median = &points[n / 2];
|
|
struct node* new_node = malloc(sizeof(struct node));
|
|
new_node->record = median;
|
|
new_node->point[0] = median->lon;
|
|
new_node->point[1] = median->lat;
|
|
new_node->axis = axis;
|
|
if (n/2 != 0) {
|
|
new_node->left = kdtree(points, depth+1, n/2);
|
|
} else {
|
|
new_node->left = NULL;
|
|
}
|
|
if (n/2 + 1 <= n - 1) {
|
|
new_node->right = kdtree(&points[n/2 + 1], depth+1, n/2 - 1 + (n % 2));
|
|
} else {
|
|
new_node->right = NULL;
|
|
}
|
|
|
|
return new_node;
|
|
}
|
|
|
|
struct node* mk_kdtree(struct record* rs, int n) {
|
|
struct node* data = kdtree(rs, 0, n);
|
|
|
|
return data;
|
|
}
|
|
|
|
void free_kdtree(struct node* data) {
|
|
if (data == NULL) {
|
|
return;
|
|
}
|
|
|
|
struct node* left = data->left;
|
|
struct node* right = data->right;
|
|
free(data);
|
|
|
|
free_kdtree(left);
|
|
free_kdtree(right);
|
|
}
|
|
|
|
void lookup(double query[2], struct node* tree) {
|
|
if (tree == NULL) {
|
|
return;
|
|
}
|
|
|
|
double compare_coord[2] = {tree->point[0], tree->point[1]};
|
|
double dist = calc_euclidean(query, compare_coord);
|
|
|
|
double closest_coords[2] = {closest_point->lon, closest_point->lat};
|
|
closest_point_distance = calc_euclidean(query, closest_coords);
|
|
if (dist < closest_point_distance) {
|
|
closest_point = tree->record;
|
|
}
|
|
|
|
double diff = tree->point[tree->axis] - query[tree->axis];
|
|
|
|
if (diff >= 0 || closest_point_distance > fabs(diff)) {
|
|
lookup(query, tree->left);
|
|
}
|
|
|
|
if (diff <= 0 || closest_point_distance > fabs(diff)) {
|
|
lookup(query, tree->right);
|
|
}
|
|
|
|
return;
|
|
}
|
|
|
|
const struct record* lookup_kdtree(struct node *data, double lon, double lat) {
|
|
closest_point = data->record;
|
|
double query[2] = {lon, lat};
|
|
double compare_coord[2] = {data->point[0], data->point[1]};
|
|
closest_point_distance = calc_euclidean(query, compare_coord);
|
|
|
|
lookup(query, data);
|
|
|
|
return closest_point;
|
|
}
|
|
|
|
int main(int argc, char** argv) {
|
|
return coord_query_loop(argc, argv,
|
|
(mk_index_fn)mk_kdtree,
|
|
(free_index_fn)free_kdtree,
|
|
(lookup_fn)lookup_kdtree);
|
|
}
|