plan_ma_msg_t *planMAMsgNew(int type, int subtype, int agent_id)
{
plan_ma_msg_t *msg;
msg = BOR_ALLOC(plan_ma_msg_t);
planMAMsgInit(msg, type, subtype, agent_id);
return msg;
}
plan_state_t *planStateNew(int size)
{
plan_state_t *state;
state = BOR_ALLOC(plan_state_t);
planStateInit(state, size);
return state;
}
plan_ma_msg_t *planMAMsgUnpacked(void *buf, size_t size)
{
plan_ma_msg_t *msg;
msg = BOR_ALLOC(plan_ma_msg_t);
planMsgDecode(msg, &schema_msg, buf);
return msg;
}
bor_poly2_t *borPoly2New2(const bor_vec2_t *corners, int *idx, int size)
{
bor_poly2_t *p;
p = BOR_ALLOC(bor_poly2_t);
borPoly2Init2(p, corners, idx, size);
return p;
}
bor_fifo_t *borFifoNewSize(size_t el_size, size_t buf_size)
{
bor_fifo_t *fifo;
fifo = BOR_ALLOC(bor_fifo_t);
borFifoInitSize(fifo, el_size, buf_size);
return fifo;
}
bor_lifo_t *borLifoNewSize(size_t el_size, size_t buf_size)
{
bor_lifo_t *lifo;
lifo = BOR_ALLOC(bor_lifo_t);
borLifoInitSize(lifo, el_size, buf_size);
return lifo;
}
RB_TREE *borRBTreeIntNew(void)
{
RB_TREE *rb;
rb = BOR_ALLOC(RB_TREE);
borRBTreeIntInit(rb);
return rb;
}
bor_qdelaunay_t *borQDelaunayNew(void)
{
bor_qdelaunay_t *q;
q = BOR_ALLOC(bor_qdelaunay_t);
q->bin_path = BOR_STRDUP(BOR_QDELAUNAY_BIN_PATH);
return q;
}
static bor_qhull_mesh3_t *borQHullMesh3New(size_t vertices)
{
bor_qhull_mesh3_t *m;
m = BOR_ALLOC(bor_qhull_mesh3_t);
m->mesh = borMesh3New();
m->vecs = borVec3ArrNew(vertices);
m->vecs_len = vertices;
return m;
}
bor_fibo_t *borFiboNew(bor_fibo_lt lt, void *data)
{
bor_fibo_t *fibo;
fibo = BOR_ALLOC(bor_fibo_t);
bzero(fibo, sizeof(bor_fibo_t));
borListInit(&fibo->root);
fibo->max_degree = 0;
fibo->lt = lt;
fibo->data = data;
return fibo;
}
plan_heur_t *planHeurPotentialNew(const plan_var_t *var, int var_size,
const plan_part_state_t *goal,
const plan_op_t *op, int op_size,
const plan_state_t *init_state,
unsigned flags)
{
plan_heur_potential_t *heur;
heur = BOR_ALLOC(plan_heur_potential_t);
bzero(heur, sizeof(*heur));
_planHeurInit(&heur->heur, heurPotentialDel, heurPotential, NULL);
planPotInit(&heur->pot, var, var_size, goal, op, op_size, init_state, flags, 0);
planPotCompute(&heur->pot);
return &heur->heur;
}
bor_segmarr_t *borSegmArrNew(size_t el_size, size_t segm_size)
{
bor_segmarr_t *arr;
if (el_size > segm_size)
return NULL;
arr = BOR_ALLOC(bor_segmarr_t);
arr->el_size = el_size;
arr->segm_size = segm_size;
arr->els_per_segm = segm_size / el_size;
arr->segm = NULL;
arr->num_segm = 0;
arr->alloc_segm = 0;
return arr;
}
plan_search_t *planSearchAStarNew(const plan_search_astar_params_t *params)
{
plan_search_astar_t *astar;
astar = BOR_ALLOC(plan_search_astar_t);
_planSearchInit(&astar->search, ¶ms->search,
planSearchAStarDel,
planSearchAStarInit,
planSearchAStarStep,
planSearchAStarInsertNode,
planSearchAStarTopNodeCost);
astar->list = planListTieBreaking(2);
astar->pathmax = params->pathmax;
return &astar->search;
}
plan_list_lazy_t *planListLazyFifoNew(void)
{
plan_list_lazy_fifo_t *l;
size_t segment_size;
l = BOR_ALLOC(plan_list_lazy_fifo_t);
planListLazyInit(&l->list,
planListLazyFifoDel,
planListLazyFifoPush,
planListLazyFifoPop,
planListLazyFifoClear);
segment_size = sysconf(_SC_PAGESIZE);
segment_size *= 4;
l->fifo = borFifoNewSize(sizeof(plan_list_lazy_fifo_el_t), segment_size);
return &l->list;
}
plan_ma_msg_t *planMAMsgClone(const plan_ma_msg_t *msg_in)
{
plan_ma_msg_t *msg;
int i;
msg = BOR_ALLOC(plan_ma_msg_t);
*msg = *msg_in;
if (msg_in->state_buf != NULL){
msg->state_buf = NULL;
MEMCPY_ARR(msg, state_buf, msg_in->state_buf, msg_in->state_buf_size);
}
if (msg_in->state_private_id != NULL){
msg->state_private_id = NULL;
MEMCPY_ARR(msg, state_private_id, msg_in->state_private_id,
msg_in->state_private_id_size);
}
if (msg_in->op != NULL){
msg->op = BOR_ALLOC_ARR(plan_ma_msg_op_t, msg_in->op_size);
for (i = 0; i < msg->op_size; ++i)
planMAMsgOpCopy(msg->op + i, msg_in->op + i);
}
if (msg_in->heur_requested_agent != NULL){
msg->heur_requested_agent = NULL;
MEMCPY_ARR(msg, heur_requested_agent, msg_in->heur_requested_agent,
msg_in->heur_requested_agent_size);
}
planMAMsgDTGReqCopy(&msg->dtg_req, &msg_in->dtg_req);
planMAMsgPotClone(&msg->pot, &msg_in->pot);
return msg;
}
svo_gng_eu_t *svoGNGEuNew(const svo_gng_eu_ops_t *ops,
const svo_gng_eu_params_t *params)
{
svo_gng_eu_t *gng_eu;
bor_nn_params_t nnp;
size_t i;
bor_real_t maxbeta;
gng_eu = BOR_ALLOC(svo_gng_eu_t);
gng_eu->net = borNetNew();
gng_eu->ops = *ops;
gng_eu->params = *params;
// set up ops data pointers
if (!gng_eu->ops.new_node_data)
gng_eu->ops.new_node_data = gng_eu->ops.data;
if (!gng_eu->ops.del_node_data)
gng_eu->ops.del_node_data = gng_eu->ops.data;
if (!gng_eu->ops.input_signal_data)
gng_eu->ops.input_signal_data = gng_eu->ops.data;
if (!gng_eu->ops.terminate_data)
gng_eu->ops.terminate_data = gng_eu->ops.data;
if (!gng_eu->ops.callback_data)
gng_eu->ops.callback_data = gng_eu->ops.data;
// initialize error heap
gng_eu->err_heap = borPairHeapNew(errHeapLT, (void *)gng_eu);
// precompute beta^n
gng_eu->beta_n = BOR_ALLOC_ARR(bor_real_t, gng_eu->params.lambda);
gng_eu->beta_n[0] = gng_eu->params.beta;
for (i = 1; i < gng_eu->params.lambda; i++){
gng_eu->beta_n[i] = gng_eu->beta_n[i - 1] * gng_eu->params.beta;
}
// precompute beta^(n * lambda)
maxbeta = gng_eu->beta_n[gng_eu->params.lambda - 1];
gng_eu->beta_lambda_n_len = 1000;
gng_eu->beta_lambda_n = BOR_ALLOC_ARR(bor_real_t, gng_eu->beta_lambda_n_len);
gng_eu->beta_lambda_n[0] = maxbeta;
for (i = 1; i < gng_eu->beta_lambda_n_len; i++){
gng_eu->beta_lambda_n[i] = gng_eu->beta_lambda_n[i - 1] * maxbeta;
}
gng_eu->cycle = 1L;
gng_eu->step = 1;
// initialize nncells
nnp = params->nn;
nnp.gug.dim = params->dim;
nnp.vptree.dim = params->dim;
nnp.linear.dim = params->dim;
gng_eu->nn = borNNNew(&nnp);
// initialize temporary vector
if (gng_eu->params.dim == 2){
gng_eu->tmpv = (bor_vec_t *)borVec2New(BOR_ZERO, BOR_ZERO);
}else if (gng_eu->params.dim == 3){
gng_eu->tmpv = (bor_vec_t *)borVec3New(BOR_ZERO, BOR_ZERO, BOR_ZERO);
}else{
gng_eu->tmpv = borVecNew(gng_eu->params.dim);
}
return gng_eu;
}
#define LP(l) bor_container_of((l), lp_t, cls)
static void cplexErr(lp_t *lp, int status, const char *s)
{
char errmsg[1024];
CPXgeterrorstring(lp->env, status, errmsg);
fprintf(stderr, "Error: CPLEX: %s: %s\n", s, errmsg);
exit(-1);
}
static bor_lp_t *new(int rows, int cols, unsigned flags)
{
lp_t *lp;
int st, num_threads;
lp = BOR_ALLOC(lp_t);
lp->cls.cls = &bor_lp_cplex;
lp->mip = 0;
// Initialize CPLEX structures
lp->env = CPXopenCPLEX(&st);
if (lp->env == NULL)
cplexErr(lp, st, "Could not open CPLEX environment");
// Set number of processing threads
num_threads = BOR_LP_GET_NUM_THREADS(flags);
if (num_threads == BOR_LP_NUM_THREADS_AUTO){
num_threads = 0;
}else if (num_threads == 0){
num_threads = 1;
}
static bor_qhull_mesh3_t *qdelaunayToMesh3(int fd)
{
FILE *fin;
int vertices, faces, tmp;
int i, j, k;
double x, y, z, w;
int id[4];
bor_qhull_mesh3_t *qmesh;
bor_mesh3_t *mesh;
bor_mesh3_vertex_t **verts;
bor_mesh3_vertex_t *vert;
bor_mesh3_edge_t *edge;
//bor_mesh3_face_t *face;
fin = fdopen(fd, "r");
if (!fin)
return NULL;
// first line is number of facets
if (fscanf(fin, "%d", &tmp) != 1){
fclose(fin);
return NULL;
}
// second line contains number of points, facets and ridges - the last
// one can be ignored
if (fscanf(fin, "%d %d %d", &vertices, &faces, &tmp) != 3){
fclose(fin);
return NULL;
}
// alloc mesh
qmesh = borQHullMesh3New(vertices);
mesh = borQHullMesh3(qmesh);
// allocate index array for vertices
verts = BOR_ALLOC_ARR(bor_mesh3_vertex_t *, vertices);
// read points and create vertices
for (i = 0; i < vertices; i++){
if (fscanf(fin, "%lg %lg %lg %lg", &x, &y, &z, &w) != 4){
break;
}
borVec3Set(&qmesh->vecs[i], x, y, z);
vert = BOR_ALLOC(bor_mesh3_vertex_t);
borMesh3VertexSetCoords(vert, &qmesh->vecs[i]);
borMesh3AddVertex(mesh, vert);
verts[i] = vert;
//fprintf(stdout, "[%d] %lg %lg %lg\n", i, x, y, z);
}
// read tetrahedrons
for (i = 0; i < faces; i++){
if (fscanf(fin, "%d %d %d %d", &id[0], &id[1], &id[2], &id[3]) != 4){
break;
}
// create edges
for (j = 0; j < 3; j++){
for (k = j + 1; k < 4; k++){
// create new edge only if it is not already there
edge = borMesh3VertexCommonEdge(verts[id[j]], verts[id[k]]);
if (!edge){
edge = BOR_ALLOC(bor_mesh3_edge_t);
borMesh3AddEdge(mesh, edge, verts[id[j]], verts[id[k]]);
}
}
}
//fprintf(stdout, "[%d] %d %d %d %d\n", i, id[0], id[1], id[2], id[3]);
}
if (verts)
BOR_FREE(verts);
fclose(fin);
return qmesh;
}
svo_gng_t *svoGNGNew(const svo_gng_ops_t *ops,
const svo_gng_params_t *params)
{
svo_gng_t *gng;
size_t i;
bor_real_t maxbeta;
gng = BOR_ALLOC(svo_gng_t);
gng->net = borNetNew();
gng->ops = *ops;
gng->params = *params;
// set up ops data pointers
if (!gng->ops.init_data)
gng->ops.init_data = gng->ops.data;
if (!gng->ops.new_node_data)
gng->ops.new_node_data = gng->ops.data;
if (!gng->ops.new_node_between_data)
gng->ops.new_node_between_data = gng->ops.data;
if (!gng->ops.del_node_data)
gng->ops.del_node_data = gng->ops.data;
if (!gng->ops.input_signal_data)
gng->ops.input_signal_data = gng->ops.data;
if (!gng->ops.nearest_data)
gng->ops.nearest_data = gng->ops.data;
if (!gng->ops.dist2_data)
gng->ops.dist2_data = gng->ops.data;
if (!gng->ops.move_towards_data)
gng->ops.move_towards_data = gng->ops.data;
if (!gng->ops.terminate_data)
gng->ops.terminate_data = gng->ops.data;
if (!gng->ops.callback_data)
gng->ops.callback_data = gng->ops.data;
// initialize error heap
gng->err_heap = borPairHeapNew(errHeapLT, (void *)gng);
// precompute beta^n
gng->beta_n = BOR_ALLOC_ARR(bor_real_t, gng->params.lambda);
gng->beta_n[0] = gng->params.beta;
for (i = 1; i < gng->params.lambda; i++){
gng->beta_n[i] = gng->beta_n[i - 1] * gng->params.beta;
}
// precompute beta^(n * lambda)
maxbeta = gng->beta_n[gng->params.lambda - 1];
gng->beta_lambda_n_len = 1000;
gng->beta_lambda_n = BOR_ALLOC_ARR(bor_real_t, gng->beta_lambda_n_len);
gng->beta_lambda_n[0] = maxbeta;
for (i = 1; i < gng->beta_lambda_n_len; i++){
gng->beta_lambda_n[i] = gng->beta_lambda_n[i - 1] * maxbeta;
}
gng->cycle = 1L;
gng->step = 1;
return gng;
}
bor_cl_t *borCLNewSimple2(size_t program_count, const char **program,
const char *buildopts)
{
cl_uint num_platforms, num_devices, i;
cl_int err;
cl_platform_id *platforms, platform;
cl_device_id device;
size_t bufsize;
char buf[1024], *buf2;
bor_cl_t *cl;
// find platform and device
platform = (cl_platform_id)-1;
err = clGetPlatformIDs(0, NULL, &num_platforms);
if (__borCLErrorCheck(err, "Can't get any platform") != 0)
return NULL;
if (num_platforms == 0)
return NULL;
platforms = BOR_ALLOC_ARR(cl_platform_id, num_platforms);
err = clGetPlatformIDs(num_platforms, platforms, NULL);
if (__borCLErrorCheck(err, "Can't get any platform") != 0)
return NULL;
for (i = 0; i < num_platforms; i++){
err = clGetDeviceIDs(platforms[i], CL_DEVICE_TYPE_GPU, 0, NULL, &num_devices);
if (__borCLErrorCheck(err, "Cant'get any device") != 0)
break;
err = clGetDeviceIDs(platforms[i], CL_DEVICE_TYPE_GPU, 1, &device, &num_devices);
if (__borCLErrorCheck(err, "Cant'get any device") != 0)
break;
if (num_devices > 0){
platform = platforms[i];
break;
}
}
BOR_FREE(platforms);
if (platform == (cl_platform_id)-1)
return NULL;
cl = BOR_ALLOC(bor_cl_t);
cl->platform = platform;
cl->device = device;
// create context
cl->context = clCreateContext(NULL, 1, &device, NULL, NULL, &err);
if (__borCLErrorCheck(err, "Can't create context") != 0){
BOR_FREE(cl);
return NULL;
}
// create queue
cl->queue = clCreateCommandQueue(cl->context, cl->device, 0, &err);
if (__borCLErrorCheck(err, "Can't create command queue") != 0){
clReleaseContext(cl->context);
BOR_FREE(cl);
return NULL;
}
// create program
cl->program = clCreateProgramWithSource(cl->context, program_count, program, NULL, &err);
if (__borCLErrorCheck(err, "Can't create program") != 0){
clReleaseCommandQueue(cl->queue);
clReleaseContext(cl->context);
BOR_FREE(cl);
return NULL;
}
// build program
err = clBuildProgram(cl->program, 1, &cl->device, buildopts, NULL, NULL);
if (__borCLErrorCheck(err, "Can't build program") != 0){
err = clGetProgramBuildInfo(cl->program, cl->device, CL_PROGRAM_BUILD_LOG,
1024, buf, &bufsize);
if (err == CL_INVALID_VALUE && bufsize > 1024){
buf2 = BOR_ALLOC_ARR(char, bufsize);
err = clGetProgramBuildInfo(cl->program, cl->device, CL_PROGRAM_BUILD_LOG,
bufsize, buf2, NULL);
if (__borCLErrorCheck(err, "Can't obtain build log") == 0){
fprintf(stderr, " >> Build log:\n%s\n", buf2);
}
BOR_FREE(buf2);
}else{
