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