int kd_insert3f(struct kdtree *tree, float x, float y, float z, void *data) {
double buf[3];
buf[0] = x;
buf[1] = y;
buf[2] = z;
return kd_insert(tree, buf, data);
}
static void load_ll_tz_dir ( const gchar *dir )
{
gchar *lltz = g_build_filename ( dir, "latlontz.txt", NULL );
if ( g_access(lltz, R_OK) == 0 ) {
gchar buffer[4096];
long line_num = 0;
FILE *ff = g_fopen ( lltz, "r" );
while ( fgets ( buffer, 4096, ff ) ) {
line_num++;
gchar **components = g_strsplit (buffer, " ", 3);
guint nn = g_strv_length ( components );
if ( nn == 3 ) {
double pt[2] = { g_ascii_strtod (components[0], NULL), g_ascii_strtod (components[1], NULL) };
gchar *timezone = g_strchomp ( components[2] );
if ( kd_insert ( kd, pt, timezone ) )
g_critical ( "Insertion problem of %s for line %ld of latlontz.txt", timezone, line_num );
// NB Don't free timezone as it's part of the kdtree data now
g_free ( components[0] );
g_free ( components[1] );
} else {
g_warning ( "Line %ld of latlontz.txt does not have 3 parts", line_num );
}
g_free ( components );
}
fclose ( ff );
}
g_free ( lltz );
}
int kd_insertf(struct kdtree *tree, const float *pos, void *data)
{
static double sbuf[16];
double *bptr, *buf = 0;
int res, dim = tree->dim;
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 -1;
}
} else {
bptr = buf = sbuf;
}
while(dim-- > 0) {
*bptr++ = *pos++;
}
res = kd_insert(tree, buf, data);
#ifndef NO_ALLOCA
if(tree->dim > 256)
#else
if(tree->dim > 16)
#endif
free(buf);
return res;
}
int kd_insert3(struct kdtree *tree, double x, double y, double z, void *data) {
double buf[3];
buf[0] = x;
buf[1] = y;
buf[2] = z;
return kd_insert(tree, buf, data);
}
int kd_insert2(struct kdtree *tree, double x, double y, void *data)
{
double pos[2];
pos[0] = x;
pos[1] = y;
return kd_insert(tree, pos, data);
}
int kd_insert3(struct kdtree *tree, double x, double y, double z, void *data)
{
double pos[3];
pos[0] = x;
pos[1] = y;
pos[2] = z;
return kd_insert(tree, pos, data);
}
void ExodusModel::createKdTree() {
// Need to keep the index arrays allocated.
mKdTreeData.resize(mNodalX.size());
// Create.
if (mNumberDimension == 2) {
mKdTree = kd_create(2);
for (auto i = 0; i < mNumberVertices; i++) {
mKdTreeData[i] = i;
kd_insert(mKdTree, std::vector<double> {mNodalX[i], mNodalY[i]}.data(), &mKdTreeData[i]);
}
} else {
mKdTree = kd_create(3);
for (auto i = 0; i < mNumberVertices; i++) {
mKdTreeData[i] = i;
kd_insert(mKdTree, std::vector<double> {mNodalX[i], mNodalY[i], mNodalZ[i]}.data(), &mKdTreeData[i]);
}
}
}
/**
* @function addNode
*/
int JG_RRT::addNode( const Eigen::VectorXd &qnew, int parent_ID)
{
double goalDist = wsDistance( qnew );
configVector.push_back( qnew );
parentVector.push_back( parent_ID );
goalDistVector.push_back( goalDist );
uintptr_t ID = configVector.size() - 1;
kd_insert( kdTree, qnew.data(), (void*)ID );
add_Ranking( ID );
activeNode = ID;
return ID;
}
/**
* @function addNode
* @brief Add _qnew to tree with parent _parentId
* @return id of node added
*/
int B1RRT::addNode( const Eigen::VectorXd &_qnew, int _parentId )
{
/// Update graph vectors -- what does this mean?
configVector.push_back( _qnew );
parentVector.push_back( _parentId );
uintptr_t id = configVector.size() - 1;
kd_insert( kdTree, _qnew.data(), (void*) id ); //&idVector[id]; /// WTH? -- ahq
activeNode = id;
/// Add node to ranking
Eigen::VectorXd entry(3);
entry << id, HeuristicCost( id, targetPose ), 0;
pushRanking( entry );
return id;
}
/*
Pendulum constructor
start = the starting state,
goal = the end state (x, v, and theta)
*/
Pendulum::Pendulum(Node* start, Point goal) {
b = goal;
LoadBounds();
LoadObstacles();
root = new Node(start);
nodes.push_back(root);
nodes_tree = kd_create(4);
double pt[4] = {root->x*10.0/(bounds->ux-bounds->lx), root->v*10.0/(2*bounds->max_v)+5.0, root->theta*10.0/(6.28318530718), root->w*10.0/(2*bounds->max_w)+5.0};
if (kd_insert(nodes_tree, pt, root) != 0) {
cout << "didn't insert root successfully\n";
}
srand(time(NULL));
}
int opttree_reinitialize (opttree_t *self) {
// Clear the tree
GSList *node_curr_list = self->list_nodes;
while (node_curr_list) {
node_t *node_curr = node_curr_list->data;
opttree_free_node (self, node_curr);
node_curr_list = g_slist_next (node_curr_list);
}
g_slist_free (self->list_nodes);
self->list_nodes = NULL;
// Reinitialize the basics
self->lower_bound = DBL_MAX;
self->lower_bound_node = NULL;
// Reinitialize the kdtree
kd_clear (self->kdtree);
// Initialize the root node
self->root = opttree_new_node (self);
optsystem_get_initial_state (self->optsys, self->root->state);
self->root->distance_from_root = 0.0;
self->root->distance_from_parent = 0.0;
kd_insert (self->kdtree, optsystem_get_state_key (self->optsys, self->root->state), self->root);
// Initialize the list of all nodes
self->list_nodes = NULL;
self->list_nodes = g_slist_prepend (self->list_nodes, (gpointer) (self->root));
self->num_nodes = 1;
return 1;
}
opttree_t* opttree_create () {
opttree_t *self = (opttree_t *) calloc (sizeof (opttree_t), 1);
// Set up the dynamical system
self->optsys = (optsystem_t *) calloc (sizeof (optsystem_t), 1);
optsystem_new_system (self->optsys);
// Initialize the kdtree
self->kdtree = kd_create (optsystem_get_num_states(self->optsys));
// Set the lower bound to infinity
self->lower_bound = DBL_MAX;
self->lower_bound_node = NULL;
// Initialize the root node
self->root = opttree_new_node (self);
optsystem_get_initial_state (self->optsys, self->root->state);
self->root->distance_from_root = 0.0;
self->root->distance_from_parent = 0.0;
kd_insert (self->kdtree, optsystem_get_state_key (self->optsys, self->root->state), self->root);
// Initialize the list of all nodes
self->list_nodes = NULL;
self->list_nodes = g_slist_prepend (self->list_nodes, (gpointer) (self->root));
self->num_nodes = 1;
// Initialize parameters to default values
self->run_rrtstar = 1;
self->ball_radius_constant = 30.0;
self->ball_radius_max = 1.0;
self->target_sample_prob_after_first_solution = 0.0;
self->target_sample_prob_before_first_solution = 0.0;
return self;
}
void get_netcdf(e *E){
int ncid;
int varid;
int retval;
size_t attlen = 0;
double pos[2];
int i,j,t;
int count;
// read the target grid - this is the roms curvilinear grid
// in this function we want to find the indexes that intersect
// out tc tracks
/* roms output data required
double lon_rho(eta_rho, xi_rho) ;
lon_rho:long_name = "longitude of RHO-points" ;
lon_rho:units = "degree_east" ;
lon_rho:standard_name = "longitude" ;
lon_rho:field = "lon_rho, scalar" ;
double lat_rho(eta_rho, xi_rho) ;
lat_rho:long_name = "latitude of RHO-points" ;
lat_rho:units = "degree_north" ;
lat_rho:standard_name = "latitude" ;
lat_rho:field = "lat_rho, scalar" ;
*/
// open the roms_input file
// open the file
if((retval = nc_open(E->roms_grid, NC_NOWRITE, &ncid)))
fail("failed to open roms input file: %s error is %d\n", E->roms_grid, retval);
// get the lat dimension sizes
if((retval = nc_inq_dimid(ncid, "eta_rho", &varid)))
fail("failed to get roms lat dimid: error is %d\n",retval);
if((retval = nc_inq_dimlen(ncid,varid,&E->nEtaRho)))
fail("failed to get roms lat dimid: error is %d\n",retval);
//printf("xi_rho = %zu\n", E->nEtaRho);
// get the lon dimension sizes
if((retval = nc_inq_dimid(ncid, "xi_rho", &varid)))
fail("failed to get roms lon_rho dimid: error is %d\n",retval);
if((retval = nc_inq_dimlen(ncid,varid,&E->nXiRho)))
fail("failed to read roms lon_rho data: error is %d\n",retval);
//printf("eta_rho = %zu\n", E->nXiRho);
// malloc room for the arrays
E->lat_rho = malloc2d_double( E->nEtaRho, E->nXiRho);
E->lon_rho = malloc2d_double( E->nEtaRho, E->nXiRho);
nc_inq_varid(ncid, "lat_rho", &varid);
if((retval = nc_get_var_double(ncid, varid, &E->lat_rho[0][0])))
fail("failed to read roms lat_rho data: error is %d\n", retval);
//printf("lat_rho[0][0] = %f\n", E->lat_rho[0][0]);
nc_inq_varid(ncid, "lon_rho", &varid);
if((retval = nc_get_var_double(ncid, varid, &E->lon_rho[0][0])))
fail("failed to read roms lat_rho data: error is %d\n", retval);
nc_close(ncid);
// construct the kdtree for grid searching
E->kd = kd_create(2);
E->roms_index = malloc(E->nEtaRho*E->nXiRho*sizeof(grid_index));
count = 0;
for(i=0;i<E->nEtaRho;i++){
for(j=0;j<E->nXiRho;j++){
E->roms_index[count].i = i;
E->roms_index[count].j = j;
pos[0] = E->lon_rho[i][j];
pos[1] = E->lat_rho[i][j];
kd_insert(E->kd, pos, &E->roms_index[count]);
count++;
}
}
}
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;
}
vector<Node*> Pendulum::FindPathIterations(int iterations) {
cout << "starting path iteration!\n";
vector<Node*> goal_nodes;
double min_cost = DBL_MAX;
int sol = 0;
for (int iter = 0; iter < iterations; iter++){
if(iter%5000==0){cout<<"iterations:"<<iter<<endl;}
//cout << sol << endl;
/**/
Node* prev = GetNodeToExpandRRT();
double u =((double)rand() / RAND_MAX - 0.5)*max_u;
Node* n = d.update(prev, u);
/*Test-Node generation
double testx = 2;
double testtheta = .1;
vector<double> times;
times.push_back(0);
times.push_back(0);
state_type s1;
s1.push_back(3);
s1.push_back(0);
s1.push_back(0);
s1.push_back(0);
state_type s2;
s2.push_back(40);
s2.push_back(0);
s2.push_back(2*PI-0.1);
s2.push_back(0);
vector<state_type> trajectory;
trajectory.push_back(s1);
trajectory.push_back(s2);
Node* n = new Node(0, testx, 0, testtheta, 0, 0, 0, NULL, times, trajectory);
n->print_node();
cout << Feasible(n) << endl;
*/
/**/
while (!Feasible(n)){
//n->print_node();
//cout << "found unfeasible node!\n";
int baditer = 1;
if(baditer%5000==0){cout<<"bad iterations:"<<baditer<<endl;}
delete n;
prev = GetNodeToExpandRRT();
u =((double)rand() / RAND_MAX - 0.5)*max_u;
n = d.update(prev, u);
}
prev->add_child(n);
//n->print_node();
if (CheckGoal(n)){
if (n->cost < min_cost){
min_cost = n->cost;
goal_nodes.push_back(n);
cout << "found better goal node! " << n->cost << endl;
sol = sol+1;
} /*
else{
cout << "found less optimal goal node! " << n->cost << endl;
}
*/
//Time and work are strictly positive
} else {
double pt[4] = {n->x*10.0/(bounds->ux-bounds->lx), n->v*10.0/(2*bounds->max_v)+5.0, n->theta*10.0/(6.28318530718), n->w*10.0/(2*bounds->max_w)+5.0};
if (kd_insert(nodes_tree, pt, n) != 0) {
cout << "didn't insert root successfully\n";
}
nodes.push_back(n);
}
}
cout << "finished iterating!\n";
cout << "Number of solutions found: " << sol << endl;
#ifdef OUTNEAREST
if (sol < 1){
double pt[4] = {b->x*10.0/(bounds->ux-bounds->lx), b->v*10.0/(2*bounds->max_v)+5.0, b->theta*10.0/(6.28318530718), b->w*10.0/(2*bounds->max_w)+5.0};
goalnodes.push_back()
}
// Adds a given trajectory to the graph and returns a pointer to the the last node
node_t *opttree_add_traj_to_graph (opttree_t *self, node_t *node_start, node_t *node_end,
GSList *trajectory, int num_node_states, int *node_states, GSList *inputs) {
node_t *node_prev = node_start; // This variable maintains the node last added to the graph to update parents
int trajectory_count = 0;
int node_states_count = 0;
GSList *subtraj_curr = NULL;
GSList *inputs_curr = NULL;
GSList *inputs_ptr = inputs;
GSList *trajectory_ptr = trajectory;
while (trajectory_ptr) {
state_t *state_curr = (state_t *) (trajectory_ptr->data);
// Check whether this the end of the trajectory
if (!g_slist_next (trajectory_ptr)) {
// This must be the last node in the traj.
node_t *node_curr;
if (node_end) { // If node_end is given, then modify node end accordingly
node_curr = node_end;
node_t *node_parent = node_curr->parent;
node_parent->children = g_slist_remove (node_parent->children, (gpointer) node_curr);
// Free traj_from_parent
GSList *traj_from_parent_ptr = node_curr->traj_from_parent;
while (traj_from_parent_ptr) {
optsystem_free_state (self->optsys, (state_t *) (traj_from_parent_ptr->data));
traj_from_parent_ptr = g_slist_next (traj_from_parent_ptr);
}
g_slist_free (node_curr->traj_from_parent);
node_curr->traj_from_parent = NULL;
// Free input_from_parent
GSList *inputs_from_parent_ptr = node_curr->inputs_from_parent;
while (inputs_from_parent_ptr) {
optsystem_free_input (self->optsys, (input_t *) (inputs_from_parent_ptr->data));
inputs_from_parent_ptr = g_slist_next (inputs_from_parent_ptr);
}
g_slist_free (node_curr->inputs_from_parent);
node_curr->inputs_from_parent = NULL;
// Free this state
optsystem_free_state (self->optsys, state_curr);
}
else { // If node_end is not given, then insert this state into the graph
node_curr = opttree_new_node_no_state ();
node_curr->children = NULL;
node_curr->state = state_curr;
node_curr->reaches_target = optsystem_is_reaching_target (self->optsys, node_curr->state);
kd_insert (self->kdtree, optsystem_get_state_key (self->optsys, node_curr->state), node_curr);
// Add node to the graph
self->list_nodes = g_slist_prepend (self->list_nodes, (gpointer) (node_curr));
self->num_nodes++;
}
node_curr->parent = node_prev;
if (inputs) {
input_t *input_this = (input_t *)(inputs_ptr->data);
inputs_curr = g_slist_prepend (inputs_curr, input_this);
}
node_curr->inputs_from_parent = g_slist_reverse (inputs_curr);
node_curr->traj_from_parent = g_slist_reverse (subtraj_curr);
node_curr->distance_from_parent = optsystem_evaluate_distance_for_cost (self->optsys,
node_curr->inputs_from_parent);
opttree_update_distance_from_root (self, node_prev, node_curr);
// Add this node to the children of the previous node
node_prev->children = g_slist_prepend (node_prev->children, node_curr);
// Reevaluate reaches_target variables
if (node_curr->reaches_target) {
if (! (node_prev->reaches_target) ) {
node_t *node_temp = node_prev;
while (node_temp ) {
node_temp->reaches_target = TRUE;
node_temp = node_temp->parent;
}
}
if (node_curr->reaches_target) {
if (node_curr->distance_from_root < self->lower_bound) {
self->lower_bound = node_curr->distance_from_root;
self->lower_bound_node = node_curr;
}
}
}
// Reset the pointers
subtraj_curr = NULL;
node_prev = node_curr;
goto end_iteration;
}
if (node_states) {
if ( trajectory_count == node_states[node_states_count] ) {
// Create the new node
node_t *node_curr = opttree_new_node_no_state ();
node_curr->state =state_curr;
node_curr->parent = node_prev;
node_curr->children = NULL;
node_curr->reaches_target = optsystem_is_reaching_target (self->optsys, node_curr->state);
if (inputs) {
inputs_curr = g_slist_prepend (inputs_curr, inputs_ptr->data);
}
node_curr->inputs_from_parent = g_slist_reverse (inputs_curr);
node_curr->traj_from_parent = g_slist_reverse (subtraj_curr);
node_curr->distance_from_parent = optsystem_evaluate_distance_for_cost (self->optsys,
node_curr->inputs_from_parent);
node_curr->distance_from_root = node_prev->distance_from_root + node_curr->distance_from_parent;
kd_insert (self->kdtree, optsystem_get_state_key (self->optsys, node_curr->state), node_curr);
// Update the parent node children pointer
node_prev->children = g_slist_prepend (node_prev->children, node_curr);
// Reevaluate reaches_target variables
if (node_curr->reaches_target) {
if (! (node_prev->reaches_target) ) {
node_t *node_temp = node_prev;
while (node_temp ) {
node_temp->reaches_target = TRUE;
node_temp = node_temp->parent;
}
}
if (node_curr->distance_from_root < self->lower_bound) {
self->lower_bound = node_curr->distance_from_root;
self->lower_bound_node = node_curr;
}
}
self->list_nodes = g_slist_prepend (self->list_nodes, node_curr);
self->num_nodes++;
// Reset the pointers
subtraj_curr = NULL;
inputs_curr = NULL;
node_prev = node_curr;
// Increase the node_states counter
node_states_count++;
goto end_iteration;
}
}
// Add current state to the subtrajectory
subtraj_curr = g_slist_prepend (subtraj_curr, state_curr);
if (inputs)
inputs_curr = g_slist_prepend (inputs_curr, inputs_ptr->data);
end_iteration:
trajectory_ptr = g_slist_next (trajectory_ptr);
if (inputs) {
inputs_ptr = g_slist_next (inputs_ptr);
}
trajectory_count++;
}
g_slist_free (trajectory);
g_slist_free (inputs);
free (node_states);
return node_prev;
}
