void PhotonMap::InitPhotonMap()
{
causticsCount = 0;
indirectCount = 0;
volumeMap = 0;
if ( confEnablePhotonMapping ) {
if ( confEnablePhotonCaustics ) { causticsMap = kd_create(3); }
else { causticsMap = NULL; }
if ( confEnablePhotonIndirect ) { indirectMap = kd_create(3); }
else { indirectMap = NULL; }
if ( confEnablePhotonVolumetric ) { volumeMap = kd_create(3); }
else { volumeMap = NULL; }
}
}
/**
* @function initialize
* @brief Unneeded explanation here
*/
void JG_RRT::initialize( World* _world,
const int &_robot_ID,
const std::vector<int> &_links_ID,
const Eigen::Transform<double, 3, Eigen::Affine> &_TWBase,
const Eigen::Transform<double, 3, Eigen::Affine> &_Tee )
{
//-- Clean up from previous searches
cleanup();
//-- Copy input
this->world = _world;
this->robot_ID = _robot_ID;
this->links_ID = _links_ID;
this->ndim = _links_ID.size();
this->TWBase = _TWBase;
this->Tee = _Tee;
this->kdTree = kd_create( ndim );
printf( "ndim: %d \n", ndim );
printf(" robot ID: %d \n", robot_ID );
cout<<"link ID: "<<links_ID[0]<<","<<links_ID[6]<<endl;
//-- Seed the random generator
srand( time(NULL) );
// addNode( _startConfig, -1 );
}
/**
* @function B1RRT
* @brief Constructor
*/
B1RRT::B1RRT( planning::World *_world,
Collision *_collision,
int _robotId,
const Eigen::VectorXi &_links,
const Eigen::VectorXd &_root,
const Eigen::VectorXd &_targetPose,
double _stepSize ) {
/// Initialize some member variables
world = _world;
collision = _collision;
robotId = _robotId;
links = _links;
targetPose= _targetPose;
ndim = links.size();
stepSize = _stepSize;
EEId = world->mRobots[robotId]->getDof( links[ links.size()-1 ] )->getJoint()->getChildNode()->getSkelIndex();
ranking.resize(0);
/// Initialize random generator
srand( time(NULL) );
/// Create kdtree and add the first node (start)
kdTree = kd_create( ndim );
addNode( _root, -1 );
}
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;
}
void Discontinuity::createKDTreePacked ()
{
Constants con;
std::cout << "Creating KDTree ( crust ).\n";
crustTree = kd_create (3);
int l = 0;
KDdatCrust = new int [crust_col_rad[0].size()*crust_lon_rad[0].size()]();
crust_col_deg_unpack.reserve ( crust_col_rad[0].size()*
crust_lon_rad[0].size() );
crust_lon_deg_unpack.reserve ( crust_col_rad[0].size()*
crust_lon_rad[0].size() );
for ( size_t r=0; r!=crust_col_rad.size(); r++ ) {
for ( size_t i=0; i!=crust_col_rad[r].size(); i++ ) {
for ( size_t j=0; j!=crust_lon_rad[r].size(); j++ ) {
KDdatCrust[l] = l;
kd_insert3 ( crustTree, crust_col_deg[r][i], crust_lon_deg[r][j],
con.R_EARTH, &KDdatCrust[l] );
crust_col_deg_unpack[l] = crust_col_deg[r][i];
crust_lon_deg_unpack[l] = crust_lon_deg[r][j];
l++;
}
}
}
}
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;
}
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]);
}
}
}
void simulationInit(SimulationParameters *simParams, RulesParameters *applyingRules,InfoCache *cache, int numberOfDesires)
{
int j;
unsigned long i;
Vector acc,vel;
firstTime=TRUE;
abortSimulation=FALSE;
nDesires=numberOfDesires;
dt=1/(double)(simParams->fps);
simulationProgress = 0;
leader=NULL;
memcpy(&cacheFileOption,cache,sizeof(InfoCache));
rParameters=(RulesParameters**)malloc(nDesires*sizeof(RulesParameters*));
for(j=0;j<nDesires;j++)
{
rParameters[j]=(RulesParameters*)malloc(sizeof(RulesParameters));
memcpy(rParameters[j],&applyingRules[j],sizeof(RulesParameters));
}
memcpy(&simParameters,simParams,sizeof(SimulationParameters));
actions=(Action*)malloc(sizeof(Action)*nDesires);
boidSet=(Boid *)malloc(sizeof(Boid)*simParameters.numberOfBoids);
if((nDesires>3) && (applyingRules[FOLLOWRULE].enabled))
{
leader=(Boid*)malloc(sizeof(Boid)*(unsigned)(simParameters.fps*simParameters.lenght));
leaderInit(20);
}
randomVector(&acc, simParams->maxAcceleration, 0, 0);
randomVector(&vel, simParams->maxVelocity, 0, 0);
k3=kd_create(3);
for(i=0; i<simParameters.numberOfBoids; i++)
{
boidInitialization(&boidSet[i],i+1, 20, 10, 5, &vel, &acc);
kd_insert3(k3,boidSet[i].currentPosition.x, boidSet[i].currentPosition.y, boidSet[i].currentPosition.z, &boidSet[i]);
}
initDesires(leader);
//sortingRules();
}
/** Builds a grid map structure to facilitate conversion from coordinate
* to index space.
*
* @param gx array of X coordinates [nce2 + 1][nce1 + 1]
* @param gy array of Y coordinates [nce2 + 1][nce1 + 1]
* @param nce1 number of cells in e1 direction
* @param nce2 number of cells in e2 direction
* @return a map tree to be used by xy2ij
*/
gridkmap* gridkmap_build(int nce1, int nce2, double** gx, double** gy)
{
gridkmap* gm = malloc(sizeof(gridkmap));
double* data[2];
gm->nce1 = nce1;
gm->nce2 = nce2;
gm->gx = gx;
gm->gy = gy;
gm->tree = kd_create(2);
data[0] = gx[0];
data[1] = gy[0];
kd_insertnodes(gm->tree, (nce1 + 1) * (nce2 + 1), data, 1 /* shuffle */ );
return gm;
}
/**
* vu_setup_lat_lon_tz_lookup:
*
* Can be called multiple times but only initializes the lookup once
*/
void vu_setup_lat_lon_tz_lookup ()
{
// Only setup once
if ( kd )
return;
kd = kd_create(2);
// Look in the directories of data path
gchar **data_dirs = a_get_viking_data_path();
// Process directories in reverse order for priority
guint n_data_dirs = g_strv_length ( data_dirs );
for (; n_data_dirs > 0; n_data_dirs--) {
load_ll_tz_dir(data_dirs[n_data_dirs-1]);
}
g_strfreev ( data_dirs );
}
void Discontinuity::createKDTreeUnpacked ( )
{
Constants con;
std::cout << "Creating KDTree ( model ).\n";
elvTree = kd_create (3);
KDdatElv = new int [ lonElv.size() ];
#pragma omp parallel for
for ( size_t i=0; i<lonElv.size(); i++ )
{
KDdatElv[i] = i;
{
kd_insert3 ( elvTree, colElv[i], lonElv[i], con.R_EARTH, &KDdatElv[i] );
}
}
}
/*
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));
}
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++;
}
}
}
static kdtree* kd_readfile(char* fname, int ndimin, int dorandomise)
{
int ndim = (isll) ? 3 : ndim;
FILE* f = NULL;
kdtree* tree = NULL;
char buf[BUFSIZE];
char seps[] = " ,;\t";
char* token;
double** coords;
double point[NDIMMAX];
size_t nallocated;
size_t npoints;
int i;
if (ndim < 1 || ndim > NDIMMAX)
quit("no. of dimensions = %d; expected 1 <= ndim <= %d", ndim, NDIMMAX);
if (fname == NULL)
f = stdin;
else {
if (strcmp(fname, "stdin") == 0 || strcmp(fname, "-") == 0)
f = stdin;
else {
f = fopen(fname, "r");
if (f == NULL)
quit("%s: %s", fname, strerror(errno));
}
}
coords = malloc(ndim * sizeof(double*));
nallocated = NALLOCSTART;
for (i = 0; i < ndim; ++i)
coords[i] = malloc(nallocated * sizeof(double));
npoints = 0;
while (fgets(buf, BUFSIZE, f) != NULL) {
if (buf[0] == '#')
continue;
for (i = 0; i < ndimin; ++i) {
if ((token = strtok((i == 0) ? buf : NULL, seps)) == NULL)
break;
if (!str2double(token, &point[i]))
break;
}
if (i < ndimin)
continue;
if (isll) {
double lon = point[0] * INVRAD;
double lat = point[1] * INVRAD;
double coslat = cos(lat);
point[0] = REARTH * sin(lon) * coslat;
point[1] = REARTH * cos(lon) * coslat;
point[2] = REARTH * sin(lat);
}
if (npoints == nallocated) {
nallocated *= 2;
for (i = 0; i < ndim; ++i)
coords[i] = realloc(coords[i], nallocated * sizeof(double));
}
for (i = 0; i < ndim; ++i)
coords[i][npoints] = point[i];
npoints++;
}
if (f != stdin)
if (fclose(f) != 0)
quit("%s: %s", fname, strerror(errno));
tree = kd_create(ndim);
kd_insertnodes(tree, npoints, coords, dorandomise);
for (i = 0; i < ndim; ++i)
free(coords[i]);
free(coords);
return tree;
}
int main(int argc, char **argv)
{
int i,j, vcount = 10;
int tot_count = 101;
void *kd[tot_count], *set[tot_count];
unsigned int msec, start;
/*
if(argc > 1 && isdigit(argv[1][0])) {
vcount = atoi(argv[1]);
}
*/
printf("inserting random vectors... ");
/* Creating Trees correspinding to each age group */
for (i=0;i<tot_count;i++)
kd[i]=kd_create(3);
start = get_msec();
/* Reading Input File */
FILE *file;
char *line = NULL;
size_t len = 0;
char read;
file=fopen(argv[1], "r");
if (file == NULL)
return 1;
int a;
double b;
double c;
double d;
/* Inseting the person's co-ordinates and age in the respective trees */
while ((read = getline(&line, &len, file)) != -1) {
convert(line, &a, &b, &c, &d);
assert(kd_insert3(kd[a], b, c, d, 0) == 0);
}
if (line)
free(line);
msec = get_msec() - start;
printf("%.3f sec\n", (float)msec / 1000.0);
start = get_msec();
double lat, longitude, latitude;
/**
printf("range query returned %d items in %.5f sec\n", kd_res_size(set[0]), (float)msec / 1000.0);
**/
int reqd_dist,index , ind;
while(1){
printf("Enter How many top places you want\n");
scanf("%d",&reqd_dist);
int counter = 0;
int initial_radius = 100;
printf("Enter the age of the person\n");
scanf("%d",&index);
printf("Enter the co-ordinates of a person \n");
scanf("%f %f",&longitude,&latitude);
double a_x, a_y,a_z;
coordinates_to_lat(longitude, latitude, &a_x,&a_y,&a_z);
int starting_index[101] ={0};
/* Copmuter Science */
int loop_break = 0;
int end_index = index + sqrt(reqd_dist);
if(end_index >= 101)
end_index = 100;
while(counter <= reqd_dist){
for (i=index ; i<= end_index ; i++){
set[i] = kd_nearest_range3(kd[i], a_x, a_y,a_z, initial_radius);
/*printf("After %d\n",kd_res_size(set[i]));
*/
}
/* TODO :: change sqrt(reqd_dist) to something else...... */
for(ind = index; ind <= end_index; ind++){
double x = 1, y = 1, z = 1;
a = 1;
while(starting_index[ind]){
kd_res_next(set[ind]);
starting_index[ind]-=1;
}
while(a){
kd_res_item3(set[ind], &x, &y, &z);
/*printf("%f %f %f\n",x,y,z);*/
lat_to_coordinates(x, y, z, &lat, &longitude);
printf("%d %0.2f %0.2f\n",ind,lat, longitude);
counter += 1;
int val = kd_res_end(set[ind]);
if(val == 1){
a = 0;
break;
}
else{
a = kd_res_next(set[ind]);
/*printf("%d\n",a);*/
}
if(counter >= reqd_dist){
loop_break = 1;
break;
}
}
if(loop_break ==1)
break;
}
for (i=index ; i<= end_index;i++){
starting_index[i] = kd_res_size(set[i]);
}
if(loop_break ==1)
break;
initial_radius += initial_radius;
}
}
msec = get_msec() - start;
/* for(i=1;i<tot_count;i++){
kd_res_free(set[i]);
kd_free(kd[i]);
}
*/
/**
printf("range query returned %d items in %.5f sec\n", kd_res_size(set[i]), (float)msec / 1000.0);
**/
/*
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;
}
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;
}