modules/base/math/geometry/polygon.c

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