int gridkmap_xy2ij(gridkmap* gm, double x, double y, int* iout, int* jout) { double* minmax = kd_getminmax(gm->tree); double pos[2]; size_t nearest; size_t id; poly* p; int i, j, i1, i2, j1, j2; int success = 0; if (x < minmax[0] || y < minmax[1] || x > minmax[2] || y > minmax[3]) return success; pos[0] = x; pos[1] = y; nearest = kd_findnearestnode(gm->tree, pos); id = kd_getnodeorigid(gm->tree, nearest); p = poly_create(); j = id / (gm->nce1 + 1); i = id % (gm->nce1 + 1); i1 = (i > 0) ? i - 1 : i; i2 = (i < gm->nce1) ? i + 1 : i; j1 = (j > 0) ? j - 1 : j; j2 = (j < gm->nce2) ? j + 1 : j; /* * TODO?: looks a bit heavyweight */ for (j = j1; j <= j2 - 1; ++j) for (i = i1; i <= i2 - 1; ++i) { if (!isfinite(gm->gx[j][i]) || !isfinite(gm->gx[j][i + 1]) || !isfinite(gm->gx[j + 1][i + 1]) || !isfinite(gm->gx[j + 1][i])) continue; poly_addpoint(p, gm->gx[j][i], gm->gy[j][i]); poly_addpoint(p, gm->gx[j][i + 1], gm->gy[j][i + 1]); poly_addpoint(p, gm->gx[j + 1][i + 1], gm->gy[j + 1][i + 1]); poly_addpoint(p, gm->gx[j + 1][i], gm->gy[j + 1][i]); poly_addpoint(p, gm->gx[j][i], gm->gy[j][i]); if (poly_containspoint(p, x, y)) { success = 1; *iout = i; *jout = j; goto finish; } poly_clear(p); } finish: poly_destroy(p); return success; }
pk_t* poly_asssub_linexpr_array_det(bool assign, ap_manager_t* man, bool destructive, pk_t* pa, ap_dim_t* tdim, ap_linexpr0_t** texpr, size_t size) { size_t i; ap_dim_t* tdim2; numint_t** tvec; size_t nbcols; matrix_t* mat; pk_t* po; pk_internal_t* pk = (pk_internal_t*)man->internal; po = destructive ? pa : poly_alloc(pa->intdim,pa->realdim); if (!assign) poly_dual(pa); /* Obtain the needed matrix */ poly_obtain_F_dual(man,pa,"of the argument",assign); if (pk->exn){ pk->exn = AP_EXC_NONE; man->result.flag_best = man->result.flag_exact = false; poly_set_top(pk,po); goto _poly_asssub_linexpr_array_det_exit; } /* Return empty if empty */ if (!pa->C && !pa->F){ man->result.flag_best = man->result.flag_exact = true; poly_set_bottom(pk,po); return po; } /* Convert linear expressions */ nbcols = pk->dec + pa->intdim + pa->realdim; tvec = (numint_t**)malloc(size*sizeof(numint_t*)); for (i=0; i<size; i++){ tvec[i] = vector_alloc(nbcols); itv_linexpr_set_ap_linexpr0(pk->itv, &pk->poly_itv_linexpr, texpr[i]); vector_set_itv_linexpr(pk, tvec[i], &pk->poly_itv_linexpr, pa->intdim+pa->realdim,1); } /* Copy tdim because of sorting */ tdim2 = (ap_dim_t*)malloc(size*sizeof(ap_dim_t)); memcpy(tdim2,tdim,size*sizeof(ap_dim_t)); pk_asssub_isort(tdim2,tvec,size); /* Perform the operation */ mat = assign ? matrix_assign_variables(pk, pa->F, tdim2, tvec, size) : matrix_substitute_variables(pk, pa->F, tdim2, tvec, size); /* Free allocated stuff */ for (i=0; i<size; i++){ vector_free(tvec[i],nbcols); } free(tvec); free(tdim2); /* Update polyhedra */ if (destructive){ poly_clear(po); } po->F = mat; po->status = 0; _poly_asssub_linexpr_array_det_exit: if (!assign){ poly_dual(pa); if (!destructive) poly_dual(po); } assert(poly_check(pk,po)); return po; }
/* ---------------------------------------------------------------------- */ static void poly_approximate_123(ap_manager_t* man, pk_t* po, int algorithm) { bool change; pk_internal_t* pk = (pk_internal_t*)man->internal; if (algorithm==1){ poly_approximate_1(man,po,po); } else { size_t nbrows, nbrows2; ap_dim_t dim; ap_dim_t i,j; int sgn, sgny; itv_t itv; pk_t* pa = poly_alloc(po->intdim,po->realdim); change = poly_approximate_1(man,pa,po); if (!change){ pk_free(man,pa); return; } poly_obtain_F(man,po,NULL); if (!po->F){ if (!po->C){ pk_free(man,pa); poly_set_bottom(pk,po); man->result.flag_exact = true; return; } else { poly_clear(po); *po = *pa; free(pa); } return; } dim = pa->intdim + pa->realdim; nbrows = pa->C->nbrows; itv_init(itv); if (algorithm>=2){ /* Add interval constraints */ nbrows2 = 2*dim; matrix_resize_rows_lazy(pa->C, nbrows + nbrows2); pa->C->_sorted = false; for (i=0; i<dim;i++){ matrix_bound_dimension(pk,itv,i,po->F); if (!bound_infty(itv->inf)){ vector_set_dim_bound(pk, pa->C->p[nbrows], i, bound_numref(itv->inf), -1, po->intdim, po->realdim, false); nbrows++; } if (!bound_infty(itv->sup)){ vector_set_dim_bound(pk, pa->C->p[nbrows], i, bound_numref(itv->sup), 1, po->intdim, po->realdim, false); nbrows++; } } } pa->C->nbrows = nbrows; if (algorithm>=3){ /* Add octagonal constraints */ vector_clear(pk->poly_numintp,po->F->nbcolumns); numint_set_int(pk->poly_numintp[0],1); for (i=0; i<dim;i++){ numint_set_int(pk->poly_numintp[pk->dec+i],1); nbrows2 = 4*(dim-i-1); matrix_resize_rows_lazy(pa->C, nbrows + nbrows2); for (j=i+1; j<dim;j++){ for (sgny=-1; sgny<=1; sgny += 2){ numint_set_int(pk->poly_numintp[pk->dec+j],sgny); matrix_bound_vector(pk,itv,pk->poly_numintp,po->F); if (!bound_infty(itv->inf)){ vector_set_linexpr_bound(pk, pa->C->p[nbrows], pk->poly_numintp, itv->inf, -1, po->intdim, po->realdim, false); nbrows++; } if (!bound_infty(itv->sup)){ vector_set_linexpr_bound(pk, pa->C->p[nbrows], pk->poly_numintp, itv->sup, 1, po->intdim, po->realdim, false); nbrows++; } } numint_set_int(pk->poly_numintp[pk->dec+j],0); } numint_set_int(pk->poly_numintp[pk->dec+i],0); } pa->C->nbrows = nbrows; } itv_clear(itv); poly_clear(po); *po = *pa; free(pa); return; } }