1/* $Id$ $Revision$ */
2/* vim:set shiftwidth=4 ts=8: */
3
4/*************************************************************************
5 * Copyright (c) 2011 AT&T Intellectual Property
6 * All rights reserved. This program and the accompanying materials
7 * are made available under the terms of the Eclipse Public License v1.0
8 * which accompanies this distribution, and is available at
9 * http://www.eclipse.org/legal/epl-v10.html
10 *
11 * Contributors: See CVS logs. Details at http://www.graphviz.org/
12 *************************************************************************/
13
14
15#include "vis.h"
16
17#ifdef DMALLOC
18#include "dmalloc.h"
19#endif
20
21 /* TRANSPARENT means router sees past colinear obstacles */
22#ifdef TRANSPARENT
23#define INTERSECT(a,b,c,d,e) intersect1((a),(b),(c),(d),(e))
24#else
25#define INTERSECT(a,b,c,d,e) intersect((a),(b),(c),(d))
26#endif
27
28/* allocArray:
29 * Allocate a VxV array of COORD values.
30 * (array2 is a pointer to an array of pointers; the array is
31 * accessed in row-major order.)
32 * The values in the array are initialized to 0.
33 * Add extra rows.
34 */
35static array2 allocArray(int V, int extra)
36{
37 int i;
38 array2 arr;
39 COORD *p;
40
41 arr = (COORD **) malloc((V + extra) * sizeof(COORD *));
42 p = (COORD *) calloc(V * V, sizeof(COORD));
43 for (i = 0; i < V; i++) {
44 arr[i] = p;
45 p += V;
46 }
47 for (i = V; i < V + extra; i++)
48 arr[i] = (COORD *) 0;
49
50 return arr;
51}
52
53/* area2:
54 * Returns twice the area of triangle abc.
55 */
56COORD area2(Ppoint_t a, Ppoint_t b, Ppoint_t c)
57{
58 return ((a.y - b.y) * (c.x - b.x) - (c.y - b.y) * (a.x - b.x));
59}
60
61/* wind:
62 * Returns 1, 0, -1 if the points abc are counterclockwise,
63 * collinear, or clockwise.
64 */
65int wind(Ppoint_t a, Ppoint_t b, Ppoint_t c)
66{
67 COORD w;
68
69 w = ((a.y - b.y) * (c.x - b.x) - (c.y - b.y) * (a.x - b.x));
70 /* need to allow for small math errors. seen with "gcc -O2 -mcpu=i686 -ffast-math" */
71 return (w > .0001) ? 1 : ((w < -.0001) ? -1 : 0);
72}
73
74#if 0 /* NOT USED */
75/* open_intersect:
76 * Returns true iff segment ab intersects segment cd.
77 * NB: segments are considered open sets
78 */
79static int open_intersect(Ppoint_t a, Ppoint_t b, Ppoint_t c, Ppoint_t d)
80{
81 return (((area2(a, b, c) > 0 && area2(a, b, d) < 0) ||
82 (area2(a, b, c) < 0 && area2(a, b, d) > 0))
83 &&
84 ((area2(c, d, a) > 0 && area2(c, d, b) < 0) ||
85 (area2(c, d, a) < 0 && area2(c, d, b) > 0)));
86}
87#endif
88
89/* inBetween:
90 * Return true if c is in (a,b), assuming a,b,c are collinear.
91 */
92int inBetween(Ppoint_t a, Ppoint_t b, Ppoint_t c)
93{
94 if (a.x != b.x) /* not vertical */
95 return (((a.x < c.x) && (c.x < b.x))
96 || ((b.x < c.x) && (c.x < a.x)));
97 else
98 return (((a.y < c.y) && (c.y < b.y))
99 || ((b.y < c.y) && (c.y < a.y)));
100}
101
102 /* TRANSPARENT means router sees past colinear obstacles */
103#ifdef TRANSPARENT
104/* intersect1:
105 * Returns true if the polygon segment [q,n) blocks a and b from seeing
106 * each other.
107 * More specifically, returns true iff the two segments intersect as open
108 * sets, or if q lies on (a,b) and either n and p lie on
109 * different sides of (a,b), i.e., wind(a,b,n)*wind(a,b,p) < 0, or the polygon
110 * makes a left turn at q, i.e., wind(p,q,n) > 0.
111 *
112 * We are assuming the p,q,n are three consecutive vertices of a barrier
113 * polygon with the polygon interior to the right of p-q-n.
114 *
115 * Note that given the constraints of our problem, we could probably
116 * simplify this code even more. For example, if abq are collinear, but
117 * q is not in (a,b), we could return false since n will not be in (a,b)
118 * nor will the (a,b) intersect (q,n).
119 *
120 * Also note that we are computing w_abq twice in a tour of a polygon,
121 * once for each edge of which it is a vertex.
122 */
123static int intersect1(Ppoint_t a, Ppoint_t b, Ppoint_t q, Ppoint_t n,
124 Ppoint_t p)
125{
126 int w_abq;
127 int w_abn;
128 int w_qna;
129 int w_qnb;
130
131 w_abq = wind(a, b, q);
132 w_abn = wind(a, b, n);
133
134 /* If q lies on (a,b),... */
135 if ((w_abq == 0) && inBetween(a, b, q)) {
136 return ((w_abn * wind(a, b, p) < 0) || (wind(p, q, n) > 0));
137 } else {
138 w_qna = wind(q, n, a);
139 w_qnb = wind(q, n, b);
140 /* True if q and n are on opposite sides of ab,
141 * and a and b are on opposite sides of qn.
142 */
143 return (((w_abq * w_abn) < 0) && ((w_qna * w_qnb) < 0));
144 }
145}
146#else
147
148/* intersect:
149 * Returns true if the segment [c,d] blocks a and b from seeing each other.
150 * More specifically, returns true iff c or d lies on (a,b) or the two
151 * segments intersect as open sets.
152 */
153int intersect(Ppoint_t a, Ppoint_t b, Ppoint_t c, Ppoint_t d)
154{
155 int a_abc;
156 int a_abd;
157 int a_cda;
158 int a_cdb;
159
160 a_abc = wind(a, b, c);
161 if ((a_abc == 0) && inBetween(a, b, c)) {
162 return 1;
163 }
164 a_abd = wind(a, b, d);
165 if ((a_abd == 0) && inBetween(a, b, d)) {
166 return 1;
167 }
168 a_cda = wind(c, d, a);
169 a_cdb = wind(c, d, b);
170
171 /* True if c and d are on opposite sides of ab,
172 * and a and b are on opposite sides of cd.
173 */
174 return (((a_abc * a_abd) < 0) && ((a_cda * a_cdb) < 0));
175}
176#endif
177
178/* in_cone:
179 * Returns true iff point b is in the cone a0,a1,a2
180 * NB: the cone is considered a closed set
181 */
182static int in_cone(Ppoint_t a0, Ppoint_t a1, Ppoint_t a2, Ppoint_t b)
183{
184 int m = wind(b, a0, a1);
185 int p = wind(b, a1, a2);
186
187 if (wind(a0, a1, a2) > 0)
188 return (m >= 0 && p >= 0); /* convex at a */
189 else
190 return (m >= 0 || p >= 0); /* reflex at a */
191}
192
193#if 0 /* NOT USED */
194/* in_open_cone:
195 * Returns true iff point b is in the cone a0,a1,a2
196 * NB: the cone is considered an open set
197 */
198static int in_open_cone(Ppoint_t a0, Ppoint_t a1, Ppoint_t a2, Ppoint_t b)
199{
200 int m = wind(b, a0, a1);
201 int p = wind(b, a1, a2);
202
203 if (wind(a0, a1, a2) >= 0)
204 return (m > 0 && p > 0); /* convex at a */
205 else
206 return (m > 0 || p > 0); /* reflex at a */
207}
208#endif
209
210/* dist2:
211 * Returns the square of the distance between points a and b.
212 */
213COORD dist2(Ppoint_t a, Ppoint_t b)
214{
215 COORD delx = a.x - b.x;
216 COORD dely = a.y - b.y;
217
218 return (delx * delx + dely * dely);
219}
220
221/* dist:
222 * Returns the distance between points a and b.
223 */
224static COORD dist(Ppoint_t a, Ppoint_t b)
225{
226 return sqrt(dist2(a, b));
227}
228
229static int inCone(int i, int j, Ppoint_t pts[], int nextPt[], int prevPt[])
230{
231 return in_cone(pts[prevPt[i]], pts[i], pts[nextPt[i]], pts[j]);
232}
233
234/* clear:
235 * Return true if no polygon line segment non-trivially intersects
236 * the segment [pti,ptj], ignoring segments in [start,end).
237 */
238static int clear(Ppoint_t pti, Ppoint_t ptj,
239 int start, int end,
240 int V, Ppoint_t pts[], int nextPt[], int prevPt[])
241{
242 int k;
243
244 for (k = 0; k < start; k++) {
245 if (INTERSECT(pti, ptj, pts[k], pts[nextPt[k]], pts[prevPt[k]]))
246 return 0;
247 }
248 for (k = end; k < V; k++) {
249 if (INTERSECT(pti, ptj, pts[k], pts[nextPt[k]], pts[prevPt[k]]))
250 return 0;
251 }
252 return 1;
253}
254
255/* compVis:
256 * Compute visibility graph of vertices of polygons.
257 * Only do polygons from index startp to end.
258 * If two nodes cannot see each other, the matrix entry is 0.
259 * If two nodes can see each other, the matrix entry is the distance
260 * between them.
261 */
262static void compVis(vconfig_t * conf, int start)
263{
264 int V = conf->N;
265 Ppoint_t *pts = conf->P;
266 int *nextPt = conf->next;
267 int *prevPt = conf->prev;
268 array2 wadj = conf->vis;
269 int j, i, previ;
270 COORD d;
271
272 for (i = start; i < V; i++) {
273 /* add edge between i and previ.
274 * Note that this works for the cases of polygons of 1 and 2
275 * vertices, though needless work is done.
276 */
277 previ = prevPt[i];
278 d = dist(pts[i], pts[previ]);
279 wadj[i][previ] = d;
280 wadj[previ][i] = d;
281
282 /* Check remaining, earlier vertices */
283 if (previ == i - 1)
284 j = i - 2;
285 else
286 j = i - 1;
287 for (; j >= 0; j--) {
288 if (inCone(i, j, pts, nextPt, prevPt) &&
289 inCone(j, i, pts, nextPt, prevPt) &&
290 clear(pts[i], pts[j], V, V, V, pts, nextPt, prevPt)) {
291 /* if i and j see each other, add edge */
292 d = dist(pts[i], pts[j]);
293 wadj[i][j] = d;
294 wadj[j][i] = d;
295 }
296 }
297 }
298}
299
300/* visibility:
301 * Given a vconfig_t conf, representing polygonal barriers,
302 * compute the visibility graph of the vertices of conf.
303 * The graph is stored in conf->vis.
304 */
305void visibility(vconfig_t * conf)
306{
307 conf->vis = allocArray(conf->N, 2);
308 compVis(conf, 0);
309}
310
311/* polyhit:
312 * Given a vconfig_t conf, as above, and a point,
313 * return the index of the polygon that contains
314 * the point, or else POLYID_NONE.
315 */
316static int polyhit(vconfig_t * conf, Ppoint_t p)
317{
318 int i;
319 Ppoly_t poly;
320
321 for (i = 0; i < conf->Npoly; i++) {
322 poly.ps = &(conf->P[conf->start[i]]);
323 poly.pn = conf->start[i + 1] - conf->start[i];
324 if (in_poly(poly, p))
325 return i;
326 }
327 return POLYID_NONE;
328}
329
330/* ptVis:
331 * Given a vconfig_t conf, representing polygonal barriers,
332 * and a point within one of the polygons, compute the point's
333 * visibility vector relative to the vertices of the remaining
334 * polygons, i.e., pretend the argument polygon is invisible.
335 *
336 * If pp is NIL, ptVis computes the visibility vector for p
337 * relative to all barrier vertices.
338 */
339COORD *ptVis(vconfig_t * conf, int pp, Ppoint_t p)
340{
341 int V = conf->N;
342 Ppoint_t *pts = conf->P;
343 int *nextPt = conf->next;
344 int *prevPt = conf->prev;
345 int k;
346 int start, end;
347 COORD *vadj;
348 Ppoint_t pk;
349 COORD d;
350
351 vadj = (COORD *) malloc((V + 2) * sizeof(COORD));
352
353
354 if (pp == POLYID_UNKNOWN)
355 pp = polyhit(conf, p);
356 if (pp >= 0) {
357 start = conf->start[pp];
358 end = conf->start[pp + 1];
359 } else {
360 start = V;
361 end = V;
362 }
363
364 for (k = 0; k < start; k++) {
365 pk = pts[k];
366 if (in_cone(pts[prevPt[k]], pk, pts[nextPt[k]], p) &&
367 clear(p, pk, start, end, V, pts, nextPt, prevPt)) {
368 /* if p and pk see each other, add edge */
369 d = dist(p, pk);
370 vadj[k] = d;
371 } else
372 vadj[k] = 0;
373 }
374
375 for (k = start; k < end; k++)
376 vadj[k] = 0;
377
378 for (k = end; k < V; k++) {
379 pk = pts[k];
380 if (in_cone(pts[prevPt[k]], pk, pts[nextPt[k]], p) &&
381 clear(p, pk, start, end, V, pts, nextPt, prevPt)) {
382 /* if p and pk see each other, add edge */
383 d = dist(p, pk);
384 vadj[k] = d;
385 } else
386 vadj[k] = 0;
387 }
388 vadj[V] = 0;
389 vadj[V + 1] = 0;
390
391 return vadj;
392
393}
394
395/* directVis:
396 * Given two points, return true if the points can directly see each other.
397 * If a point is associated with a polygon, the edges of the polygon
398 * are ignored when checking visibility.
399 */
400int directVis(Ppoint_t p, int pp, Ppoint_t q, int qp, vconfig_t * conf)
401{
402 int V = conf->N;
403 Ppoint_t *pts = conf->P;
404 int *nextPt = conf->next;
405 /* int* prevPt = conf->prev; */
406 int k;
407 int s1, e1;
408 int s2, e2;
409
410 if (pp < 0) {
411 s1 = 0;
412 e1 = 0;
413 if (qp < 0) {
414 s2 = 0;
415 e2 = 0;
416 } else {
417 s2 = conf->start[qp];
418 e2 = conf->start[qp + 1];
419 }
420 } else if (qp < 0) {
421 s1 = 0;
422 e1 = 0;
423 s2 = conf->start[pp];
424 e2 = conf->start[pp + 1];
425 } else if (pp <= qp) {
426 s1 = conf->start[pp];
427 e1 = conf->start[pp + 1];
428 s2 = conf->start[qp];
429 e2 = conf->start[qp + 1];
430 } else {
431 s1 = conf->start[qp];
432 e1 = conf->start[qp + 1];
433 s2 = conf->start[pp];
434 e2 = conf->start[pp + 1];
435 }
436
437 for (k = 0; k < s1; k++) {
438 if (INTERSECT(p, q, pts[k], pts[nextPt[k]], pts[prevPt[k]]))
439 return 0;
440 }
441 for (k = e1; k < s2; k++) {
442 if (INTERSECT(p, q, pts[k], pts[nextPt[k]], pts[prevPt[k]]))
443 return 0;
444 }
445 for (k = e2; k < V; k++) {
446 if (INTERSECT(p, q, pts[k], pts[nextPt[k]], pts[prevPt[k]]))
447 return 0;
448 }
449 return 1;
450}
451