modules/base/math/geometry/polygon.c

   1 #include <stdint.h>
   2 #include <inttypes.h>
   3 #include <stdlib.h>
   4 #include <stdio.h>
   5 #include <math.h>
   6 #include <vect_base.h>
   7 #include <lines.h>
   8 #include <polygon.h>
   9
  10 #define DEBUG 0
  11
  12 #if DEBUG == 1
  13 #define debug_printf(args...) printf(args)
  14 #else
  15 #define debug_printf(args...)
  16 #endif
  17
  18 /* default bounding box is (0,0) (100,100) */
  19 static int32_t bbox_x1 = 0;
  20 static int32_t bbox_y1 = 0;
  21 static int32_t bbox_x2 = 100;
  22 static int32_t bbox_y2 = 100;
  23
  24 void polygon_set_boundingbox(int32_t x1, int32_t y1, int32_t x2, int32_t y2)
  25 {
  26         bbox_x1 = x1;
  27         bbox_y1 = y1;
  28         bbox_x2 = x2;
  29         bbox_y2 = y2;
  30 }
  31
  32 uint8_t is_in_boundingbox(const point_t *p)
  33 {
  34         if (p->x >= bbox_x1 &&
  35             p->x <= bbox_x2 &&
  36             p->y >= bbox_y1 &&
  37             p->y <= bbox_y2)
  38                 return 1;
  39         return 0;
  40 }
  41
  42 /* Test if a point is in a polygon (including edges)
  43  *  0 not inside
  44  *  1 inside
  45  *  2 on edge
  46  */
  47 uint8_t
  48 is_in_poly(const point_t *p, poly_t *pol)
  49 {
  50         uint8_t i;
  51         uint8_t ii;
  52         int8_t z;
  53         uint8_t ret=1;
  54         vect_t v, w;
  55
  56         for (i=0;i<pol->l;i++) {
  57                 /* is a polygon point */
  58                 if (p->x == pol->pts[i].x && p->y == pol->pts[i].y)
  59                         return 2;
  60         }
  61
  62         for (i=0;i<pol->l;i++) {
  63
  64                 ii = (i+1)%pol->l;
  65                 v.x = pol->pts[ii].x-p->x;
  66                 v.y = pol->pts[ii].y-p->y;
  67                 w.x = pol->pts[i].x-p->x;
  68                 w.y = pol->pts[i].y-p->y;
  69                 z = vect_pvect_sign(&v, &w );
  70                 if (z>0)
  71                         return 0;
  72                 if (z==0)
  73                         ret=2;
  74         }
  75
  76         return ret;
  77 }
  78
  79 /* public wrapper for is_in_poly() */
  80 uint8_t is_point_in_poly(poly_t *pol, int16_t x, int16_t y)
  81 {
  82         point_t p;
  83         p.x = x;
  84         p.y = y;
  85         return is_in_poly(&p, pol);
  86 }
  87
  88 /* Is segment crossing polygon? (including edges)
  89  *  0 don't cross
  90  *  1 cross
  91  *  2 on a side
  92  *  3 touch out (a segment boundary is on a polygon edge,
  93  *  and the second segment boundary is out of the polygon)
  94  */
  95 uint8_t
  96 is_crossing_poly(point_t p1, point_t p2, point_t *intersect_pt,
  97                  poly_t *pol)
  98 {
  99         uint8_t i;
 100         uint8_t ret;
 101         point_t p;
 102         uint8_t ret1, ret2;
 103         uint8_t cpt=0;
 104
 105         debug_printf("%" PRIi32 " %" PRIi32 " -> %" PRIi32 " %" PRIi32 " crossing poly %p ?\n",
 106                p1.x, p1.y, p2.x, p2.y, pol);
 107         debug_printf("poly is : ");
 108         for (i=0; i<pol->l; i++) {
 109                 debug_printf("%" PRIi32 ",%" PRIi32 " ", pol->pts[i].x, pol->pts[i].y);
 110         }
 111         debug_printf("\n");
 112
 113         for (i=0;i<pol->l;i++) {
 114                 ret = intersect_segment(&p1, &p2, &pol->pts[i], &pol->pts[(i+1)%pol->l], &p);
 115                 debug_printf("%" PRIi32 ",%" PRIi32 " -> %" PRIi32 ",%" PRIi32
 116                              " return %d\n", pol->pts[i].x, pol->pts[i].y,
 117                        pol->pts[(i+1)%pol->l].x, pol->pts[(i+1)%pol->l].y, ret);
 118
 119
 120                 switch(ret) {
 121                 case 0:
 122                         break;
 123                 case 1:
 124                         if (intersect_pt)
 125                                 *intersect_pt = p;
 126                         return 1;
 127                         break;
 128                 case 2:
 129                         cpt++;
 130                         if (intersect_pt)
 131                                 *intersect_pt = p;
 132
 133                         break;
 134                 case 3:
 135                         if (intersect_pt)
 136                                 *intersect_pt = p;
 137                         return 2;
 138                         break;
 139                 }
 140         }
 141
 142         if (cpt==3 ||cpt==4)
 143                 return 1;
 144
 145         ret1 = is_in_poly(&p1, pol);
 146         /* XXX opt */
 147         ret2 = is_in_poly(&p2, pol);
 148
 149         debug_printf("is in poly: p1 %d p2: %d cpt %d\r\n", ret1, ret2, cpt);
 150
 151         debug_printf("p intersect: %"PRIi32" %"PRIi32"\r\n", p.x, p.y);
 152
 153
 154         if (cpt==0) {
 155                 if (ret1==1 || ret2==1)
 156                         return 1;
 157                 return 0;
 158         }
 159
 160
 161         if (cpt==1) {
 162                 if (ret1==1 || ret2==1)
 163                         return 1;
 164                 return 3;
 165         }
 166         if (cpt==2) {
 167                 if (ret1==1 || ret2==1)
 168                         return 1;
 169                 if (ret1==0 || ret2==0)
 170                         return 3;
 171                 return 1;
 172         }
 173
 174         return 1;
 175 }
 176
 177 /* Giving the list of poygons, compute the graph of "visibility rays".
 178  * This rays array is composed of indexes representing 2 polygon
 179  * vertices that can "see" each others:
 180  *
 181  *  i  : the first polygon number in the input polygon list
 182  *  i+1: the vertex index of this polygon (vertex 1)
 183  *  i+2: the second polygon number in the input polygon list
 184  *  i+3: the vertex index of this polygon (vertex 2)
 185  *
 186  *  Here, vertex 1 can "see" vertex 2 in our visibility graph
 187  *
 188  *  As the first polygon is not a real polygon but the start/stop
 189  *  point, the polygon is NOT an ocluding polygon (but its vertices
 190  *  are used to compute visibility to start/stop points)
 191  */
 192
 193 uint8_t
 194 calc_rays(poly_t *polys, uint8_t npolys, uint8_t *rays)
 195 {
 196         uint8_t i, ii, index;
 197         uint8_t ray_n=0;
 198         uint8_t is_ok;
 199         uint8_t n;
 200         uint8_t pt1, pt2;
 201
 202         /* !\\first poly is the start stop point */
 203
 204         /* 1: calc inner polygon rays
 205          * compute for each polygon edges, if the vertices can see each others
 206          * (usefull if interlaced polygons)
 207          */
 208
 209         for (i=0; i<npolys; i++) {
 210                 debug_printf("%s(): poly num %d/%d\n", __FUNCTION__, i, npolys);
 211                 for (ii=0; ii<polys[i].l; ii++) {
 212                         debug_printf("%s() line num %d/%d\n", __FUNCTION__, ii, polys[i].l);
 213                         if (! is_in_boundingbox(&polys[i].pts[ii]))
 214                                 continue;
 215                         is_ok = 1;
 216                         n = (ii+1)%polys[i].l;
 217
 218                         if (!(is_in_boundingbox(&polys[i].pts[n])))
 219                                 continue;
 220
 221
 222                         /* check if a polygon cross our ray */
 223                         for (index=1; index<npolys; index++) {
 224
 225                                 /* don't check polygon against itself */
 226                                 if (index == i) continue;
 227
 228                                 if (is_crossing_poly(polys[i].pts[ii], polys[i].pts[n], NULL,
 229                                                      &polys[index]) == 1) {
 230                                         is_ok = 0;
 231                                         debug_printf("is_crossing_poly() returned 1\n");
 232                                         break;
 233                                 }
 234                         }
 235                         /* if ray is not crossed, add it */
 236                         if (is_ok) {
 237                                 rays[ray_n++] = i;
 238                                 rays[ray_n++] = ii;
 239                                 rays[ray_n++] = i;
 240                                 rays[ray_n++] = n;
 241                         }
 242                 }
 243         }
 244
 245
 246         /* 2: calc inter polygon rays.
 247          * Visibility of inter-polygon vertices.*/
 248
 249         /* For all poly */
 250         for (i=0; i<npolys-1; i++) {
 251                 for (pt1=0;pt1<polys[i].l;pt1++) {
 252
 253                         if (!(is_in_boundingbox(&polys[i].pts[pt1])))
 254                                 continue;
 255
 256                         /* for next poly */
 257                         for (ii=i+1; ii<npolys; ii++) {
 258                                 for (pt2=0;pt2<polys[ii].l;pt2++) {
 259
 260                                         if (!(is_in_boundingbox(&polys[ii].pts[pt2])))
 261                                                 continue;
 262
 263                                         is_ok=1;
 264                                         /* test if a poly cross */
 265                                         for (index=1;index<npolys;index++) {
 266                                                 if (is_crossing_poly(polys[i].pts[pt1],
 267                                                                      polys[ii].pts[pt2], NULL,
 268                                                                      &polys[index]) == 1) {
 269                                                         is_ok=0;
 270                                                         break;
 271                                                 }
 272                                         }
 273                                         /* if not crossed, we found a vilisity ray */
 274                                         if (is_ok) {
 275                                                 rays[ray_n++] = i;
 276                                                 rays[ray_n++] = pt1;
 277                                                 rays[ray_n++] = ii;
 278                                                 rays[ray_n++] = pt2;
 279                                         }
 280                                 }
 281                         }
 282                 }
 283         }
 284
 285
 286         return ray_n;
 287 }
 288
 289 /* Compute the weight of every rays: the length of the rays is used
 290  * here.
 291  *
 292  * Note the +1 is a little hack to introduce a preference between to
 293  * possiblity path: If we have 3 checpoint aligned in a path (say A,
 294  * B, C) the algorithm will prefer (A, C) instead of (A, B, C) */
 295 void
 296 calc_rays_weight(poly_t *polys, uint8_t npolys, uint8_t *rays,
 297                  uint8_t ray_n, uint16_t *weight)
 298 {
 299         uint8_t i;
 300         vect_t v;
 301
 302         for (i=0;i<ray_n;i+=4) {
 303                 v.x = polys[rays[i]].pts[rays[i+1]].x - polys[rays[i+2]].pts[rays[i+3]].x;
 304                 v.y = polys[rays[i]].pts[rays[i+1]].y - polys[rays[i+2]].pts[rays[i+3]].y;
 305                 weight[i/4] = vect_norm(&v) + 1;
 306         }
 307 }
 308
 309
 310