/**
* 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;
}
Node* Pendulum::GetNodeToExpandRRT(){
double max_v = bounds->max_v;
double max_w = bounds->max_w;
double pt[4] = {10.0*rand()/RAND_MAX, 10.0*rand()/RAND_MAX, 10.0*rand()/RAND_MAX, 10.0*rand()/RAND_MAX};
kdres* resultsNearest = kd_nearest(nodes_tree, pt);
double pos[4];
Node* tree_node = (Node*) kd_res_item(resultsNearest, pos);
return tree_node;
}
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;
}
void BVNDFProcessor::computeDirac(){
int nbFacet = m_geom->getNbFacet();
m_dirac = new int[nbFacet];
m_bnormals = new bool[m_nbNormal];
for(int i = 0; i < m_nbNormal; ++i)
m_bnormals[i] = false;
void * tree = kd_create(3);
struct kdres *presults;
// Creation of a KdTree of all the normals
for (int i = 0; i < m_nbNormal; ++i){
void * data = malloc(sizeof(i));
memcpy(data, &i, sizeof(i));
kd_insert3f((kdtree*)tree,
m_normals[i].x,
m_normals[i].y,
m_normals[i].z,
data);
int temp;
memcpy(&temp, data, sizeof(temp));
//std::cout << temp << std::endl;
}
// For each facet we find its closest normal
for (int i = 0; i < nbFacet; ++i){
glm::vec3 normal = m_geom->getNormal(i);
presults = kd_nearest3f((kdtree*) tree, normal.x, normal.y, normal.z);
int * data;
data = (int*)kd_res_item(presults, NULL);
int normalId;
memcpy(&normalId, data, sizeof(normalId));
// Test
int closest = getClosestNormal(normal);
// Test
m_dirac[i] = normalId;
m_bnormals[normalId] = true;
}
m_corres = new int[m_nbNormal];
int currId = 0;
for (int i = 0; i < m_nbNormal; ++i){
if (m_bnormals[i]){
m_corres[i] = currId;
currId++;
}
else{
m_corres[i] = -1;
}
}
m_nbNonZeroNormals = currId;
m_diracInitialized = true;
}
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;
}
void *kd_res_item_data(struct kdres *set)
{
return kd_res_item(set, 0);
}
int RRT_kernel(double control_solution[])
{
double start_state[] = {-M_PI/2,0}; // initial state; angle position measured from x-axis
double end_state[] = {M_PI/2,0}; // goal state
double state_limits[2][2] = {{-M_PI,M_PI},{-8,8}}; // state limits; angular position between -pi & pi rad; angular velocity between -10 & 10 rad/s
// control torques to be used: linspace(-5,5,20)
double discrete_control_torques[] = {-5.0000,-4.4737,-3.9474,-3.4211,-2.8947,-2.3684,-1.8421,-1.3158,-0.7895,-0.2632,
5.0000, 4.4737, 3.9474, 3.4211, 2.8947, 2.3684, 1.8421, 1.3158, 0.7895, 0.2632};
int number_of_discrete_torques = (int)( sizeof(discrete_control_torques)/sizeof(discrete_control_torques[0]) );
double time_step = 0.02; // time interval between application of subsequent control torques
// static memory allocation
double random_state[2]; // stores a state
double next_state[2];
//double RRT_tree[NUM_OF_ITERATIONS][2] = { [0 ... NUM_OF_ITERATIONS-1] = {-M_PI/2,0} }; // graph of states in RRT; each index corresponds to a vertex (using designated initializer)
double RRT_tree[NUM_OF_ITERATIONS][2];
int x;
for(x = 0; x < NUM_OF_ITERATIONS; x++)
{
RRT_tree[x][0] = -M_PI/2;
RRT_tree[x][1] = 0;
}
int parent_state_index[NUM_OF_ITERATIONS]; // stores index of parent state for each state in graph RRT_tree
int control_action_index[NUM_OF_ITERATIONS]; // stores index of control actions in discrete_control_torques (each state will use a control action value in discrete_control_torques)
int state_index = 0; // stores sequence of states joining initial to goal state
double temp_achievable_states[number_of_discrete_torques][2]; // stores temporary achievable states from a particular vertex
double distance_square_values[NUM_OF_ITERATIONS]; // stores distance square values
void * kd_tree;
struct kdres * kd_results;
int tree_node_index[NUM_OF_ITERATIONS];
kd_tree = kd_create(2);
tree_node_index[0] = 0;
kd_insert(kd_tree, start_state, &tree_node_index[0]);
srand(time(NULL)); // initialize random number generator
double temp[2];
// keep growing RRT until goal found or run out of iterations
int iteration;
for(iteration = 1; iteration < NUM_OF_ITERATIONS; iteration++)
{
// get random state
random_state[0] = generateRandomDouble(state_limits[0][0],state_limits[0][1]);
random_state[1] = generateRandomDouble(state_limits[1][0],state_limits[1][1]);
// find vertex in RRT closest to random state
kd_results = kd_nearest(kd_tree, random_state);
int * nearest_state_ptr = (int*) kd_res_item(kd_results, temp);
int nearest_state_index = *nearest_state_ptr;
// from the closest RRT vertex, compute all the states that can be reached,
// given the pendulum dynamics and available torques
int ui;
for(ui = 0; ui < number_of_discrete_torques; ui++)
{
pendulumDynamics(RRT_tree[nearest_state_index],discrete_control_torques[ui],next_state);
temp_achievable_states[ui][0] = RRT_tree[nearest_state_index][0] + time_step*next_state[0];
temp_achievable_states[ui][1] = RRT_tree[nearest_state_index][1] + time_step*next_state[1];
}
// select the closest reachable state point
euclidianDistSquare(random_state,temp_achievable_states,number_of_discrete_torques,distance_square_values);
ui = findMin(distance_square_values,number_of_discrete_torques);
random_state[0] = temp_achievable_states[ui][0];
random_state[1] = temp_achievable_states[ui][1];
// if angular position is greater than pi rads, wrap around
if(random_state[0] > M_PI || random_state[0] < -M_PI)
random_state[0] = fmod((random_state[0]+M_PI), (2*M_PI)) - M_PI;
// link reachable state point to the nearest vertex in the tree
RRT_tree[iteration][0] = random_state[0];
RRT_tree[iteration][1] = random_state[1];
parent_state_index[iteration] = nearest_state_index;
control_action_index[iteration] = ui;
tree_node_index[iteration] = iteration;
kd_insert(kd_tree, random_state, &tree_node_index[iteration]);
if( (random_state[0] <= end_state[0]+0.05) && (random_state[0] >= end_state[0]-0.05) )
{
if( (random_state[1] <= end_state[1]+0.25) && (random_state[1] >= end_state[1]-0.25) )
{
break;
}
}
}
if(iteration == NUM_OF_ITERATIONS)
{
printf("Number of iterations: %d\n",iteration);
return 0;
} else
{
printf("Number of iterations: %d\n",iteration);
state_index = iteration;
int length_of_soln = 0;
while(state_index != 0)
{
control_solution[length_of_soln] = discrete_control_torques[ control_action_index[state_index] ];
length_of_soln++;
state_index = parent_state_index[state_index];
}
return length_of_soln;
}
}
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;
}
