1 package cz.cuni.amis.pogamut.ut2004.agent.module.sensor;
2
3 import java.util.Collection;
4 import java.util.HashSet;
5 import java.util.Iterator;
6 import java.util.Map;
7 import java.util.Set;
8 import java.util.logging.Level;
9
10 import math.geom3d.Point3D;
11 import math.geom3d.line.StraightLine3D;
12 import cz.cuni.amis.pogamut.base.agent.module.SensorModule;
13 import cz.cuni.amis.pogamut.base.communication.worldview.IWorldView;
14 import cz.cuni.amis.pogamut.base.communication.worldview.event.IWorldEventListener;
15 import cz.cuni.amis.pogamut.base.utils.math.DistanceUtils;
16 import cz.cuni.amis.pogamut.base3d.worldview.object.ILocated;
17 import cz.cuni.amis.pogamut.base3d.worldview.object.Location;
18 import cz.cuni.amis.pogamut.unreal.communication.messages.UnrealId;
19 import cz.cuni.amis.pogamut.ut2004.bot.impl.UT2004Bot;
20 import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.ItemPickedUp;
21 import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.NavPoint;
22 import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.NavPointNeighbourLink;
23 import cz.cuni.amis.pogamut.ut2004.communication.translator.shared.events.MapPointListObtained;
24 import cz.cuni.amis.utils.maps.LazyMap;
25
26
27
28
29
30
31 public class NavigationGraphHelper extends SensorModule<UT2004Bot> {
32
33 public static final DistanceUtils.IGetDistance<NavPointNeighbourLink> NAV_LINK_GET_DISTANCE = new DistanceUtils.IGetDistance<NavPointNeighbourLink>() {
34
35 @Override
36 public double getDistance(NavPointNeighbourLink object, ILocated point) {
37 if (object == null) return Double.MAX_VALUE;
38 if (point == null) return Double.MAX_VALUE;
39
40
41 Location x0 = point.getLocation();
42 Location x1 = object.getFromNavPoint().getLocation();
43 Location x2 = object.getToNavPoint().getLocation();
44 double distance = x0.sub(x1).cross(x0.sub(x2)).getLength() / x2.sub(x1).getLength();
45
46 return distance;
47 }
48
49 };
50
51 public static final DistanceUtils.IGetDistance<NavLinkPair> NAV_LINK_PAIR_GET_DISTANCE = new DistanceUtils.IGetDistance<NavLinkPair>() {
52
53 @Override
54 public double getDistance(NavLinkPair object, ILocated target) {
55 return object.getDistance(target.getLocation());
56 }
57
58 };
59
60
61
62
63
64 protected Map<UnrealId, Set<NavLinkPair>> navPointLinks = new LazyMap<UnrealId, Set<NavLinkPair>>() {
65
66 @Override
67 protected Set<NavLinkPair> create(UnrealId key) {
68 return new HashSet<NavLinkPair>();
69 }
70
71 };
72
73
74
75
76 protected Set<NavLinkPair> navLinkPairs = new HashSet<NavLinkPair>();
77
78
79
80
81
82
83
84
85 public NavPoint getNearestNavPoint() {
86 return DistanceUtils.getNearest(agent.getWorldView().getAll(NavPoint.class).values(), agent.getLocation());
87 }
88
89
90
91
92 public NavPoint getNearestNavPoint(double maxDistance) {
93 NavPoint result = DistanceUtils.getNearest(agent.getWorldView().getAll(NavPoint.class).values(), agent.getLocation());
94 if (result == null) return null;
95 if (agent.getLocation().getDistance(result.getLocation()) > maxDistance) return null;
96 return result;
97 }
98
99
100
101
102
103 public NavLinkPair getNearestNavLinkPair() {
104 return DistanceUtils.getNearest(navLinkPairs, agent.getLocation(), NAV_LINK_PAIR_GET_DISTANCE);
105 }
106
107
108
109
110
111
112 public NavPoint getNearestNavPoint(ILocated target) {
113 return DistanceUtils.getNearest(agent.getWorldView().getAll(NavPoint.class).values(), target);
114 }
115
116
117
118
119
120
121
122 public NavPoint getNearestNavPoint(ILocated target, double maxDistance) {
123 if (target == null || target.getLocation() == null) return null;
124 NavPoint result = DistanceUtils.getNearest(agent.getWorldView().getAll(NavPoint.class).values(), target);
125 if (result == null) return null;
126 if (target.getLocation().getDistance(result.getLocation()) > maxDistance) return null;
127 return result;
128 }
129
130
131
132
133
134
135 public NavLinkPair getNearestNavLinkPair(ILocated target) {
136 return DistanceUtils.getNearest(navLinkPairs, target, NAV_LINK_PAIR_GET_DISTANCE);
137 }
138
139
140
141
142
143
144
145
146
147
148
149
150 public static Location projectPointToLinkLine(NavPointNeighbourLink link, ILocated point) {
151 if (link == null || point == null || point.getLocation() == null) return null;
152 StraightLine3D line = new StraightLine3D(link.getFromNavPoint().getLocation().asPoint3D(), link.getToNavPoint().getLocation().asPoint3D());
153 Point3D result = line.projectPoint(point.getLocation().asPoint3D());
154 return new Location(result);
155 }
156
157
158
159
160
161
162
163 public static Boolean isPointProjectionOnLinkSegment(NavPointNeighbourLink link, ILocated point) {
164 if (link == null || point == null || point.getLocation() == null) return null;
165 StraightLine3D line = new StraightLine3D(link.getFromNavPoint().getLocation().asPoint3D(), link.getToNavPoint().getLocation().asPoint3D());
166 double u = line.project(point.getLocation().asPoint3D());
167 return u >= 0 && u <= 1;
168 }
169
170
171
172
173
174
175
176 public static Boolean isPointProjectionBeforeLinkSegment(NavPointNeighbourLink link, ILocated point) {
177 if (link == null || point == null || point.getLocation() == null) return null;
178 StraightLine3D line = new StraightLine3D(link.getFromNavPoint().getLocation().asPoint3D(), link.getToNavPoint().getLocation().asPoint3D());
179 double u = line.project(point.getLocation().asPoint3D());
180 return u < 0;
181 }
182
183
184
185
186
187
188
189 public static Boolean isPointProjectionAfterLinkSegment(NavPointNeighbourLink link, ILocated point) {
190 if (link == null || point == null || point.getLocation() == null) return null;
191 StraightLine3D line = new StraightLine3D(link.getFromNavPoint().getLocation().asPoint3D(), link.getToNavPoint().getLocation().asPoint3D());
192 double u = line.project(point.getLocation().asPoint3D());
193 return u > 1;
194 }
195
196
197
198
199
200
201 protected void init(Collection<NavPoint> navPoints) {
202 if (log != null && log.isLoggable(Level.FINE)) log.fine("Computing nav-link pairs...");
203
204 navPointLinks.clear();
205 navLinkPairs.clear();
206
207 if (navPoints.size() == 0) return;
208
209 Set<NavPointNeighbourLink> finishedLinks = new HashSet<NavPointNeighbourLink>();
210 Set<NavPoint> finished = new HashSet<NavPoint>();
211 Set<NavPoint> pending = new HashSet<NavPoint>();
212
213 pending.add(navPoints.iterator().next());
214
215 while (pending.size() > 0) {
216 NavPoint nav1;
217
218 Iterator<NavPoint> pendingIterator = pending.iterator();
219 nav1 = pendingIterator.next();
220 pendingIterator.remove();
221
222 for (NavPointNeighbourLink outgoing : nav1.getOutgoingEdges().values()) {
223 if (finishedLinks.contains(outgoing)) continue;
224 NavPoint nav2 = outgoing.getToNavPoint();
225 NavPointNeighbourLink incoming = outgoing.getToNavPoint().getOutgoingEdges().get(nav2.getId());
226 NavLinkPair pair = new NavLinkPair(outgoing, incoming);
227
228 navPointLinks.get(nav1.getId()).add(pair);
229 navPointLinks.get(nav2.getId()).add(pair);
230 navLinkPairs.add(pair);
231
232 finishedLinks.add(outgoing);
233 if (incoming != null) {
234 finishedLinks.add(incoming);
235 }
236
237 if (!finished.contains(nav2)) {
238 pending.add(nav2);
239 }
240 }
241
242 finished.add(nav1);
243 }
244
245 if (log != null && log.isLoggable(Level.INFO)) log.info("Computed nav-link pairs.");
246 }
247
248
249
250
251
252
253 protected class MapPointListObtainedListener implements IWorldEventListener<MapPointListObtained>
254 {
255
256
257
258
259 public MapPointListObtainedListener(IWorldView worldView)
260 {
261 worldView.addEventListener(ItemPickedUp.class, this);
262 }
263
264 @Override
265 public void notify(MapPointListObtained event)
266 {
267 if (event.getNavPoints() != null) {
268 init(event.getNavPoints().values());
269 }
270 }
271
272 }
273
274 protected MapPointListObtainedListener mapPointListObtainedListener;
275
276
277
278 public NavigationGraphHelper(UT2004Bot bot) {
279 super(bot);
280
281 mapPointListObtainedListener = new MapPointListObtainedListener(bot.getWorldView());
282
283
284 if (bot.getWorldView().getAll(NavPoint.class).size() > 0) {
285 init(bot.getWorldView().getAll(NavPoint.class).values());
286 }
287 }
288
289 }