static void poly2Init(bor_poly2_t *p, int size)
{
p->size = size;
p->px = BOR_ALLOC_ARR(bor_real_t, size);
p->py = BOR_ALLOC_ARR(bor_real_t, size);
p->constant = BOR_ALLOC_ARR(bor_real_t, p->size);
p->multiple = BOR_ALLOC_ARR(bor_real_t, p->size);
}
static void _svoGNGInit(svo_gng_t *gng)
{
const void *is;
svo_gng_node_t *n1 = NULL, *n2 = NULL;
size_t i;
bor_real_t maxbeta;
gng->cycle = 1L;
// initialize error heap
if (gng->err_heap)
borPairHeapDel(gng->err_heap);
gng->err_heap = borPairHeapNew(errHeapLT, (void *)gng);
// precompute beta^n
if (gng->beta_n)
BOR_FREE(gng->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)
if (gng->beta_lambda_n)
BOR_FREE(gng->beta_lambda_n);
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;
}
if (gng->ops.init){
OPS(gng, init)(&n1, &n2, OPS_DATA(gng, init));
}else{
is = OPS(gng, input_signal)(OPS_DATA(gng, input_signal));
n1 = OPS(gng, new_node)(is, OPS_DATA(gng, new_node));
is = OPS(gng, input_signal)(OPS_DATA(gng, input_signal));
n2 = OPS(gng, new_node)(is, OPS_DATA(gng, new_node));
}
nodeAdd(gng, n1);
nodeAdd(gng, n2);
edgeNew(gng, n1, n2);
}
size_t borNearestLinear(bor_list_t *list, void *p,
bor_nearest_linear_dist_t dist_cb,
bor_list_t **nearest, size_t num,
void *data)
{
bor_list_t *item;
bor_real_t *dists, dist;
size_t len;
if (num == 0)
return 0;
dists = BOR_ALLOC_ARR(bor_real_t, num);
len = 0;
BOR_LIST_FOR_EACH(list, item){
dist = dist_cb(p, item, data);
if (len < num){
dists[len] = dist;
nearest[len] = item;
len++;
bubbleUp(dists, nearest, len);
}else if (dist < dists[len - 1]){
dists[len - 1] = dist;
nearest[len - 1] = item;
bubbleUp(dists, nearest, len);
}
}
static void planMAMsgOpCopy(plan_ma_msg_op_t *dst,
const plan_ma_msg_op_t *src)
{
*dst = *src;
if (src->name != NULL){
dst->name = BOR_ALLOC_ARR(int8_t, src->name_size);
memcpy(dst->name, src->name, src->name_size);
}
}
static void planMAMsgDTGReqCopy(plan_ma_msg_dtg_req_t *dst,
const plan_ma_msg_dtg_req_t *src)
{
*dst = *src;
if (src->reachable != NULL){
dst->reachable = BOR_ALLOC_ARR(int32_t, src->reachable_size);
memcpy(dst->reachable, src->reachable,
src->reachable_size * sizeof(int32_t));
}
}
static el_t *randomEls(size_t num)
{
bor_rand_t r;
bor_real_t val;
el_t *els;
size_t i;
borRandInit(&r);
els = BOR_ALLOC_ARR(el_t, num);
for (i = 0; i < num; i++){
val = borRand(&r, -500., 500.);
els[i].val = val;
els[i].id = i;
}
return els;
}
static void potAgentSetSubmatrix(plan_ma_msg_pot_submatrix_t *ms,
const plan_pot_submatrix_t *s)
{
int i, size;
ms->header |= M_pot_submatrix_cols;
ms->cols = s->cols;
ms->header |= M_pot_submatrix_rows;
ms->rows = s->rows;
if (s->cols * s->rows > 0){
ms->header |= M_pot_submatrix_coef;
size = s->cols * s->rows;
ms->coef_size = size;
ms->coef = BOR_ALLOC_ARR(int32_t, size);
for (i = 0; i < size; ++i)
ms->coef[i] = s->coef[i];
}
}
static el_t *elsNew(size_t len)
{
size_t i;
bor_real_t x, y;
el_t *ns;
ns = BOR_ALLOC_ARR(el_t, arr_len);
for (i = 0; i < len; i++){
x = borRand(&r, -15., 15.);
y = borRand(&r, -20., 20.);
borVec2Set(&ns[i].v, x, y);
borNNElInit(linear, &ns[i].linear, (const bor_vec_t *)&ns[i].v);
borNNElInit(gug, &ns[i].gug, (const bor_vec_t *)&ns[i].v);
borNNElInit(vp, &ns[i].vp, (const bor_vec_t *)&ns[i].v);
borNNAdd(linear, &ns[i].linear);
borNNAdd(gug, &ns[i].gug);
borNNAdd(vp, &ns[i].vp);
}
return ns;
}
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;
}
void planStateInit(plan_state_t *state, int size)
{
state->val = BOR_ALLOC_ARR(plan_val_t, size);
state->size = size;
state->state_id = PLAN_NO_STATE;
}
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{
