1 package cz.cuni.amis.pogamut.ut2004.agent.module.sensor.visibility.model;
2
3 import java.io.File;
4 import java.io.FileInputStream;
5 import java.io.FileOutputStream;
6 import java.io.ObjectInputStream;
7 import java.io.ObjectOutputStream;
8 import java.io.ObjectStreamException;
9 import java.io.Serializable;
10 import java.util.BitSet;
11 import java.util.Collection;
12 import java.util.HashMap;
13 import java.util.HashSet;
14 import java.util.Map;
15 import java.util.Set;
16 import java.util.Map.Entry;
17
18 import cz.cuni.amis.pogamut.base.utils.math.DistanceUtils;
19 import cz.cuni.amis.pogamut.base3d.worldview.object.ILocated;
20 import cz.cuni.amis.pogamut.base3d.worldview.object.Location;
21 import cz.cuni.amis.pogamut.ut2004.agent.module.sensor.visibility.Visibility;
22 import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.NavPoint;
23 import cz.cuni.amis.utils.IFilter;
24
25
26 public class VisibilityMatrix implements Serializable {
27
28
29
30
31 private static final long serialVersionUID = -5542380797228986422L;
32
33
34
35
36
37
38
39
40
41
42
43
44
45 private Map<Integer, VisibilityLocation> locations = new HashMap<Integer, VisibilityLocation>();
46
47 private BitMatrix matrix;
48
49
50
51 private String mapName;
52
53 protected VisibilityMatrix() {
54 }
55
56 public VisibilityMatrix(String mapName, int size) {
57 this.mapName = mapName;
58 matrix = new BitMatrix(size, size);
59 }
60
61 public String getMapName() {
62 return mapName;
63 }
64
65 public Map<Integer, VisibilityLocation> getLocations() {
66 return locations;
67 }
68
69 public VisibilityLocation getLocation(int index) {
70 return locations.get(index);
71 }
72
73
74
75
76
77
78 public BitMatrix getMatrix() {
79 return matrix;
80 }
81
82
83
84
85
86
87
88
89
90 public static String getFileName_All(String mapName) {
91 return "VisibilityMatrix-" + mapName + "-All.bin";
92 }
93
94 public static File getFile_All(File directory, String mapName) {
95 return new File(directory, getFileName_All(mapName));
96 }
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138 public void save(File directory) {
139 try {
140 File file1 = getFile_All(directory, mapName);
141
142 ObjectOutputStream output = new ObjectOutputStream(new FileOutputStream(file1));
143 try {
144 output.writeObject(this);
145 } finally {
146 output.close();
147 }
148
149 } catch (Exception e) {
150 throw new RuntimeException("Failed to save VisibilityMatrix.", e);
151 }
152 }
153
154 public static VisibilityMatrix load(File directory, String mapName) {
155 try {
156 File file1 = getFile_All(directory, mapName);
157
158 ObjectInputStream input = new ObjectInputStream(new FileInputStream(file1));
159 VisibilityMatrix result = null;
160 try {
161 result = (VisibilityMatrix)input.readObject();
162 } finally {
163 input.close();
164 }
165
166 return result;
167 } catch (Exception e) {
168 throw new RuntimeException("Failed to load VisibilityMatrix.", e);
169 }
170 }
171
172
173
174
175
176 public int getPairCount() {
177 return locations.size() * (locations.size() - 1) / 2;
178 }
179
180
181
182
183
184
185
186
187 public int getVisiblePairCount() {
188 int count = 0;
189 for (int i = 0; i < locations.size(); ++i) {
190 for (int j = i+1; j < locations.size(); ++j) {
191 if (matrix.get(i, j)) {
192 ++count;
193 }
194 }
195 }
196 return count;
197 }
198
199
200
201
202
203 private Object readResolve() throws ObjectStreamException {
204 mutex = new Object();
205 coverPoints = new HashMap<Integer, Set<VisibilityLocation>>();
206 visiblePoints = new HashMap<Integer, Set<VisibilityLocation>>();
207 coverNavPoints = new HashMap<Integer, Set<NavPoint>>();
208 visibleNavPoints = new HashMap<Integer, Set<NavPoint>>();
209
210 return this;
211 }
212
213
214
215
216
217 private transient Object mutex = new Object();
218
219 private transient Map<Integer, Set<VisibilityLocation>> coverPoints = new HashMap<Integer, Set<VisibilityLocation>>();
220
221 private transient Map<Integer, Set<VisibilityLocation>> visiblePoints = new HashMap<Integer, Set<VisibilityLocation>>();
222
223 private transient Map<Integer, Set<NavPoint>> coverNavPoints = new HashMap<Integer, Set<NavPoint>>();
224
225 private transient Map<Integer, Set<NavPoint>> visibleNavPoints = new HashMap<Integer, Set<NavPoint>>();
226
227
228
229
230
231
232 public Set<VisibilityLocation> getVisiblePoints(BitSet column) {
233 if (column == null) return null;
234 Set<VisibilityLocation> result = new HashSet<VisibilityLocation>();
235 for (int i = 0; i < column.length() && i < matrix.rows(); ++i) {
236 if (column.get(i)) {
237 result.add(locations.get(i));
238 }
239 }
240 return result;
241 }
242
243
244
245
246
247
248 public Set<NavPoint> getVisibleNavPoints(BitSet column) {
249 if (column == null) return null;
250 Set<NavPoint> result = new HashSet<NavPoint>();
251 for (int i = 0; i < column.length() && i < matrix.rows(); ++i) {
252 if (column.get(i)) {
253 VisibilityLocation vLoc = locations.get(i);
254 if (vLoc.navPoint != null) result.add(vLoc.navPoint);
255 }
256 }
257 return result;
258 }
259
260
261
262
263
264
265 public Set<NavPoint> getNavPoints(Collection<VisibilityLocation> visibilityLocations) {
266 Set<NavPoint> result = new HashSet<NavPoint>();
267 for (VisibilityLocation vLoc : visibilityLocations) {
268 if (vLoc.navPoint != null) {
269 result.add(vLoc.navPoint);
270 }
271 }
272 return result;
273 }
274
275
276
277
278
279
280 public Set<VisibilityLocation> getCoverPoints(BitSet column) {
281 Set<VisibilityLocation> result = new HashSet<VisibilityLocation>();
282 for (int i = 0; i < column.length() && i < matrix.rows(); ++i) {
283 if (!column.get(i)) {
284 result.add(locations.get(i));
285 }
286 }
287 return result;
288 }
289
290
291
292
293
294
295 public Set<NavPoint> getCoverNavPoints(BitSet column) {
296 Set<NavPoint> result = new HashSet<NavPoint>();
297 for (int i = 0; i < column.length() && i < matrix.rows(); ++i) {
298 if (!column.get(i)) {
299 VisibilityLocation vLoc = locations.get(i);
300 if (vLoc.navPoint != null) result.add(vLoc.navPoint);
301 }
302 }
303 return result;
304 }
305
306
307
308
309
310 public Entry<Integer, VisibilityLocation> getNearestEntry(ILocated located) {
311 if (located == null) return null;
312 Location location = located.getLocation();
313 double minDistance = Double.MAX_VALUE;
314 Entry<Integer, VisibilityLocation> nearest = null;
315 for (Entry<Integer, VisibilityLocation> entry : locations.entrySet()) {
316 double distance = location.getDistance(entry.getValue().getLocation());
317 if (distance < minDistance) {
318 minDistance = distance;
319 nearest = entry;
320 }
321 }
322 return nearest;
323 }
324
325
326
327
328
329
330 public VisibilityLocation getNearest(ILocated located) {
331 if (located == null) return null;
332 Entry<Integer, VisibilityLocation> nearest = getNearestEntry(located);
333 if (nearest == null) return null;
334 return nearest.getValue();
335 }
336
337
338
339
340
341
342 public NavPoint getNearestNavPoint(ILocated located) {
343 if (located == null) return null;
344 VisibilityLocation nearest =
345 DistanceUtils.getNearestFiltered(
346 locations.values(),
347 located,
348 new IFilter<VisibilityLocation>() {
349 @Override
350 public boolean isAccepted(VisibilityLocation object) {
351 return object.navPoint != null;
352 }
353 }
354 );
355 if (nearest == null) return null;
356 return nearest.navPoint;
357 }
358
359
360
361
362
363
364 public Integer getNearestIndex(ILocated located) {
365 if (located == null) return null;
366 Entry<Integer, VisibilityLocation> nearest = getNearestEntry(located);
367 if (nearest == null) return null;
368 return nearest.getKey();
369 }
370
371
372
373
374
375
376
377
378
379
380
381 public boolean isVisible(ILocated loc1, ILocated loc2) {
382 if (loc1 == null) return false;
383 if (loc2 == null) return false;
384 Integer index1 = getNearestIndex(loc1);
385 if (index1 == null) return false;
386 Integer index2 = getNearestIndex(loc2);
387 if (index2 == null) return false;
388 return matrix.get(index1, index2);
389 }
390
391
392
393
394
395
396 public Set<VisibilityLocation> getCoverPoints(ILocated loc) {
397 if (loc == null) return new HashSet<VisibilityLocation>();
398
399 Entry<Integer, VisibilityLocation> vLocEntry = getNearestEntry(loc);
400 if (vLocEntry == null) return null;
401
402 int key = vLocEntry.getKey();
403 VisibilityLocation vLoc = vLocEntry.getValue();
404
405 Set<VisibilityLocation> result = coverPoints.get(key);
406 if (result != null) return result;
407
408 synchronized(mutex) {
409 result = coverPoints.get(key);
410 if (result != null) return result;
411
412 BitSet visibility = matrix.getColumn(key);
413 result = getCoverPoints(visibility);
414 coverPoints.put(key, result);
415
416 return result;
417 }
418 }
419
420
421
422
423
424
425 public VisibilityLocation getNearestCoverPoint(ILocated loc) {
426 return DistanceUtils.getNearest(getCoverPoints(loc), loc);
427 }
428
429
430
431
432
433
434 public VisibilityLocation getNearestCoverPoint(ILocated target, ILocated from) {
435 return DistanceUtils.getNearest(getCoverPoints(from), target);
436 }
437
438
439
440
441
442
443
444 public Set<VisibilityLocation> getVisiblePoints(ILocated loc) {
445 if (loc == null) return new HashSet<VisibilityLocation>();
446
447 Entry<Integer, VisibilityLocation> vLocEntry = getNearestEntry(loc);
448 if (vLocEntry == null) return null;
449
450 int key = vLocEntry.getKey();
451 VisibilityLocation vLoc = vLocEntry.getValue();
452
453 Set<VisibilityLocation> result = visiblePoints.get(key);
454 if (result != null) return result;
455
456 synchronized(mutex) {
457 result = coverPoints.get(key);
458 if (result != null) return result;
459
460 BitSet visibility = matrix.getColumn(key);
461 result = getVisiblePoints(visibility);
462 visiblePoints.put(key, result);
463
464 return result;
465 }
466 }
467
468
469
470
471
472
473 public Set<NavPoint> getCoverNavPoints(ILocated loc) {
474 if (loc == null) return new HashSet<NavPoint>();
475
476 Entry<Integer, VisibilityLocation> vLocEntry = getNearestEntry(loc);
477 if (vLocEntry == null) return null;
478
479 int key = vLocEntry.getKey();
480 VisibilityLocation vLoc = vLocEntry.getValue();
481
482 Set<NavPoint> result = coverNavPoints.get(key);
483 if (result != null) return result;
484
485 synchronized(mutex) {
486 result = coverNavPoints.get(key);
487 if (result != null) return result;
488
489 BitSet visibility = matrix.getColumn(key);
490 result = getCoverNavPoints(visibility);
491 coverNavPoints.put(key, result);
492
493 return result;
494 }
495 }
496
497
498
499
500
501
502 public NavPoint getNearestCoverNavPoint(ILocated loc) {
503 return DistanceUtils.getNearest(getCoverNavPoints(loc), loc);
504 }
505
506
507
508
509
510
511 public NavPoint getNearestCoverNavPoint(ILocated target, ILocated loc) {
512 return DistanceUtils.getNearest(getCoverNavPoints(loc), loc);
513 }
514
515
516
517
518
519
520
521 public Set<NavPoint> getVisibleNavPoints(ILocated loc) {
522 if (loc == null) return new HashSet<NavPoint>();
523
524 Entry<Integer, VisibilityLocation> vLocEntry = getNearestEntry(loc);
525 if (vLocEntry == null) return null;
526
527 int key = vLocEntry.getKey();
528 VisibilityLocation vLoc = vLocEntry.getValue();
529
530 Set<NavPoint> result = visibleNavPoints.get(key);
531 if (result != null) return result;
532
533 synchronized(mutex) {
534 result = coverNavPoints.get(key);
535 if (result != null) return result;
536
537 BitSet visibility = matrix.getColumn(key);
538 result = getVisibleNavPoints(visibility);
539 visibleNavPoints.put(key, result);
540
541 return result;
542 }
543 }
544
545
546
547
548
549
550 public Set<Integer> getNearestIndices(ILocated... locs) {
551 if (locs == null) return null;
552 Set<Integer> keys = new HashSet<Integer>();
553 for (ILocated loc : locs) {
554 if (loc == null) continue;
555 Integer key = getNearestIndex(loc);
556 keys.add(key);
557 }
558 return keys;
559 }
560
561
562
563
564
565
566 public Set<VisibilityLocation> getCoverPointsN(ILocated... locs) {
567 if (locs == null) return null;
568 Set<Integer> keys = getNearestIndices(locs);
569
570 BitSet covers = matrix.or(keys);
571
572 return getCoverPoints(covers);
573 }
574
575
576
577
578
579
580
581 public VisibilityLocation getNearestCoverPointN(ILocated target, ILocated... coveredFrom) {
582 if (target == null) return null;
583 if (coveredFrom == null) return getNearest(target);
584 return DistanceUtils.getNearest(getCoverPointsN(coveredFrom), target);
585 }
586
587
588
589
590
591
592 public Set<NavPoint> getCoverNavPointsN(ILocated... locs) {
593 if (locs == null) return null;
594 return getNavPoints(getCoverPointsN(locs));
595 }
596
597
598
599
600
601
602
603 public NavPoint getNearestCoverNavPointN(ILocated target, ILocated... coveredFrom) {
604 if (target == null) return null;
605 if (coveredFrom == null) return getNearestNavPoint(target);
606 return DistanceUtils.getNearest(getCoverNavPointsN(coveredFrom), target);
607 }
608
609 }