void svoGNGEuDel(svo_gng_eu_t *gng_eu)
{
if (gng_eu->beta_n)
BOR_FREE(gng_eu->beta_n);
if (gng_eu->beta_lambda_n)
BOR_FREE(gng_eu->beta_lambda_n);
if (gng_eu->net){
borNetDel2(gng_eu->net, nodeFinalDel, gng_eu,
delEdge, gng_eu);
}
if (gng_eu->err_heap)
borPairHeapDel(gng_eu->err_heap);
if (gng_eu->nn)
borNNDel(gng_eu->nn);
if (gng_eu->params.dim == 2){
borVec2Del((bor_vec2_t *)gng_eu->tmpv);
}else if (gng_eu->params.dim == 3){
borVec3Del((bor_vec3_t *)gng_eu->tmpv);
}else{
borVecDel(gng_eu->tmpv);
}
BOR_FREE(gng_eu);
}
void borPoly2Free(bor_poly2_t *p)
{
if (p->px)
BOR_FREE(p->px);
if (p->py)
BOR_FREE(p->py);
if (p->constant)
BOR_FREE(p->constant);
if (p->multiple)
BOR_FREE(p->multiple);
}
void borSegmArrDel(bor_segmarr_t *arr)
{
size_t i;
if (arr->segm){
for (i = 0; i < arr->num_segm; ++i)
BOR_FREE(arr->segm[i]);
BOR_FREE(arr->segm);
}
BOR_FREE(arr);
}
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);
}
void borFifoPop(bor_fifo_t *fifo)
{
bor_fifo_segm_t *next;
if (!fifo->front)
return;
// Notice that we are not really removing an element we just move the
// pointer and remove only the whole segment if necessary
fifo->front_el += fifo->el_size;
// Remove the front segment if no more elements can fit it or if have
// pop'ed the last element.
if (fifo->front_el + fifo->el_size > fifo->front_end
|| (fifo->front == fifo->back && fifo->front_el >= fifo->back_el)){
// Remember the next segment
next = fifo->front->next;
// free the whole segment
BOR_FREE(fifo->front);
// Use the remembered segment and initialize front_el and front_end
fifo->front = next;
if (fifo->front != NULL){
fifoInitElPtrs(fifo, fifo->front,
&fifo->front_el, &fifo->front_end);
}else{
fifoReset(fifo);
}
}
}
void borLifoPop(bor_lifo_t *lifo)
{
bor_lifo_segm_t *prev;
if (!lifo->back)
return;
// Notice that we are not really removing an element we just move the
// pointer and remove only the whole segment if necessary
lifo->cur -= lifo->el_size;
// Remove last segment if this is the last element popped.
if (lifo->cur < lifo->data_begin){
prev = lifo->back->prev;
BOR_FREE(lifo->back);
lifo->back = prev;
if (lifo->back == NULL){
// Remove the last segment, lifo is empty
lifoReset(lifo);
}else{
lifoResetBack(lifo);
lifo->cur = lifo->data_end - lifo->el_size;
}
}
}
void planListLazyFifoDel(plan_list_lazy_t *_l)
{
plan_list_lazy_fifo_t *l = LIST_FROM_PARENT(_l);
planListLazyFree(&l->list);
borFifoDel(l->fifo);
BOR_FREE(l);
}
int main(int argc, char *argv[])
{
test_msg2_t msg;
unsigned char *buf;
int fd, bufsize, size, wsize;
if (argc != 2){
fprintf(stderr, "Usage: %s out.bin >out.txt\n", argv[0]);
return -1;
}
msg2Rand(&msg);
borMsgSetHeader(&msg, test_msg2_t_schema);
buf = NULL;
bufsize = 0;
size = borMsgEncode(&msg, test_msg2_t_schema, &buf, &bufsize);
fd = open(argv[1], O_WRONLY | O_CREAT, 0644);
if (fd < 0){
fprintf(stderr, "Error: Could not open `%s'\n", argv[1]);
return -1;
}
wsize = 0;
while (wsize < size)
wsize += write(fd, buf + wsize, size - wsize);
close(fd);
msg2Print(&msg, stdout);
borMsgFree(&msg, test_msg2_t_schema);
BOR_FREE(buf);
return 0;
}
void svoGNGDel(svo_gng_t *gng)
{
if (gng->beta_n)
BOR_FREE(gng->beta_n);
if (gng->beta_lambda_n)
BOR_FREE(gng->beta_lambda_n);
if (gng->net){
borNetDel2(gng->net, nodeFinalDel, gng,
delEdge, gng);
}
if (gng->err_heap)
borPairHeapDel(gng->err_heap);
BOR_FREE(gng);
}
void planMAMsgOpSetName(plan_ma_msg_op_t *op, const char *name)
{
if (op->name != NULL)
BOR_FREE(op->name);
op->name_size = strlen(name) + 1;
op->name = (int8_t *)BOR_STRDUP(name);
op->header |= M_name;
}
static void heurPotentialDel(plan_heur_t *_heur)
{
plan_heur_potential_t *h = HEUR(_heur);
planPotFree(&h->pot);
_planHeurFree(&h->heur);
BOR_FREE(h);
}
static void planSearchAStarDel(plan_search_t *search)
{
plan_search_astar_t *astar = SEARCH_FROM_PARENT(search);
_planSearchFree(search);
if (astar->list)
planListDel(astar->list);
BOR_FREE(astar);
}
void planMAMsgFree(plan_ma_msg_t *msg)
{
int i;
if (msg->state_buf != NULL)
BOR_FREE(msg->state_buf);
if (msg->state_private_id != NULL)
BOR_FREE(msg->state_private_id);
if (msg->op != NULL){
for (i = 0; i < msg->op_size; ++i)
planMAMsgOpFree(msg->op + i);
BOR_FREE(msg->op);
}
if (msg->heur_requested_agent != NULL)
BOR_FREE(msg->heur_requested_agent);
planMAMsgDTGReqFree(&msg->dtg_req);
planMAMsgPotFree(&msg->pot);
}
void borFifoClear(bor_fifo_t *fifo)
{
bor_fifo_segm_t *next;
while (fifo->front){
next = fifo->front->next;
BOR_FREE(fifo->front);
fifo->front = next;
}
fifoReset(fifo);
}
void borLifoClear(bor_lifo_t *lifo)
{
bor_lifo_segm_t *prev;
while (lifo->back){
prev = lifo->back->prev;
BOR_FREE(lifo->back);
lifo->back = prev;
}
lifoReset(lifo);
}
int main(int argc, char *argv[])
{
test_msg2_t msg;
unsigned char *buf;
int fsize, bufsize, size, wsize;
FILE *fin;
if (argc != 2){
fprintf(stderr, "Usage: %s in.bin >out\n", argv[0]);
return -1;
}
fin = fopen(argv[1], "rb");
if (fin == NULL){
fprintf(stderr, "Error: Could not open `%s'\n", argv[1]);
return -1;
}
fseek(fin, 0, SEEK_END);
fsize = ftell(fin);
fseek(fin, 0, SEEK_SET);
buf = malloc(fsize + 1);
fread(buf, fsize, 1, fin);
fclose(fin);
borMsgDecode(buf, fsize, &msg, test_msg2_t_schema);
BOR_FREE(buf);
buf = NULL;
bufsize = 0;
size = borMsgEncode(&msg, test_msg2_t_schema, &buf, &bufsize);
wsize = 0;
while (wsize < size)
wsize += write(1, buf + wsize, size - wsize);
borMsgFree(&msg, test_msg2_t_schema);
BOR_FREE(buf);
return 0;
}
void borQHullMesh3Del(bor_qhull_mesh3_t *m)
{
if (m->mesh){
borMesh3Del2(m->mesh, mesh3DelVert, NULL,
mesh3DelEdge, NULL,
mesh3DelFace, NULL);
}
if (m->vecs){
borVec3ArrDel(m->vecs);
}
BOR_FREE(m);
}
void borSetFree(bor_set_t *s)
{
if (s->s)
BOR_FREE(s->s);
}
static void testCorrect(void)
{
bor_nn_params_t params;
bor_real_t range[4] = { -15., 15., -18., 17. };
el_t *ns, *near[3];
bor_nn_el_t *el_linear[50], *el_gug[50], *el_vp[50];
int i, j, k;
int len_linear, len_gug, len_vp;
int incorrect;
bor_vec2_t v;
borNNParamsInit(¶ms);
borNNParamsSetDim(¶ms, 2);
params.gug.num_cells = 0;
params.gug.max_dens = 1;
params.gug.expand_rate = 2.;
params.gug.aabb = range;
params.type = BOR_NN_LINEAR;
linear = borNNNew(¶ms);
params.type = BOR_NN_GUG;
gug = borNNNew(¶ms);
params.type = BOR_NN_VPTREE;
vp = borNNNew(¶ms);
ns = elsNew(arr_len);
for (k = 0; k < nearest_len; k++){
incorrect = 0;
for (i=0; i < loops; i++){
fprintf(stderr, "[%d] %08d / %08d\r", (int)k, (int)i, (int)loops);
borVec2Set(&v, borRand(&r, -10., 10.), borRand(&r, -10, 10));
len_linear = borNNNearest(linear, (const bor_vec_t *)&v, k + 1, el_linear);
len_gug = borNNNearest(gug, (const bor_vec_t *)&v, k + 1, el_gug);
len_vp = borNNNearest(vp, (const bor_vec_t *)&v, k + 1, el_vp);
if (len_linear != len_gug
|| len_linear != len_vp
|| len_vp != len_gug
|| len_linear != k + 1){
incorrect = 1;
}
for (j = 0; j < k + 1; j++){
near[0] = bor_container_of(el_linear[j], el_t, linear);
near[1] = bor_container_of(el_gug[j], el_t, gug);
near[2] = bor_container_of(el_vp[j], el_t, vp);
if (near[0] != near[1]
|| near[0] != near[2]
|| near[1] != near[2]){
incorrect = 1;
}
}
}
if (incorrect){
fprintf(stderr, "[%d] %08d / %08d FAIL\n", (int)k, (int)i, (int)loops);
}else{
fprintf(stderr, "[%d] %08d / %08d OK\n", (int)k, (int)i, (int)loops);
}
}
BOR_FREE(ns);
borNNDel(linear);
borNNDel(gug);
borNNDel(vp);
}
static void bench(void)
{
bor_nn_params_t params;
bor_real_t range[4] = { -15., 15., -18., 17. };
el_t *ns, *near;
bor_nn_el_t *el[50];
int i, j, k;
bor_vec2_t v;
int devnull;
bor_timer_t timer;
devnull = open("/dev/null", O_WRONLY);
if (devnull < 0){
perror("Error: ");
return;
}
borNNParamsInit(¶ms);
borNNParamsSetDim(¶ms, 2);
params.gug.num_cells = 0;
params.gug.max_dens = 1;
params.gug.expand_rate = 2.;
params.gug.aabb = range;
params.type = BOR_NN_LINEAR;
linear = borNNNew(¶ms);
params.type = BOR_NN_GUG;
gug = borNNNew(¶ms);
params.type = BOR_NN_VPTREE;
vp = borNNNew(¶ms);
ns = elsNew(arr_len);
for (k = 0; k < nearest_len; k++){
borTimerStart(&timer);
for (i=0; i < loops; i++){
//fprintf(stderr, "[%d] %08d / %08d\r", (int)k, (int)i, (int)loops);
borVec2Set(&v, borRand(&r, -10., 10.), borRand(&r, -10, 10));
borNNNearest(linear, (const bor_vec_t *)&v, k + 1, el);
for (j = 0; j < k + 1; j++){
near = bor_container_of(el[j], el_t, linear);
write(devnull, &near->v, 1);
}
}
borTimerStop(&timer);
borTimerPrintElapsed(&timer, stderr, " - [%d] - linear - \n", k);
borTimerStart(&timer);
for (i=0; i < loops; i++){
//fprintf(stderr, "[%d] %08d / %08d\r", (int)k, (int)i, (int)loops);
borVec2Set(&v, borRand(&r, -10., 10.), borRand(&r, -10, 10));
borNNNearest(gug, (const bor_vec_t *)&v, k + 1, el);
for (j = 0; j < k + 1; j++){
near = bor_container_of(el[j], el_t, gug);
write(devnull, &near->v, 1);
}
}
borTimerStop(&timer);
borTimerPrintElapsed(&timer, stderr, " - [%d] - gug - \n", k);
borTimerStart(&timer);
for (i=0; i < loops; i++){
//fprintf(stderr, "[%d] %08d / %08d\r", (int)k, (int)i, (int)loops);
borVec2Set(&v, borRand(&r, -10., 10.), borRand(&r, -10, 10));
borNNNearest(vp, (const bor_vec_t *)&v, k + 1, el);
for (j = 0; j < k + 1; j++){
near = bor_container_of(el[j], el_t, vp);
write(devnull, &near->v, 1);
}
}
borTimerStop(&timer);
borTimerPrintElapsed(&timer, stderr, " - [%d] - vptree - \n", k);
}
BOR_FREE(ns);
borNNDel(linear);
borNNDel(gug);
borNNDel(vp);
close(devnull);
}
void borQDelaunaySetPath(bor_qdelaunay_t *q, const char *path)
{
if (q->bin_path)
BOR_FREE(q->bin_path);
q->bin_path = BOR_STRDUP(path);
}
void borQDelaunayDel(bor_qdelaunay_t *q)
{
if (q->bin_path)
BOR_FREE(q->bin_path);
BOR_FREE(q);
}
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;
}
static void mesh3DelFace(bor_mesh3_face_t *f, void *data)
{
BOR_FREE(f);
}
static void mesh3DelEdge(bor_mesh3_edge_t *e, void *data)
{
BOR_FREE(e);
}
static void mesh3DelVert(bor_mesh3_vertex_t *v, void *data)
{
BOR_FREE(v);
}
void planStateFree(plan_state_t *state)
{
if (state->val)
BOR_FREE(state->val);
}
void borFifoDel(bor_fifo_t *fifo)
{
borFifoFree(fifo);
BOR_FREE(fifo);
}
void borFiboDel(bor_fibo_t *fibo)
{
BOR_FREE(fibo);
}
void borPoly2Del(bor_poly2_t *p)
{
borPoly2Free(p);
BOR_FREE(p);
}
