polygon intersector improvements
[swftools.git] / lib / gfxpoly / poly.c
1 #include <stdlib.h>
2 #include <assert.h>
3 #include <memory.h>
4 #include <math.h>
5 #include "../mem.h"
6 #include "../q.h"
7 #include "poly.h"
8 #include "active.h"
9 #include "xrow.h"
10
11 //#define DEBUG
12 //#undef assert
13 //#define assert(x) 
14
15 char point_equals(const void*o1, const void*o2) 
16 {
17     const point_t*p1 = o1;
18     const point_t*p2 = o2;
19     return p1->x == p2->x && p1->y == p2->y;
20 }
21 unsigned int point_hash(const void*o) 
22 {
23     const point_t*p = o;
24     return p->x^p->y;
25 }
26 void* point_dup(const void*o) 
27 {
28     const point_t*p = o;
29     point_t*n = malloc(sizeof(point_t));
30     n->x = p->x;
31     n->y = p->y;
32     return n;
33 }
34 void point_free(void*o) 
35 {
36     point_t*p = o;
37     p->x = 0;
38     p->y = 0;
39     free(p);
40 }
41 type_t point_type = {
42     equals: point_equals,
43     hash: point_hash,
44     dup: point_dup,
45     free: point_free,
46 };
47
48 typedef struct _status {
49     int y;
50     actlist_t*actlist;
51     heap_t*queue;
52     edge_t*output;
53     xrow_t*xrow;
54 #ifdef DEBUG
55     dict_t*seen_crossings; //list of crossing we saw so far
56     dict_t*intersecting_segs; //list of segments intersecting in this scanline
57     dict_t*segs_with_point; //lists of segments that received a point in this scanline
58 #endif
59 } status_t;
60
61 int compare_events(const void*_a,const void*_b)
62 {
63     event_t* a = (event_t*)_a;
64     event_t* b = (event_t*)_b; 
65     if(a->p.y < b->p.y) {
66         return 1;
67     } else if(a->p.y > b->p.y) {
68         return -1;
69     /* we should schedule start events after end/intersect.
70        The order of end/intersect doesn't actually matter, however,
71        so this might be doing too much */
72     } else if(a->type < b->type) {
73         return 1;
74     } else if(a->type > b->type) {
75         return -1;
76     } else if(a->p.x < b->p.x) {
77         return 1;
78     } else if(a->p.x > b->p.x) {
79         return -1;
80     } else
81         return 0;
82 }
83
84 gfxpoly_t* gfxpoly_new()
85 {
86     return 0;
87 }
88 void gfxpoly_destroy(gfxpoly_t*poly)
89 {
90     edge_t* s = poly;
91     while(s) {
92         edge_t*next  = s->next;
93         free(s);
94         s = next;
95     }
96 }
97
98 void gfxpoly_dump(gfxpoly_t*poly)
99 {
100     edge_t* s = (edge_t*)poly;
101     while(s) {
102         fprintf(stderr, "(%d,%d) -> (%d,%d)\n", s->a.x, s->a.y, s->b.x, s->b.y);
103         s = s->next;
104     }
105 }
106
107 inline static event_t event_new()
108 {
109     event_t e;
110     memset(&e, 0, sizeof(e));
111     return e;
112 }
113
114 void event_dump(event_t*e)
115 {
116     if(e->type == EVENT_HORIZONTAL) {
117         fprintf(stderr, "Horizontal [%d] (%d,%d) -> (%d,%d)\n", e->s1->nr, e->s1->a.x, e->s1->a.y, e->s1->b.x, e->s1->b.y);
118     } else if(e->type == EVENT_START) {
119         fprintf(stderr, "event: segment [%d] starts at (%d,%d)\n", e->s1->nr, e->p.x, e->p.y);
120     } else if(e->type == EVENT_END) {
121         fprintf(stderr, "event: segment [%d] ends at (%d,%d)\n", e->s1->nr, e->p.x, e->p.y);
122     } else if(e->type == EVENT_CROSS) {
123         fprintf(stderr, "event: segment [%d] and [%d] intersect at (%d,%d)\n", e->s1->nr, e->s2->nr, e->p.x, e->p.y);
124     } else {
125         assert(0);
126     }
127 }
128
129 static inline max32(int32_t v1, int32_t v2) {return v1>v2?v1:v2;}
130 static inline min32(int32_t v1, int32_t v2) {return v1<v2?v1:v2;}
131
132 void segment_init(segment_t*s, int x1, int y1, int x2, int y2)
133 {
134     if(y1<y2) {
135         s->dir = DIR_DOWN;
136     } else if(y1>y2) {
137         int x = x1;x1=x2;x2=x;
138         int y = y1;y1=y2;y2=y;
139         s->dir = DIR_UP;
140     } else {
141         /* up/down for horizontal segments is handled by "rotating"
142            them 90° anticlockwise in screen coordinates (tilt your head to 
143            the right) */
144         s->dir = DIR_UP;
145         if(x1>x2) {
146             s->dir = DIR_DOWN;
147             int x = x1;x1=x2;x2=x;
148             int y = y1;y1=y2;y2=y;
149         }
150     }
151     s->a.x = x1;
152     s->a.y = y1;
153     s->b.x = x2;
154     s->b.y = y2;
155     s->k = (double)x1*y2-(double)x2*y1;
156     s->left = s->right = 0;
157     s->delta.x = x2-x1;
158     s->delta.y = y2-y1;
159     s->pos = s->a;
160     s->new_point.y = y1-1;
161 #define XDEBUG
162 #ifdef XDEBUG
163     static int segment_count=0;
164     s->nr = segment_count++;
165 #endif
166
167     assert(LINE_EQ(s->a, s) == 0);
168     assert(LINE_EQ(s->b, s) == 0);
169     
170     /* check that all signs are in order:
171        a        a
172        |\      /|
173        | \    / |
174      minx-b  b--maxx
175      < 0        > 0
176     */
177     point_t p = s->b;
178     p.x = min32(s->a.x, s->b.x);
179     assert(LINE_EQ(p, s) <= 0);
180     p.x = max32(s->a.x, s->b.x);
181     assert(LINE_EQ(p, s) >= 0);
182
183     dict_init2(&s->scheduled_crossings, &ptr_type, 0);
184 }
185
186 segment_t* segment_new(int32_t x1, int32_t y1, int32_t x2, int32_t y2)
187 {
188     segment_t*s = (segment_t*)rfx_calloc(sizeof(segment_t));
189     segment_init(s, x1, y1, x2, y2);
190     return s;
191 }
192 void segment_destroy(segment_t*s)
193 {
194     dict_clear(&s->scheduled_crossings);
195     free(s);
196 }
197
198 void gfxpoly_enqueue(edge_t*list, heap_t*queue)
199 {
200     edge_t*l;
201     for(l=list;l;l=l->next) {
202         if(l->a.x == l->b.x && 
203            l->a.y == l->b.y) {
204             fprintf(stderr, "Warning: intersector input contains zero-length segments\n");
205             continue;
206         }
207         segment_t*s = segment_new(l->a.x, l->a.y, l->b.x, l->b.y);
208 #ifdef DEBUG
209         fprintf(stderr, "[%d] (%d,%d) -> (%d,%d) %s\n",
210                 s->nr, s->a.x, s->a.y, s->b.x, s->b.y,
211                 s->dir==DIR_UP?"up":"down");
212 #endif
213         event_t e = event_new();
214         e.type = s->delta.y ? EVENT_START : EVENT_HORIZONTAL;
215         e.p = s->a;
216         e.s1 = s;
217         e.s2 = 0;
218         heap_put(queue, &e);
219     }
220 }
221
222 void schedule_endpoint(status_t*status, segment_t*s)
223 {
224     // schedule end point of segment
225     assert(s->b.y > status->y);
226     event_t e;
227     e.type = EVENT_END;
228     e.p = s->b;
229     e.s1 = s;
230     e.s2 = 0;
231     heap_put(status->queue, &e);
232 }
233
234 void schedule_crossing(status_t*status, segment_t*s1, segment_t*s2)
235 {
236     /* the code that's required (and the checks you can perform) before
237        it can be said with 100% certainty that we indeed have a valid crossing
238        amazes me every time. -mk */
239     assert(s1!=s2);
240
241     /* we probably could precompute these */
242     int32_t minx1 = min32(s1->a.x,s1->b.x);
243     int32_t miny1 = min32(s1->a.y,s1->b.y);
244     int32_t maxx1 = max32(s1->a.x,s1->b.x);
245     int32_t maxy1 = max32(s1->a.y,s1->b.y);
246     int32_t minx2 = min32(s2->a.x,s2->b.x);
247     int32_t miny2 = min32(s2->a.y,s2->b.y);
248     int32_t maxx2 = max32(s2->a.x,s2->b.x);
249     int32_t maxy2 = max32(s2->a.y,s2->b.y);
250       
251     /* both segments are active, so this can't happen */
252     assert(!(maxy1 <= miny2 || maxy2 <= miny1));
253
254     /* TODO: optimize this. remove y, precompute the two x values */
255     if(maxx1 <= minx2 || maxx2 <= minx1 ||
256        maxy1 <= miny2 || maxy2 <= miny1) {
257         /* bounding boxes don't intersect */
258         return;
259     }
260     
261     if(dict_contains(&s1->scheduled_crossings, s2)) {
262         /* FIXME: this whole segment hashing thing is really slow */
263         //fprintf(stderr, "Encountered crossing between [%d] and [%d] twice\n", s1->nr, s2->nr);
264
265         // we already know about this one
266         return;
267     }
268
269     double adx = s1->delta.x;
270     double ady = s1->delta.y;
271     double bdx = s2->delta.x;
272     double bdy = s2->delta.y;
273     double det = adx*bdy - ady*bdx;
274     if(!det) {
275         if(s1->k == s2->k) {
276             // lines are exactly on top of each other (ignored)
277 #ifdef DEBUG
278             fprintf(stderr, "Notice: segments [%d] and [%d] are exactly on top of each other\n", s1->nr, s2->nr);
279 #endif
280             return;
281         } else {
282             /* lines are parallel */
283             return;
284         }
285     }
286     double asign2 = LINE_EQ(s1->a, s2);
287     double bsign2 = LINE_EQ(s1->b, s2);
288     if(asign2<0 && bsign2<0) {
289         // segment1 is completely to the left of segment2
290         return;
291     }
292     if(asign2>0 && bsign2>0)  {
293         // segment2 is completely to the left of segment1
294         return;
295     }
296     if(asign2==0) {
297         // segment1 touches segment2 in a single point (ignored)
298 #ifdef DEBUG
299         fprintf(stderr, "Notice: segment [%d]'s start point touches segment [%d]\n", s1->nr, s2->nr);
300 #endif
301         return;
302     }
303     if(bsign2==0) {
304         // segment1 touches segment2 in a single point (ignored)
305 #ifdef DEBUG
306         fprintf(stderr, "Notice: segment [%d]'s end point touches segment [%d]\n", s1->nr, s2->nr);
307 #endif
308         return;
309     }
310     double asign1 = LINE_EQ(s2->a, s1);
311     double bsign1 = LINE_EQ(s2->b, s1);
312     if(asign1<0 && bsign1<0) {
313         // segment1 is completely to the left of segment2
314         return;
315     }
316     if(asign1>0 && bsign1>0)  {
317         // segment2 is completely to the left of segment1
318         return;
319     }
320     if(asign1==0) {
321         // segment2 touches segment1 in a single point (ignored)
322 #ifdef DEBUG
323         fprintf(stderr, "Notice: segment [%d]'s start point touches segment [%d]\n", s2->nr, s1->nr);
324 #endif
325         return;
326     }
327     if(asign2==0) {
328         // segment2 touches segment1 in a single point (ignored)
329 #ifdef DEBUG
330         fprintf(stderr, "Notice: segment [%d]'s end point touches segment [%d]\n", s2->nr, s1->nr);
331 #endif
332         return;
333     }
334
335     double la = (double)s1->a.x*(double)s1->b.y - (double)s1->a.y*(double)s1->b.x;
336     double lb = (double)s2->a.x*(double)s2->b.y - (double)s2->a.y*(double)s2->b.x;
337
338     point_t p;
339     p.x = (int32_t)ceil((-la*bdx +lb*adx) / det);
340     p.y = (int32_t)ceil((+lb*ady -la*bdy) / det);
341
342     assert(p.y >= status->y);
343 #ifdef DEBUG
344     point_t pair;
345     pair.x = s1->nr;
346     pair.y = s2->nr;
347     assert(!dict_contains(status->seen_crossings, &pair));
348     dict_put(status->seen_crossings, &pair, 0);
349     fprintf(stderr, "schedule crossing between [%d] and [%d] at (%d,%d)\n", s1->nr, s2->nr, p.x, p.y);
350 #endif
351
352     /* we insert into each other's intersection history because these segments might switch
353        places and we still want to look them up quickly after they did */
354     dict_put(&s1->scheduled_crossings, s2, 0);
355     dict_put(&s2->scheduled_crossings, s1, 0);
356
357     event_t e = event_new();
358     e.type = EVENT_CROSS;
359     e.p = p;
360     e.s1 = s1;
361     e.s2 = s2;
362     heap_put(status->queue, &e);
363     return;
364 }
365
366 void exchange_two(status_t*status, event_t*e)
367 {
368     //exchange two segments in list
369     segment_t*s1 = e->s1;
370     segment_t*s2 = e->s2;
371 #ifdef DEBUG
372     if(!dict_contains(status->intersecting_segs, s1))
373         dict_put(status->intersecting_segs, s1, 0);
374     if(!dict_contains(status->intersecting_segs, s2))
375         dict_put(status->intersecting_segs, s2, 0);
376 #endif
377     segment_t*left = actlist_left(status->actlist, s2);
378     segment_t*right = actlist_right(status->actlist, s1);
379     assert(left == s1);
380     assert(right == s2);
381     actlist_swap(status->actlist, s1, s2);
382     assert(actlist_right(status->actlist, s2) == s1);
383     assert(actlist_left(status->actlist, s1) == s2);
384     left = actlist_left(status->actlist, s2);
385     right = actlist_right(status->actlist, s1);
386     if(left)
387         schedule_crossing(status, left, s2);
388     if(right)
389         schedule_crossing(status, s1, right);
390 }
391
392 typedef struct _box {
393     point_t left1, left2, right1, right2;
394 } box_t;
395 static inline box_t box_new(int x, int y)
396 {
397     box_t box;
398     box.right1.x = box.right2.x = x;
399     box.left1.x = box.left2.x = x-1;
400     box.left1.y = box.right1.y = y-1;
401     box.left2.y = box.right2.y = y;
402     return box;
403 }
404
405 void insert_point_into_segment(status_t*status, segment_t*s, point_t p)
406 {
407     edge_t*e = malloc(sizeof(edge_t));
408     e->a = s->pos;
409     e->b = p;
410     assert(e->a.y != e->b.y);
411     e->next = status->output;
412     status->output = e;
413 }
414
415 void mark_point_in_segment(status_t*status, segment_t*s, point_t p)
416 {
417 #ifdef DEBUG
418     if(s->pos.x == p.x && s->pos.y == p.y) {
419         fprintf(stderr, "Error: tried to add (%d,%d) to segment [%d] twice\n", p.x, p.y, s->nr);
420     }
421 #endif
422     assert(s->pos.x != p.x || s->pos.y != p.y);
423 #ifdef DEBUG
424     fprintf(stderr, "[%d] gets extra point (%d,%d)\n", s->nr, p.x, p.y);
425     if(!dict_contains(status->segs_with_point, s))
426         dict_put(status->segs_with_point, s, 0);
427 #endif
428     if(s->new_point.y != p.y) {
429         s->new_point = p;
430     }
431     s->new_pos = p;
432 }
433
434 /* possible optimizations:
435    1.) keep two different active lists around, one for negative and one for
436        positive slopes
437    2.) delay starting events until after this function (tricky, because we *do*
438        need the start coordinates)
439 */
440 /*
441    SLOPE_POSITIVE:
442       \+     \ +        
443 ------ I      \I       
444       -I\----  I      
445        I \   --I\---
446        I  \    I \  -------
447        +   \   +  \
448 */
449 static void mark_points_in_positively_sloped_segments(status_t*status, int32_t y)
450 {
451     int t;
452     for(t=0;t<status->xrow->num;t++) {
453         box_t box = box_new(status->xrow->x[t], y);
454         segment_t*seg = actlist_find(status->actlist, box.left2, box.left2);
455         seg = actlist_right(status->actlist, seg);
456         while(seg) {
457             if(seg->a.y == y) {
458                 // this segment just started, ignore it
459             } else if(seg->delta.x < 0) {
460                 // ignore segment w/ negative slope
461             } else {
462                 double d1 = LINE_EQ(box.right1, seg);
463                 double d2 = LINE_EQ(box.right2, seg);
464                 if(d1>=0 || d2>=0) {
465                     mark_point_in_segment(status, seg, box.right2);
466                 } else {
467                     break;
468                 }
469             }
470             seg = actlist_right(status->actlist, seg);
471         }
472     }
473 }
474 /* SLOPE_NEGATIVE:
475    |   +   /|  +  /    /
476    |   I  / |  I /    /
477    |   I /  |  I/    /
478    |   I/   |  I    /
479    |   I    | /I   /
480    |  /+    |/ +  /
481 */
482 static void mark_points_in_negatively_sloped_segments(status_t*status, int32_t y)
483 {
484     int t;
485     for(t=status->xrow->num-1;t>=0;t--) {
486         box_t box = box_new(status->xrow->x[t], y);
487         segment_t*seg = actlist_find(status->actlist, box.right2, box.right2);
488         while(seg) {
489             if(seg->a.y == y) {
490                 // this segment just started, ignore it
491             } else if(seg->delta.x >= 0) {
492                 // ignore segment w/ positive slope
493             } else {
494                 double d1 = LINE_EQ(box.left1, seg);
495                 double d2 = LINE_EQ(box.left2, seg);
496                 if(d1<0 || d2<0) {
497                     mark_point_in_segment(status, seg, box.right2);
498                 } else {
499                     break;
500                 }
501             }
502             seg = actlist_left(status->actlist, seg);
503         }
504     }
505 }
506
507 static void add_points(status_t*status)
508 {
509     /* TODO: we could use some clever second linked list structure so that we
510              only need to process points which we know we marked */
511     int t;
512     segment_t*s = actlist_leftmost(status->actlist);
513     while(s) {
514         if(s->new_point.y == status->y) {
515             insert_point_into_segment(status, s, s->new_point);
516             s->pos = s->new_pos;
517         }
518         s = actlist_right(status->actlist, s);
519     }
520 }
521
522 void intersect_with_horizontal(status_t*status, segment_t*h)
523 {
524     segment_t* left = actlist_find(status->actlist, h->a, h->a);
525     segment_t* right = actlist_find(status->actlist, h->b, h->b);
526
527     segment_t* s = right;
528
529     while(s!=left) {
530         assert(s);
531         /*
532            x1 + ((x2-x1)*(y-y1)) / dy = 
533            (x1*(y2-y) + x2*(y-y1)) / dy
534         */
535         point_t p;
536         p.y = status->y;
537         p.x = XPOS(s, p.y);
538 #ifdef DEBUG
539         fprintf(stderr, "...into [%d] (%d,%d) -> (%d,%d) at (%d,%d)\n", s->nr, 
540                 s->a.x, s->a.y,
541                 s->b.x, s->b.y,
542                 p.x, p.y
543                 );
544 #endif
545         assert(p.x >= h->a.x);
546         assert(p.x <= h->b.x);
547         assert(s->delta.x > 0 && p.x >= s->a.x || s->delta.x <= 0 && p.x <= s->a.x);
548         assert(s->delta.x > 0 && p.x <= s->b.x || s->delta.x <= 0 && p.x >= s->b.x);
549         xrow_add(status->xrow, p.x);
550
551         s = actlist_left(status->actlist, s);
552     }
553     xrow_add(status->xrow, h->a.x);
554 }
555
556 void event_apply(status_t*status, event_t*e)
557 {
558     switch(e->type) {
559         case EVENT_HORIZONTAL: {
560 #ifdef DEBUG
561             event_dump(e);
562 #endif
563             intersect_with_horizontal(status, e->s1);
564             break;
565         }
566         case EVENT_END: {
567             //delete segment from list
568             segment_t*s = e->s1;
569 #ifdef DEBUG
570             event_dump(e);
571             dict_del(status->intersecting_segs, s);
572             dict_del(status->segs_with_point, s);
573             assert(!dict_contains(status->intersecting_segs, s));
574             assert(!dict_contains(status->segs_with_point, s));
575 #endif
576             insert_point_into_segment(status, s, s->b);
577             segment_t*left = actlist_left(status->actlist, s);
578             segment_t*right = actlist_right(status->actlist, s);
579             actlist_delete(status->actlist, s);
580             if(left && right)
581                 schedule_crossing(status, left, right);
582             segment_destroy(s);e->s1=0;
583             break;
584         }
585         case EVENT_START: {
586             //insert segment into list
587 #ifdef DEBUG
588             event_dump(e);
589 #endif
590             segment_t*s = e->s1;
591             actlist_insert(status->actlist, e->p, s);
592             segment_t*left = actlist_left(status->actlist, s);
593             segment_t*right = actlist_right(status->actlist, s);
594             if(left)
595                 schedule_crossing(status, left, s);
596             if(right)
597                 schedule_crossing(status, s, right);
598
599             schedule_endpoint(status, e->s1);
600             break;
601         }
602         case EVENT_CROSS: {
603             // exchange two (or more) segments
604             if(actlist_right(status->actlist, e->s1) == e->s2 &&
605                actlist_left(status->actlist, e->s2) == e->s1) {
606                 exchange_two(status, e);
607             } else {
608                 /* ignore this crossing for now (there are some line segments in between).
609                    it'll get rescheduled as soon as the "obstacles" are gone */
610                 char del1 = dict_del(&e->s1->scheduled_crossings, e->s2);
611                 char del2 = dict_del(&e->s2->scheduled_crossings, e->s1);
612                 assert(del1 && del2);
613 #ifdef DEBUG
614                 point_t pair;
615                 pair.x = e->s1->nr;
616                 pair.y = e->s2->nr;
617                 assert(dict_contains(status->seen_crossings, &pair));
618                 dict_del(status->seen_crossings, &pair);
619 #endif
620             }
621         }
622     }
623 }
624
625 #ifdef DEBUG
626 void check_status(status_t*status)
627 {
628     DICT_ITERATE_KEY(status->intersecting_segs, segment_t*, s) {
629         if((s->pos.x != s->b.x ||
630             s->pos.y != s->b.y) && 
631            !dict_contains(status->segs_with_point, s)) {
632             fprintf(stderr, "Error: segment [%d] (%sslope) intersects in scanline %d, but it didn't receive a point\n", 
633                     s->nr, 
634                     s->delta.x<0?"-":"+",
635                     status->y);
636             assert(0);
637         }
638     }
639 }
640 #endif
641
642 edge_t* gfxpoly_process(edge_t*poly)
643 {
644     heap_t* queue = heap_new(sizeof(event_t), compare_events);
645     gfxpoly_enqueue(poly, queue);
646     status_t status;
647     memset(&status, 0, sizeof(status_t));
648     status.queue = queue;
649     status.actlist = actlist_new();
650 #ifdef DEBUG
651     status.seen_crossings = dict_new2(&point_type);
652     gfxpoly_dump(poly);
653 #endif
654     
655     status.xrow = xrow_new();
656
657     event_t*e = heap_chopmax(queue);
658     while(e) {
659         status.y = e->p.y;
660 #ifdef DEBUG
661         status.intersecting_segs = dict_new2(&ptr_type);
662         status.segs_with_point = dict_new2(&ptr_type);
663         fprintf(stderr, "----------------------------------- %d\n", status.y);
664         actlist_verify_and_dump(status.actlist, status.y-1);
665 #endif
666         xrow_reset(status.xrow);
667         do {
668             if(e->type != EVENT_HORIZONTAL) {
669                 xrow_add(status.xrow, e->p.x);
670             }
671             event_apply(&status, e);
672             free(e);
673             e = heap_chopmax(queue);
674         } while(e && status.y == e->p.y);
675
676         xrow_sort(status.xrow);
677         mark_points_in_positively_sloped_segments(&status, status.y);
678         mark_points_in_negatively_sloped_segments(&status, status.y);
679         add_points(&status);
680 #ifdef DEBUG
681         check_status(&status);
682         dict_destroy(status.intersecting_segs);
683         dict_destroy(status.segs_with_point);
684 #endif
685     }
686 #ifdef DEBUG
687     dict_destroy(status.seen_crossings);
688 #endif
689     actlist_destroy(status.actlist);
690     heap_destroy(queue);
691     xrow_destroy(status.xrow);
692
693     return status.output;
694 }