struct kdres *kd_nearest(struct kdtree *kd, const double *pos) {
struct kdhyperrect *rect;
struct kdnode *result;
struct kdres *rset;
double dist_sq;
int i;
if (!kd)
return 0;
if (!kd->rect)
return 0;
/* Allocate result set */
if (!(rset = malloc(sizeof *rset))) {
return 0;
}
if (!(rset->rlist = alloc_resnode())) {
free(rset);
return 0;
}
rset->rlist->next = 0;
rset->tree = kd;
/* Duplicate the bounding hyperrectangle, we will work on the copy */
if (!(rect = hyperrect_duplicate(kd->rect))) {
kd_res_free(rset);
return 0;
}
/* Our first guesstimate is the root node */
result = kd->root;
dist_sq = 0;
for (i = 0; i < kd->dim; i++)
dist_sq += SQ(result->pos[i] - pos[i]);
/* Search for the nearest neighbour recursively */
kd_nearest_i(kd->root, pos, &result, &dist_sq, rect);
/* Free the copy of the hyperrect */
hyperrect_free(rect);
/* Store the result */
if (result) {
if (rlist_insert(rset->rlist, result, -1.0) == -1) {
kd_res_free(rset);
return 0;
}
rset->size = 1;
kd_res_rewind(rset);
return rset;
} else {
kd_res_free(rset);
return 0;
}
}
struct kdres *kd_nearest_range(struct kdtree *kd, const double *pos, double range)
{
int ret;
struct kdres *rset;
if(!(rset = new kdres)){
return 0;
}
if(!(rset->rlist = new res_node)){
delete rset;
return 0;
}
rset->tlist->next=0;
rset->tree = kd;
if((ret = find_nearest(kd->root, pos, range, rset->rlist, 0, kd->dim))==-1){
kd_res_free(rset);
return 0;
}
rset->size = rst;
kd_res_rewind(rset);
return rset;
}
int main(int argc, char **argv) {
int i, vcount = 1000000;
void *kd, *set;
unsigned int msec, start;
if (argc > 1 && isdigit(argv[1][0])) {
vcount = atoi(argv[1]);
}
printf("inserting %d random vectors... ", vcount);
fflush(stdout);
kd = kd_create(3);
start = get_msec();
for (i = 0; i < vcount; i++) {
float x, y, z;
x = ((float) rand() / RAND_MAX) * 200.0 - 100.0;
y = ((float) rand() / RAND_MAX) * 200.0 - 100.0;
z = ((float) rand() / RAND_MAX) * 200.0 - 100.0;
assert(kd_insert3(kd, x, y, z, 0) == 0);
}
msec = get_msec() - start;
printf("%.3f sec\n", (float) msec / 1000.0);
start = get_msec();
set = kd_nearest_range3(kd, 0, 0, 0, 40);
msec = get_msec() - start;
printf("range query returned %d items in %.5f sec\n", kd_res_size(set), (float) msec / 1000.0);
kd_res_free(set);
kd_free(kd);
return 0;
}
struct kdres *kd_nearest_range(struct kdtree *kd, const double *pos, double range)
{
int ret;
struct kdres *rset;
//pthread_mutex_lock(&alloc_mutex2);
rset = malloc(sizeof *rset);
//pthread_mutex_unlock(&alloc_mutex2);
if(!(rset)) {
return 0;
}
//pthread_mutex_lock(&alloc_mutex2);
rset->rlist = alloc_resnode();
//pthread_mutex_unlock(&alloc_mutex2);
if(!(rset->rlist)) {
free(rset);
return 0;
}
rset->rlist->next = 0;
rset->tree = kd;
if((ret = find_nearest(kd->root, pos, range, rset->rlist, 0, kd->dim)) == -1) {
kd_res_free(rset);
return 0;
}
rset->size = ret;
kd_res_rewind(rset);
return rset;
}
struct kdres *kd_generic_nearest_range(
struct kdtree *kd,
const double *pos,
double range,
double (*distance_function)(const double *, const double *, int k),
int (*compare_function)(double, double)
)
{
int ret;
struct kdres *rset;
if(!(rset = malloc(sizeof *rset))) {
return 0;
}
if(!(rset->rlist = alloc_resnode())) {
free(rset);
return 0;
}
rset->rlist->next = 0;
rset->tree = kd;
if((ret = find_nearest(kd->root, pos, range, rset->rlist, 0, kd->dim, distance_function, compare_function)) == -1) {
kd_res_free(rset);
return 0;
}
rset->size = ret;
kd_res_rewind(rset);
return rset;
}
int ClosedPolygon::insertIndexedPoint(const Vec3d &p)
{
int vIndex = -1;
kdres * findP = points.nearest3f(p.x(), p.y(), p.z());
if(findP)
{
double * pos = findP->riter->item->pos;
Vec3d closestPoint(pos[0], pos[1], pos[2]);
double dist = (closestPoint - p).norm();
if(dist < closedPolyEpsilon)
{
vIndex = findP->riter->item->index;
kd_res_free(findP);
return vIndex;
}
}
vIndex = lastVertexIndex;
allPoints[vIndex] = p;
points.insert3f(p.x(), p.y(), p.z(), lastVertexIndex++);
return vIndex;
}
/**
* vu_get_tz_at_location:
*
* @vc: Position for which the time zone is desired
*
* Returns: TimeZone string of the nearest known location. String may be NULL.
*
* Use the k-d tree method (http://en.wikipedia.org/wiki/Kd-tree) to quickly retreive
* the nearest location to the given position.
*/
gchar* vu_get_tz_at_location ( const VikCoord* vc )
{
gchar *tz = NULL;
if ( !vc || !kd )
return tz;
struct LatLon ll;
vik_coord_to_latlon ( vc, &ll );
double pt[2] = { ll.lat, ll.lon };
gdouble nearest;
if ( !a_settings_get_double(VIK_SETTINGS_NEAREST_TZ_FACTOR, &nearest) )
nearest = 1.0;
struct kdres *presults = kd_nearest_range ( kd, pt, nearest );
while( !kd_res_end( presults ) ) {
double pos[2];
gchar *ans = (gchar*)kd_res_item ( presults, pos );
// compute the distance of the current result from the pt
double dist = sqrt( dist_sq( pt, pos, 2 ) );
if ( dist < nearest ) {
//printf( "NEARER node at (%.3f, %.3f, %.3f) is %.3f away is %s\n", pos[0], pos[1], pos[2], dist, ans );
nearest = dist;
tz = ans;
}
kd_res_next ( presults );
}
g_debug ( "TZ lookup found %d results - picked %s", kd_res_size(presults), tz );
kd_res_free ( presults );
return tz;
}
int main(int argc, char **argv) {
int i, num_pts = DEF_NUM_PTS;
void *ptree;
char *data, *pch;
struct kdres *presults;
double pos[3], dist;
double pt[3] = { 0, 0, 1 };
double radius = 10;
if(argc > 1 && isdigit(argv[1][0])) {
num_pts = atoi(argv[1]);
}
if(!(data = malloc(num_pts))) {
perror("malloc failed");
return 1;
}
srand( time(0) );
/* create a k-d tree for 3-dimensional points */
ptree = kd_create( 3 );
/* add some random nodes to the tree (assert nodes are successfully inserted) */
for( i=0; i<num_pts; i++ ) {
data[i] = 'a' + i;
assert( 0 == kd_insert3( ptree, rd(), rd(), rd(), &data[i] ) );
}
/* find points closest to the origin and within distance radius */
presults = kd_nearest_range( ptree, pt, radius );
/* print out all the points found in results */
printf( "found %d results:\n", kd_res_size(presults) );
while( !kd_res_end( presults ) ) {
/* get the data and position of the current result item */
pch = (char*)kd_res_item( presults, pos );
/* compute the distance of the current result from the pt */
dist = sqrt( dist_sq( pt, pos, 3 ) );
/* print out the retrieved data */
printf( "node at (%.3f, %.3f, %.3f) is %.3f away and has data=%c\n",
pos[0], pos[1], pos[2], dist, *pch );
/* go to the next entry */
kd_res_next( presults );
}
/* free our tree, results set, and other allocated memory */
free( data );
kd_res_free( presults );
kd_free( ptree );
return 0;
}
// Finds the set of nodes with max cost
GSList *opttree_find_nodes_in_ball (opttree_t *self, state_t *state, double ball_radius) {
GSList *nodes_in_ball = NULL;
kdres_t *kdres = kd_nearest_range (self->kdtree, optsystem_get_state_key (self->optsys, state), ball_radius);
nodes_in_ball = opttree_kdtree_to_gslist (state, kdres);
kd_res_free (kdres);
return nodes_in_ball;
}
// Find the nearest neighbor in the tree
node_t* opttree_find_nearest_neighbor (opttree_t *self, state_t *state_from) {
node_t *min_node = NULL;
kdres_t *kdres = kd_nearest (self->kdtree, optsystem_get_state_key (self->optsys, state_from));
if (kd_res_end (kdres)) {
printf ("ERROR: No nearest neighbors\n");
exit(1);
}
min_node = kd_res_item_data (kdres);
kd_res_free (kdres);
return min_node;
}
PetscReal ExodusModel::getMaterialParameterAtPoint(const std::vector<double> point,
const std::string parameter_name) {
// Ensure dimensions are consistent.
assert(point.size() == mNumberDimension);
// Get spatial index.
kdres *set = kd_nearest(mKdTree, point.data());
auto spatial_index = *(int *) kd_res_item_data(set);
kd_res_free(set);
// Get parameter index.
int i = 0;
int parameter_index;
for (auto &name: mNodalVariableNames) { if (name == parameter_name) parameter_index = i; i++; }
return mNodalVariables[parameter_index * mNumberNodalVariables + spatial_index];
}
int pixel_compare_NN(double epsilon,void* source, void *kd, double* pixel_position, PixelWand* color, PixelWand* neigh_color)
{
struct kdres *neigh;
double neigh_position[2]= {0,0};
neigh = kd_nearest(kd,pixel_position);
if(neigh == NULL)
return 1;
kd_res_item(neigh, neigh_position);
kd_res_free(neigh);//need to free the memory used for the query
MagickGetImagePixelColor(source,neigh_position[0],neigh_position[1],neigh_color);
MagickGetImagePixelColor(source,pixel_position[0],pixel_position[1],color);
double color_diff =0;
color_diff += pow(PixelGetRed(color) - PixelGetRed(neigh_color),2);
color_diff += pow(PixelGetGreen(color) - PixelGetGreen(neigh_color),2);
color_diff += pow(PixelGetBlue(color) - PixelGetBlue(neigh_color),2);
if(color_diff < epsilon)
return 0;
return 1;
}
int32_t PhotonMap::SelectNNearestPhotons(kdtree *map, PhotonPos *pp, const Vector &pos, float radius, int32_t n)
{
kdres *result = kd_nearest_range3f(map, pos.x, pos.y, pos.z, radius);
Vector ppos;
int nNow = 0;
while ( !kd_res_end(result) ) {
Photon *photon = (Photon *) kd_res_itemf(result, &(ppos.x));
float d = (ppos - pos).LengthSquared();
if ( nNow < n ) {
pp[nNow++] = PhotonPos(photon, ppos, d);
if ( nNow == n ) make_heap(pp, pp + n);
}
else {
pop_heap(pp, pp + n);
pp[n - 1] = PhotonPos(photon, ppos, d);
push_heap(pp, pp + n);
}
kd_res_next(result);
}
kd_res_free(result);
return nNow;
}
int main(int argc,char **argv) {
unsigned int width, height,seed;
double pos[2] = {0,0},target_pos[2], diff[3],min_col_diff;
struct kdres *neigh; void *kd; //kd-tree
int i,x,y,c,num_cells=500,random;
size_t row_size;//imagemagick wants this
char source_filename[50] = "lisa.png";
char dest_filename[59];//I set to 59 so user could have 49 chars + 10 in the default case of NNNNNN-sourcefilename
int oflag=0,sflag=0,vflag=0,cflag=0;
//imagemagick stuff
MagickWand *source,*dest;
PixelWand *neigh_color,*color,**pmw;
PixelIterator *imw;
while((c = getopt(argc,argv,"vc:n:s:r:d:")) != -1) {
switch (c) {
case 'v':
vflag=1;
break;
case 'n':
num_cells=atoi(optarg);
break;
case 's':
strcpy(source_filename,optarg);
break;
case 'd':
oflag=1;
strcpy(dest_filename,optarg);
break;
case 'r':
if((seed=atoi(optarg))!=0)
sflag=1;
break;
case 'c':
cflag=1;
min_col_diff = atof(optarg);
break;
default:
printf("Option %s not recognized and ignored.\n",c);
}
}
if(!oflag) sprintf(dest_filename,"%d-%s",num_cells,source_filename);
MagickWandGenesis();
source=NewMagickWand();
dest = NewMagickWand();
color = NewPixelWand();
neigh_color = NewPixelWand();
pmw = NewPixelWand();
MagickReadImage(source,source_filename);
if (source==MagickFalse) {
printf("Error reading file. Usage: vor filename\n");
return 1;
}
width = MagickGetImageWidth(source);
height = MagickGetImageHeight(source);
printf("File has width %d and height %d\n", width, height);
if(!sflag) { //seed the algorithm with /dev/random if a seed wasn't specified
random = open("/dev/random", 'r');
read(random, &seed, sizeof (seed));
close(random);
}
if(vflag) printf("seed : %d\n",seed);
srand(seed);
kd = kd_create(2);
if(cflag)
{
for(i = 0; i < num_cells; i++) {
pos[0]= (double)random_in_range(0,width);
pos[1]= (double)random_in_range(0,height);
if(pixel_compare_NN(min_col_diff,source,kd,pos,color,neigh_color))
kd_insert(kd,pos,0);
}
}
else {
for(i = 0; i < num_cells; i++) {
pos[0]= (double)random_in_range(0,width);
pos[1]= (double)random_in_range(0,height);
kd_insert(kd,pos,0);
}
}
MagickSetSize(dest,width,height);
MagickReadImage(dest,"xc:none");
imw = NewPixelIterator(dest);
for (y=0; y < height; y++) {
pos[1] = y;
pmw = PixelGetNextIteratorRow(imw, &row_size); //we iterate through the rows, grabbing one at a time
for (x=0; x < (long) width; x++) {
pos[0] =x;
neigh = kd_nearest(kd,pos);//this is the query
kd_res_item(neigh, target_pos);//then we pull out the result into target_pos
kd_res_free(neigh);//need to free the memory used for the query
MagickGetImagePixelColor(source,target_pos[0],target_pos[1],color);
PixelSetColorFromWand(pmw[x],color);
}
PixelSyncIterator(imw);//this will write to the image (MagickWand)
}
if(vflag)printf("Writing to file %s.\n",dest_filename);
if(MagickWriteImage(dest,dest_filename)==MagickFalse)
{
printf("Error writing to file %s.\n",dest_filename);
}
source=DestroyMagickWand(source);
dest=DestroyMagickWand(dest);
MagickWandTerminus();
kd_free(kd);
return 0;
}
void Discontinuity::lookCrust ( Mesh &msh, double &mshCol, double &mshLon,
double &mshRad, int &mshInd, bool &checkCrust,
bool &smoothCrust, double &upTap,
double &downTap, Model_file &mod )
{
Constants con;
double rho, vpv;
if ( mshRad > (con.R_EARTH - 100) )
{
kdres *set = kd_nearest3 ( crustTree, mshCol, mshLon, con.R_EARTH );
void *ind = kd_res_item_data ( set );
int point = * ( int * ) ind;
kd_res_free (set);
/* The moho is defined in a weird way ( depth from sea level if in the
ocean, and depth from elevation if in the crust). First, convert crust
elevation to km, and then decided whether we're taking the sea level or
or crustial surface as reference */
double ref;
if ( msh.elv[mshInd] <= 0. )
{
ref = con.R_EARTH;
}
else
{
ref = con.R_EARTH + msh.elv[mshInd] / 1000.;
}
// Do a bilinear interpolation on the Depth.
double interpDep;
double interpVs;
getCrustDepth ( mshCol, mshLon, point, interpDep, "dep" );
getCrustDepth ( mshCol, mshLon, point, interpVs, "vel" );
/* Here get crustal thickness for tapering purposes (differs if we're in)
oceanic of continental crust. This is the thickness of the crust, minus an
ocean layer. */
double crustThick = 0.;
if ( msh.elv[mshInd] <= 0. )
{
crustThick = interpDep + ( msh.elv[mshInd] );
}
else
{
crustThick = interpDep;
}
/* Figure out whether we need to smooth the crust */
smoothCrust = false;
smoothCosine ( crustThick, interpDep, mshRad, downTap, upTap, smoothCrust );
/* If we're in the crust */
// This used to say if mshRad >= (ref - interpDep) as well.
if ( mod.intentions == "CRUST" )
{
double crust_vsv = interpVs - con.aniCorrection;
double crust_vsh = interpVs;
// Scaling from isotropic vs to rho (Fichtner, multiscale)
rho = 0.2277 * crust_vsh + 2.016;
// Scaling from vs to vp
vpv = 1.5399 * crust_vsh + 0.840;
double N = rho * crust_vsh * crust_vsh;
double L = rho * crust_vsv * crust_vsv;
double A = rho * vpv * vpv;
double S = A - 2 * N;
double F = A - 2 * L;
checkCrust = true;
msh.c11[mshInd] = upTap * A + downTap * msh.c11[mshInd];
msh.c22[mshInd] = upTap * A + downTap * msh.c22[mshInd];
msh.c33[mshInd] = upTap * A + downTap * msh.c33[mshInd];
msh.c12[mshInd] = upTap * F + downTap * msh.c12[mshInd];
msh.c13[mshInd] = upTap * F + downTap * msh.c13[mshInd];
msh.c23[mshInd] = upTap * S + downTap * msh.c23[mshInd];
msh.c44[mshInd] = upTap * N + downTap * msh.c44[mshInd];
msh.c55[mshInd] = upTap * L + downTap * msh.c55[mshInd];
msh.c66[mshInd] = upTap * L + downTap * msh.c66[mshInd];
msh.rho[mshInd] = upTap * rho + downTap * msh.rho[mshInd];
}
if ( mshRad >= (ref - interpDep) )
{
checkCrust = true;
}
}
if ( mshRad <= con.R_EARTH - 100 )
{
upTap = 0.;
downTap = 1.;
}
}
void Discontinuity::getCrustDepth ( double &mshCol, double &mshLon, int &point,
double &par, std::string mode )
{
Constants con;
// Stride of one degree in crust07.
int degStri=2;
// Initializations.
int lonDir = 0;
int colDir = 0;
// Create vector to hold 4 nodes of smoothing square.
std::vector <int> nodes;
nodes.reserve (4);
// Determine on which side of the requested col. the closest point is.
if ( mshCol > crust_col_deg_unpack[point] )
colDir = 1;
if ( mshCol <= crust_col_deg_unpack[point] )
colDir = -1;
// Determine on which side of the requested lon. the closest point is.
if ( mshLon > crust_lon_deg_unpack[point] )
lonDir = 1;
if ( mshLon <= crust_lon_deg_unpack[point] )
lonDir = -1;
// Create the KDTree requests for four points of the interpolating square.
double col1 = crust_col_deg_unpack[point];
double col2 = crust_col_deg_unpack[point] + colDir * degStri;
double lon1 = crust_lon_deg_unpack[point];
double lon2 = crust_lon_deg_unpack[point] + lonDir * degStri;
// Fix col. wrapping.
if ( col2 > 180 )
col2 = 180 - ( col1 - 180 );
if ( col2 < 0 )
col2 = 0;
// Fix lon. wrapping.
if ( lon2 > 180 )
lon2 = lon2 - 360.;
if ( lon2 < -180 )
lon2 = lon2 + 360.;
// Find the four points of the rectangle in spherical space.
kdres *set1 = kd_nearest3 ( crustTree, col1, lon1, con.R_EARTH );
kdres *set2 = kd_nearest3 ( crustTree, col1, lon2, con.R_EARTH );
kdres *set3 = kd_nearest3 ( crustTree, col2, lon1, con.R_EARTH );
kdres *set4 = kd_nearest3 ( crustTree, col2, lon2, con.R_EARTH );
// Extract the indices from the result structures.
void *ind1 = kd_res_item_data ( set1 );
void *ind2 = kd_res_item_data ( set2 );
void *ind3 = kd_res_item_data ( set3 );
void *ind4 = kd_res_item_data ( set4 );
// Build the square based on the four possible quadrants.
if ( colDir == 1 && lonDir == (-1) )
{
nodes[0] = * ( int * ) ind2;
nodes[1] = * ( int * ) ind1;
nodes[2] = * ( int * ) ind3;
nodes[3] = * ( int * ) ind4;
}
else if ( colDir == 1 && lonDir == 1 )
{
nodes[0] = * ( int * ) ind1;
nodes[1] = * ( int * ) ind2;
nodes[2] = * ( int * ) ind4;
nodes[3] = * ( int * ) ind3;
}
else if ( colDir == (-1) && lonDir == 1 )
{
nodes[0] = * ( int * ) ind3;
nodes[1] = * ( int * ) ind4;
nodes[2] = * ( int * ) ind2;
nodes[3] = * ( int * ) ind1;
}
else if ( colDir == (-1) && lonDir == (-1) )
{
nodes[0] = * ( int * ) ind4;
nodes[1] = * ( int * ) ind3;
nodes[2] = * ( int * ) ind1;
nodes[3] = * ( int * ) ind2;
}
// Deallocate memory for results.
kd_res_free ( set1 );
kd_res_free ( set2 );
kd_res_free ( set3 );
kd_res_free ( set4 );
// Conditionally build the weights depending on the quandrants selected above.
double t = 0;
double u = 0;
if ( colDir == 1 )
t = ( mshCol - col1 ) / ( col2 - col1 );
if ( colDir == (-1) )
t = ( mshCol - col2 ) / ( col1 - col2 );
if ( lonDir == 1 )
u = ( mshLon - lon1 ) / ( lon2 - lon1 );
if ( lonDir == (-1) )
u = ( mshLon - lon2 ) / ( lon1 - lon2 );
/* Get the bilinearly interpolated value. Inspired by Numerical recipes,
but with a swwitched axis. */
if ( mode == "dep" )
{
par = (1 - t) * (1 - u) * crust_dp[0][nodes[0]] +
t * (1 - u) * crust_dp[0][nodes[3]] +
t * u * crust_dp[0][nodes[2]] +
(1 - t) * u * crust_dp[0][nodes[1]];
}
if ( mode == "vel" )
{
par = (1 - t) * (1 - u) * crust_vs[0][nodes[0]] +
t * (1 - u) * crust_vs[0][nodes[3]] +
t * u * crust_vs[0][nodes[2]] +
(1 - t) * u * crust_vs[0][nodes[1]];
}
#ifdef VISUAL_DEBUG
std::ofstream myfile;
myfile.open ( "surfInterp.txt", std::ios::out );
myfile << crust_col_deg_unpack[nodes[0]] << " "
<< crust_lon_deg_unpack[nodes[0]] << " " << crust_dp[0][nodes[0]]
<< std::endl;
myfile << crust_col_deg_unpack[nodes[1]] << " "
<< crust_lon_deg_unpack[nodes[1]] << " " << crust_dp[0][nodes[1]]
<< std::endl;
myfile << crust_col_deg_unpack[nodes[2]] << " "
<< crust_lon_deg_unpack[nodes[2]] << " " << crust_dp[0][nodes[2]]
<< std::endl;
myfile << crust_col_deg_unpack[nodes[3]] << " "
<< crust_lon_deg_unpack[nodes[3]] << " " << crust_dp[0][nodes[3]]
<< std::endl;
myfile << mshCol << " " << mshLon << " " << dep
<< std::endl;
std::cout << colDir << " " << lonDir << std::endl;
std::cout << mshCol << " " << col1 << " " << col2 << " " << std::endl;
std::cout << mshLon << " " << lon1 << " " << lon2 << " " << std::endl;
std::cout << t << " " << u << std::endl;
std::cout << crust_dp[0][nodes[0]] << " " << crust_dp[0][nodes[1]] << " "
<< crust_dp[0][nodes[2]] << " " << crust_dp[0][nodes[3]] << std::endl;
std::cout << dep << std::endl;
myfile.close();
std::cin.get();
std::cout << colDir << " " << lonDir << std::endl;
std::cout << mshCol << " " << col1 << " " << col2 << " " << std::endl;
std::cout << mshLon << " " << lon1 << " " << lon2 << " " << std::endl;
std::cout << t << " " << u << std::endl;
std::cout << crust_dp[0][nodes[0]] << " " << crust_dp[0][nodes[1]] << " "
<< crust_dp[0][nodes[2]] << " " << crust_dp[0][nodes[3]] << std::endl;
std::cout << dep << std::endl;
#endif
}
