1 package cz.cuni.amis.pogamut.ut2004.storyworld.place;
2
3 import java.io.File;
4 import java.io.FileNotFoundException;
5 import java.util.Collections;
6 import java.util.HashMap;
7 import java.util.HashSet;
8 import java.util.List;
9 import java.util.Map;
10 import java.util.Set;
11 import java.util.logging.Logger;
12
13 import javax.vecmath.Point3d;
14
15 import cz.cuni.amis.pogamut.base.communication.worldview.IWorldView;
16 import cz.cuni.amis.pogamut.base.communication.worldview.event.IWorldEventListener;
17 import cz.cuni.amis.pogamut.ut2004.agent.navigation.floydwarshall.FloydWarshallMap;
18 import cz.cuni.amis.pogamut.unreal.communication.messages.UnrealId;
19 import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.NavPoint;
20 import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.NavPointMessage;
21 import cz.cuni.amis.pogamut.ut2004.communication.translator.shared.events.MapPointListObtained;
22 import cz.cuni.amis.pogamut.ut2004.storyworld.perception.SPLocation;
23 import cz.cuni.amis.utils.Job;
24 import cz.cuni.amis.utils.maps.HashMapSet;
25 import cz.cuni.amis.utils.token.Token;
26 import cz.cuni.amis.utils.token.Tokens;
27
28
29
30
31
32
33 public class SPStoryWorld {
34
35 private static final double NEAR = 1000;
36
37 private static final Set EMPTY_SET = Collections.unmodifiableSet(new HashSet());
38
39 private Map<Token, SPStoryPlace> places;
40
41 private Map<Token, SPStoryPlaceBase> bases;
42
43
44
45
46 private Map<Token, SPStoryPlace> allPlaces = null;
47
48
49
50
51 private Set<NavPoint> navPoints = null;
52
53 private Map<String, NavPoint> navPointsMap = null;
54
55
56
57
58
59
60
61
62 private HashMapSet<UnrealId, SPStoryPlaceBase> navPointIdToBasePlace = null;
63
64 private IWorldEventListener<MapPointListObtained> mapPointsListener =
65 new IWorldEventListener<MapPointListObtained>() {
66
67 @Override
68 public void notify(MapPointListObtained event) {
69 mapPointsList(event);
70 new Job<Boolean>() {
71
72 @Override
73 protected void job() throws Exception {
74
75 setResult(true);
76 }
77
78 }.startJob();
79 }
80
81 };
82
83 private IWorldView worldView;
84
85 private FloydWarshallMap navigation;
86
87 private Logger log;
88
89 private SPStoryWorldData data;
90
91 public SPStoryWorld(String worldXMLDefinitionFile, IWorldView ww, Logger log) throws FileNotFoundException {
92 this(SPStoryWorldData.loadXML(new File(worldXMLDefinitionFile)), ww, log);
93 }
94
95 public SPStoryWorld(File worldXMLDefinition, IWorldView ww, Logger log) throws FileNotFoundException {
96 this(SPStoryWorldData.loadXML(worldXMLDefinition), ww, log);
97 }
98
99 public SPStoryWorld(SPStoryWorldData data, IWorldView ww, Logger log) {
100 this.log = log;
101
102 this.data = data;
103
104 this.places = data.getPlaces();
105
106 this.bases = data.getBases();
107
108 this.worldView = ww;
109 this.worldView.addEventListener(MapPointListObtained.class, mapPointsListener);
110
111
112 }
113
114 private void mapPointsList(MapPointListObtained map) {
115 for (SPStoryPlaceBase placeBase : bases.values()) {
116 placeBase.bountNavPoints(map);
117 }
118 getNavPoints();
119 getNavPointsToPlaceMap();
120
121
122 }
123
124 public SPStoryPlace getPlace(Token name) {
125 return getAllPlaces().get(name);
126 }
127
128 public SPStoryPlace getPlace(String name) {
129 return getAllPlaces().get(Tokens.get(name));
130 }
131
132 public SPStoryPlaceBase getBase(Token name) {
133 return bases.get(name);
134 }
135
136 public NavPoint getNavPoint(String id) {
137 return navPointsMap.get(id);
138 }
139
140 public SPStoryPlaceBase getBase(String name) {
141 return bases.get(Tokens.get(name));
142 }
143
144 public Set<SPStoryPlaceBase> getBase(UnrealId navPointId) {
145 Set<SPStoryPlaceBase> bases = getNavPointsToPlaceMap().get(navPointId);
146 if (bases == null || bases.size() == 0) {
147 throw new RuntimeException("story base place hasn't been found for nav point " + navPointId);
148 }
149 return bases;
150 }
151
152
153
154
155
156
157 public Set<SPStoryPlaceBase> at(SPLocation location) {
158 NavPoint nearest = getNearestNavPoint(location);
159 if (location.asPoint3d().distance(nearest.getLocation().getPoint3d()) > NEAR) {
160 return EMPTY_SET;
161 }
162 return getBase(nearest.getId());
163 }
164
165 public NavPoint getNearestNavPoint(SPLocation location) {
166
167 Point3d loc = location.asPoint3d();
168 double nearestDistance = Double.MAX_VALUE;
169 NavPoint nearest = null;
170 for (NavPoint navPoint : getNavPoints()) {
171 try{
172 double distance = loc.distance(navPoint.getLocation().getPoint3d());
173 if (distance < nearestDistance) {
174 nearestDistance = distance;
175 nearest = navPoint;
176 }
177 }catch(NullPointerException npe){
178
179 }
180
181 }
182 return nearest;
183 }
184
185 public NavPoint getFurthestNavPoint(SPLocation location) {
186
187 Point3d loc = location.asPoint3d();
188 double furthestDistance = Double.MAX_VALUE;
189 NavPoint furthest = null;
190 for (NavPoint navPoint : getNavPoints()) {
191 double distance = loc.distance(navPoint.getLocation().getPoint3d());
192 if (distance > furthestDistance) {
193 furthestDistance = distance;
194 furthest = navPoint;
195 }
196 }
197 return furthest;
198 }
199
200
201
202
203
204
205
206
207 public boolean isInside(SPLocation location, SPStoryPlace place) {
208 Set<SPStoryPlaceBase> bases = at(location);
209 if (bases == null || bases.size() == 0) return false;
210 for (SPStoryPlaceBase base : bases) {
211 if (base.contains(place)) return true;
212 }
213 return false;
214 }
215
216
217
218
219
220
221
222
223
224 public Set<NavPoint> getNavPoints() {
225 if (navPoints == null) {
226 navPoints = new HashSet<NavPoint>();
227 navPointsMap = new HashMap<String, NavPoint>();
228 for (SPStoryPlaceBase base : bases.values()) {
229 for (NavPoint np : base.getNavPoints()) {
230 navPointsMap.put(np.getId().getStringId(), np);
231 }
232 navPoints.addAll(base.getNavPoints());
233 }
234 }
235 return navPoints;
236 }
237
238 protected HashMapSet<UnrealId, SPStoryPlaceBase> getNavPointsToPlaceMap() {
239 if (navPointIdToBasePlace == null) {
240 navPointIdToBasePlace = new HashMapSet<UnrealId, SPStoryPlaceBase>();
241 for (SPStoryPlaceBase base : bases.values()) {
242 for (NavPoint navPoint : base.getNavPoints()) {
243 navPointIdToBasePlace.add(navPoint.getId(), base);
244 }
245 }
246 }
247 return navPointIdToBasePlace;
248 }
249
250 protected Map<Token, SPStoryPlace> getAllPlaces() {
251 if (allPlaces == null) {
252 allPlaces = new HashMap<Token, SPStoryPlace>();
253 for (SPStoryPlace place : places.values()) {
254 allPlaces.put(place.getName(), place);
255 }
256 for (SPStoryPlaceBase base : bases.values()) {
257 allPlaces.put(base.getName(), base);
258 }
259 }
260 return allPlaces;
261 }
262
263 public FloydWarshallMap getNavigation() {
264 return navigation;
265 }
266
267
268
269
270
271
272
273
274 public List<NavPoint> getPath(SPLocation from, SPLocation to) {
275 return getPath(from, getNearestNavPoint(to));
276 }
277
278
279
280
281
282
283
284
285 public List<NavPoint> getPath(SPLocation from, NavPoint to) {
286 return getNavigation().getPath(getNearestNavPoint(from), to);
287 }
288
289
290
291
292
293
294
295
296
297 public List<NavPoint> getPath(SPLocation from, SPStoryPlace place) {
298 NavPoint start = getNearestNavPoint(from);
299 float shortestPath = Float.POSITIVE_INFINITY;
300 List<NavPoint> path = null;
301 for (NavPoint np : place.getNavPoints()) {
302 float distance = getNavigation().getDistance(start, np);
303 if (shortestPath > distance) {
304 shortestPath = distance;
305 path = getNavigation().getPath(start, np);
306 }
307
308 }
309 return path;
310 }
311
312 public SPStoryWorldData getStoryWorldData() {
313 return data;
314 }
315
316 @Override
317 public String toString() {
318 return "SPStoryWorld";
319 }
320
321 }