/[zanavi_public1]/navit/navit/maptool/refine/visibility.c
ZANavi

Contents of /navit/navit/maptool/refine/visibility.c

Parent Directory Parent Directory | Revision Log Revision Log


Revision 31 - (show annotations) (download)
Mon Feb 4 17:41:59 2013 UTC (11 years, 1 month ago) by zoff99
File MIME type: text/plain
File size: 14735 byte(s)
new map version, lots of fixes and experimental new features
1 /*
2 * This file is a part of Poly2Tri-C
3 * (c) Barak Itkin <lightningismyname@gmail.com>
4 * http://code.google.com/p/poly2tri-c/
5 *
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without modification,
9 * are permitted provided that the following conditions are met:
10 *
11 * * Redistributions of source code must retain the above copyright notice,
12 * this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above copyright notice,
14 * this list of conditions and the following disclaimer in the documentation
15 * and/or other materials provided with the distribution.
16 * * Neither the name of Poly2Tri nor the names of its contributors may be
17 * used to endorse or promote products derived from this software without specific
18 * prior written permission.
19 *
20 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
23 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
24 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
25 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
26 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
27 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
28 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
29 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
30 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31 */
32
33 /* Given a polygon "Poly" (a list of bounded lines), a point "P" and a
34 * a planar straight line graph (pslg) "PSLG", the question is if
35 * there is a straight line from "P" to some point in/on "Poly" so that
36 * the line does not intersect any of the lines of "PSLG".
37 *
38 * In this file is an algorithm for answering the above question, and
39 * it's pseudo-code is hereby presented:
40 *
41 * IsVisible(G, Poly, P):
42 * ----------------------
43 * { G = (V,E) - The PSLG where E is the set of edges in the graph }
44 * { Poly - A polygon (a list of edges) }
45 * { P - The point we are checking whether is "visible" from }
46 * { Poly }
47 *
48 * W <- Some point in T (for example, center of weight)
49 *
50 * # A set of edges from @PSLG known to intersect some potential lines
51 * # from @P to @Poly
52 * KnownBlocks <- {}
53 *
54 * # A set of points on @Poly which should be checked whether a line
55 * # from them to @P is not obstructed by any of the edges of @PSLG
56 * SecondPoint <- {W}
57 *
58 * WHILE SecondPoint is not empty:
59 *
60 * # Pick some point to check
61 * S <- Some point from SecondPoint
62 * SecondPoint <- SecondPoint \ {S}
63 *
64 * PS <- The infinite line going through P and S
65 *
66 * IF PS intersects @Poly:
67 *
68 * IF there is an edge B=(u,v) (from E) that intersects PS so that
69 * the intersection is between @Poly and @P:
70 *
71 * IF B is not in KnownBlocks:
72 * # Try new lines going through the end points of this edge
73 * SecondPoint <- SecondPoint + {u,v}
74 * KnownBlocks <- KnownBlocks + {B}
75 *
76 * ELSE:
77 * # Nothing to do - we already tried/are trying to handle this
78 * # blocking edge by going around it
79 *
80 * ELSE:
81 * RETURN "Visible"
82 *
83 * ELSE:
84 * # PS doesn't help anyway, no matter if it's blocked or not,
85 * # since it does not reach the polygon
86 *
87 * RETURN "Ocluded"
88 */
89
90 #include <glib.h>
91 #include "bounded-line.h"
92 #include "pslg.h"
93
94
95 static gboolean
96 find_closest_intersection (P2trPSLG *pslg,
97 P2trLine *line,
98 P2trVector2 *close_to,
99 P2trVector2 *dest)
100 {
101 gdouble distance_sq = G_MAXDOUBLE;
102 gboolean found = FALSE;
103
104 const P2trBoundedLine *pslg_line = NULL;
105 P2trPSLGIter pslg_iter;
106 P2trVector2 temp;
107
108 p2tr_pslg_iter_init (&pslg_iter, pslg);
109
110 while (p2tr_pslg_iter_next (&pslg_iter, &pslg_line))
111 {
112 if (p2tr_line_intersection (&pslg_line->infinite, line, &temp)
113 == P2TR_LINE_RELATION_INTERSECTING)
114 {
115 gdouble test_distance_sq = P2TR_VECTOR2_DISTANCE_SQ (&temp, close_to);
116
117 found = TRUE;
118
119 if (test_distance_sq < distance_sq)
120 {
121 distance_sq = test_distance_sq;
122 }
123 }
124 }
125
126 if (found && dest != NULL)
127 p2tr_vector2_copy (dest, &temp);
128
129 return found;
130 }
131
132 static void
133 find_point_in_polygon (P2trPSLG *polygon,
134 P2trVector2 *out_point)
135 {
136 P2trPSLGIter iter;
137 const P2trBoundedLine *line = NULL;
138
139 g_assert (p2tr_pslg_size (polygon) >= 1);
140
141 p2tr_pslg_iter_init (&iter, polygon);
142 p2tr_pslg_iter_next (&iter, &line);
143
144 out_point->x = (line->start.x + line->end.x) / 2;
145 out_point->y = (line->start.y + line->end.y) / 2;
146 }
147
148 #ifdef EXPERIMENTAL_VISIBILITY
149 static P2trBoundedLine*
150 pslg_line_intersection (P2trPSLG *pslg,
151 P2trBoundedLine *line)
152 {
153 P2trPSLGIter iter;
154 P2trBoundedLine *pslg_line = NULL;
155
156 p2tr_pslg_iter_init (&iter, pslg);
157 while (p2tr_pslg_iter_next (&iter, &pslg_line))
158 if (p2tr_bounded_line_intersect (line, pslg_line))
159 return pslg_line;
160
161 return NULL;
162 }
163
164 gboolean
165 p2tr_pslg_visibility_check (P2trPSLG *pslg,
166 P2trVector2 *point,
167 P2trPSLG *polygon)
168 {
169 P2trPSLG *known_blocks;
170 GArray *second_points;
171 gboolean found_visibility_path = FALSE;
172
173 /* W <- Some point in T (for example, center of weight) */
174 P2trVector2 W;
175 find_point_in_polygon (polygon, &W);
176
177 /* KnownBlocks <- {} */
178 known_blocks = p2tr_pslg_new ();
179
180 /* SecondPoint <- {W} */
181 second_points = g_array_new (FALSE, FALSE, sizeof(P2trVector2));
182 g_array_append_val (second_points, W);
183
184 while ((! found_visibility_path) && second_points->len > 0)
185 {
186 P2trVector2 S;
187 P2trBoundedLine PS;
188 P2trVector2 poly_intersection;
189
190 /* S <- Some point from SecondPoint */
191 p2tr_vector2_copy (&S, &g_array_index(second_points, P2trVector2, 0));
192 /* SecondPoint <- SecondPoint \ {S} */
193 g_array_remove_index_fast (second_points, 0);
194
195 /* PS <- The infinite line going through P and S */
196 p2tr_bounded_line_init (&PS, &S, point);
197
198 /* IF PS intersects @Poly */
199 if (find_closest_intersection (polygon, &PS.infinite, point, &poly_intersection))
200 {
201 P2trBoundedLine PS_exact, *B;
202
203 /* IF there is an edge B=(u,v) (from E) that intersects PS */
204 p2tr_bounded_line_init (&PS_exact, point, &poly_intersection);
205 B = pslg_line_intersection (pslg, &PS_exact);
206
207 if (B != NULL)
208 {
209 /* IF B is not in KnownBlocks: */
210 if (! p2tr_pslg_contains_line (known_blocks, B))
211 {
212 /* SecondPoint <- SecondPoint + {u,v} */
213 g_array_append_val (second_points, B->start);
214 g_array_append_val (second_points, B->end);
215 /* KnownBlocks <- KnownBlocks + {B} */
216 p2tr_pslg_add_existing_line (known_blocks, B);
217 }
218 }
219 else
220 {
221 found_visibility_path = TRUE;
222 }
223 }
224 }
225
226 g_array_free (second_points, TRUE);
227 p2tr_pslg_free (known_blocks);
228
229 return found_visibility_path;
230 }
231 #else
232
233 /* Refer to the "Ray casting algorithm":
234 * http://en.wikipedia.org/wiki/Point_in_polygon#Ray_casting_algorithm
235 */
236 static gboolean
237 PointIsInsidePolygon (P2trVector2 *vec,
238 P2trPSLG *polygon)
239 {
240 P2trHashSetIter iter;
241 const P2trBoundedLine *polyline = NULL;
242 int count = 0;
243
244 /* So, if we work on the X axis, what we want to check is how many
245 * segments of the polygon cross the line (-inf,y)->(x,y) */
246 p2tr_pslg_iter_init (&iter, polygon);
247 while (p2tr_pslg_iter_next (&iter, &polyline))
248 {
249 if ((polyline->start.y - vec->y) * (polyline->end.y - vec->y) >= 0)
250 continue; /* The line doesn't cross the horizontal line Y = vec->y */
251 if (MIN (polyline->start.x, polyline->end.x) <= vec->x)
252 count++;
253 }
254 return (count % 2) == 1;
255 }
256
257 /* If the bounded line intersects the polygon in more than two places,
258 * then the answer is simply NO - the line is not completly outside the
259 * polygon.
260 * Else, if the bounded line intersects the polygon twice, we analyze
261 * its end points and middle point:
262 * - If both end-points are inside/on:
263 * - Test the midpoint - it it's inside then the line goes inside the
264 * polygon, otherwise it goes outside (remember, we said it
265 * intersects the polygon exactly twice!)
266 * - Otherwise, there must be a subsegment of the poly-line which is
267 * partially inside!
268 * Else, if it intersects exactly once:
269 * - If both end-points are inside/on then it's partially inside.
270 * - If exactly one end-points is inside/on then decide by the middle
271 * point (if it's partially inside then so is the line)
272 * - If both end-points are outside - CAN'T HAPPEN WITH EXACTLY ONE INTERSECTION!
273 * Else, if it does not intersect:
274 * - Test any point inside the bounded line (end-points, middle points,
275 * etc.). If it's inside then the bounded line is inside. Otherwise
276 * it is outside
277 */
278 static gboolean
279 LineIsOutsidePolygon (P2trBoundedLine *line,
280 P2trPSLG *polygon)
281 {
282 P2trHashSetIter iter;
283 const P2trBoundedLine *polyline = NULL;
284 P2trVector2 middle;
285 gint intersection_count = 0, inside_count = 0;
286
287 p2tr_pslg_iter_init (&iter, polygon);
288 while (p2tr_pslg_iter_next (&iter, &polyline))
289 {
290 if (p2tr_bounded_line_intersect (polyline, line))
291 if (++intersection_count > 2)
292 return FALSE;
293 }
294
295 inside_count += (PointIsInsidePolygon (&line->start, polygon) ? 1 : 0);
296 inside_count += (PointIsInsidePolygon (&line->end, polygon) ? 1 : 0);
297
298 /* To decrease the chance of numeric errors when working on the edge points of
299 * the line, the point we will test is the middle of the input line */
300 middle.x = (line->start.x + line->end.x) / 2;
301 middle.y = (line->start.y + line->end.y) / 2;
302
303 if (intersection_count == 2)
304 {
305 if (inside_count == 2) return PointIsInsidePolygon (&middle, polygon);
306 else return TRUE;
307 }
308 else if (intersection_count == 1)
309 {
310 if (inside_count == 2) return TRUE;
311 else return PointIsInsidePolygon (&middle, polygon);
312 }
313 else return inside_count > 0;
314 }
315
316 static gboolean
317 TryVisibilityAroundBlock(P2trPSLG *PSLG,
318 P2trVector2 *P,
319 P2trPSLG *ToSee,
320 P2trPSLG *KnownBlocks,
321 GQueue *BlocksForTest,
322 /* Try on the edges of this block */
323 const P2trBoundedLine *BlockBeingTested,
324 const P2trVector2 *SideOfBlock)
325 {
326 const P2trVector2 *S = SideOfBlock;
327 P2trVector2 ClosestIntersection;
328 P2trBoundedLine PS;
329
330 p2tr_bounded_line_init (&PS, P, S);
331
332 if (find_closest_intersection (ToSee, &PS.infinite, P, &ClosestIntersection))
333 {
334 P2trPSLGIter iter;
335 P2trBoundedLine PK;
336 const P2trBoundedLine *Segment = NULL;
337 p2tr_bounded_line_init (&PK, P, &ClosestIntersection);
338
339 /* Now we must make sure that the bounded line PK is inside
340 * the polygon, because otherwise it is not considered as a
341 * valid visibility path */
342
343 p2tr_pslg_iter_init (&iter, PSLG);
344 while (p2tr_pslg_iter_next (&iter, &Segment))
345 {
346 if (Segment == BlockBeingTested)
347 continue;
348
349 /* If we have two segments with a shared point,
350 * the point should not be blocked by any of them
351 */
352 if (p2tr_vector2_is_same (SideOfBlock, &(Segment->start))
353 || p2tr_vector2_is_same (SideOfBlock, &(Segment->end)))
354 continue;
355
356 if (p2tr_bounded_line_intersect (Segment, &PK))
357 {
358 if (g_queue_find (BlocksForTest, Segment))
359 {
360 g_queue_push_tail (BlocksForTest, (P2trBoundedLine*)Segment);
361 }
362 /* obstruction found! */
363 return FALSE;
364 }
365 }
366
367 if (LineIsOutsidePolygon (&PK, PSLG))
368 return FALSE;
369
370 /* No obstruction! */
371 return TRUE;
372 }
373 /* No intersection for this attempt, continue */
374 return FALSE;
375 }
376
377 /**
378 * Check if a point is "visible" from any one or more of the edges in a
379 * given group.
380 * Formally: Check if there is a line from @ref P to any of the edges in
381 * @ref Edges so that the line does not cross any of the lines of the
382 * PSLG @ref PSLG
383 */
384 gboolean
385 IsVisibleFromEdges (P2trPSLG *PSLG,
386 P2trVector2 *P,
387 P2trPSLG *Edges)
388 {
389 gboolean found_visibility_path = FALSE;
390 P2trPSLG *KnownBlocks = p2tr_pslg_new ();
391 GQueue *BlocksForTest = g_queue_new ();
392
393 P2trVector2 W;
394 find_point_in_polygon (Edges, &W);
395
396 if (TryVisibilityAroundBlock(PSLG, P, Edges, KnownBlocks, BlocksForTest, NULL, &W))
397 found_visibility_path = TRUE;
398
399 while (! g_queue_is_empty (BlocksForTest) && ! found_visibility_path)
400 {
401 const P2trBoundedLine *Block = (P2trBoundedLine*)g_queue_pop_head (BlocksForTest);
402
403 if (p2tr_pslg_contains_line (KnownBlocks, Block))
404 continue;
405 else if (TryVisibilityAroundBlock(PSLG, P, Edges, KnownBlocks, BlocksForTest, Block, &Block->start)
406 || TryVisibilityAroundBlock(PSLG, P, Edges, KnownBlocks, BlocksForTest, Block, &Block->end))
407 {
408 found_visibility_path = TRUE;
409 }
410 else
411 p2tr_pslg_add_existing_line (KnownBlocks, Block);
412 }
413
414 p2tr_pslg_free (KnownBlocks);
415 g_queue_free (BlocksForTest);
416
417 return found_visibility_path;
418 }
419 #endif
420
421 gboolean
422 p2tr_visibility_is_visible_from_edges (P2trPSLG *pslg,
423 P2trVector2 *p,
424 const P2trBoundedLine *lines,
425 guint line_count)
426 {
427 P2trPSLG *edges = p2tr_pslg_new ();
428 gint i;
429 gboolean result;
430
431 for (i = 0; i < line_count; i++)
432 p2tr_pslg_add_existing_line (edges, &lines[i]);
433
434 result = IsVisibleFromEdges (pslg, p, edges);
435
436 p2tr_pslg_free (edges);
437 return result;
438 }

   
Visit the ZANavi Wiki