struct kdres *kd_nearest2(struct kdtree *tree, double x, double y)
{
double pos[2];
pos[0] = x;
pos[1] = y;
return kd_nearest(tree, pos);
}
struct kdres *kd_nearestf(struct kdtree *tree, const float *pos)
{
static double sbuf[16];
double *bptr, *buf = 0;
int dim = tree->dim;
struct kdres *res;
if(dim > 16) {
#ifndef NO_ALLOCA
if(dim <= 256)
bptr = buf = alloca(dim * sizeof *bptr);
else
#endif
if(!(bptr = buf = malloc(dim * sizeof *bptr))) {
return 0;
}
} else {
bptr = buf = sbuf;
}
while(dim-- > 0) {
*bptr++ = *pos++;
}
res = kd_nearest(tree, buf);
#ifndef NO_ALLOCA
if(tree->dim > 256)
#else
if(tree->dim > 16)
#endif
free(buf);
return res;
}
int main(int argc, char* argv[])
{
int nt, nd;
float dist;
float **points, *point;
kd_node tree, near;
sf_file inp, out;
sf_init(argc,argv);
inp = sf_input("in");
out = sf_output("out");
if (SF_FLOAT != sf_gettype(inp)) sf_error("Need float input");
if (!sf_histint(inp,"n1",&nd)) sf_error("No n1= in input");
if (!sf_histint(inp,"n2",&nt)) sf_error("No n2= in input");
sf_putint(out,"n2",1);
points = sf_floatalloc2(nd,nt);
sf_floatread(points[0],nd*nt,inp);
tree = kd_tree(points,nt,nd);
point = sf_floatalloc(nd);
if (!sf_getfloats("point",point,nd)) sf_error("Need point=");
dist = SF_HUGE;
kd_nearest(tree, point, 0, nd, &near, &dist);
sf_floatwrite(kd_coord(near),nd,out);
exit(0);
}
struct kdres *kd_nearest3f(struct kdtree *tree, float x, float y, float z) {
double pos[3];
pos[0] = x;
pos[1] = y;
pos[2] = z;
return kd_nearest(tree, pos);
}
struct kdres *kd_nearest3(struct kdtree *tree, double x, double y, double z) {
double pos[3];
pos[0] = x;
pos[1] = y;
pos[2] = z;
return kd_nearest(tree, pos);
}
/**
* @function getNearestNeighbor
*/
int JG_RRT::getNearestNeighbor( const Eigen::VectorXd &qsample )
{
struct kdres* result = kd_nearest( kdTree, qsample.data() );
uintptr_t nearest = (uintptr_t) kd_res_item_data( result );
activeNode = nearest;
return nearest;
}
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;
}
// 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;
}
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;
}
