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;
}
}
