1 package Steerings;
2
3 import java.util.ArrayList;
4 import java.util.List;
5
6 import javax.vecmath.Tuple3d;
7 import javax.vecmath.Vector2d;
8 import javax.vecmath.Vector3d;
9
10 import SocialSteeringsBeta.RefLocation;
11 import SteeringProperties.SteeringProperties;
12 import SteeringProperties.StickToPathProperties;
13 import SteeringStuff.ISteering;
14 import SteeringStuff.RefBoolean;
15 import SteeringStuff.SteeringManager;
16 import SteeringStuff.SteeringTools;
17 import cz.cuni.amis.pogamut.base.agent.navigation.IPathFuture;
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.NavigationGraphHelper;
22 import cz.cuni.amis.pogamut.ut2004.bot.impl.UT2004Bot;
23 import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.ConfigChange;
24 import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.NavPoint;
25 import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.NavPointNeighbourLink;
26 import cz.cuni.amis.utils.Tuple3;
27 import cz.cuni.amis.utils.future.FutureStatus;
28
29
30
31
32
33
34 public class StickToPathSteer implements ISteering {
35
36 private UT2004Bot bot;
37
38 private static int NEARLY_THERE_DISTANCE = 150;
39
40 private static double NAVPOINT_MAX_DISTANCE = 100;
41
42 private StickToPathProperties properties;
43
44
45
46
47 private IPathFuture<ILocated> pathFuture;
48
49
50
51
52 private List<ILocated> pathLocations;
53
54
55
56
57 private List<NavPoint> pathNavPoints;
58
59
60
61
62 private List<NavPointNeighbourLink> pathLinks;
63
64 public StickToPathSteer(UT2004Bot bot) {
65 this.bot = bot;
66 }
67
68
69
70
71 @Override
72 public Vector3d run(Vector3d scaledActualVelocity, RefBoolean wantsToGoFaster, RefBoolean wantsToStop, RefLocation focus) {
73
74
75 ensurePath();
76
77 if (!hasPath()) {
78
79
80 return new Vector3d(0,0,0);
81 }
82
83 Tuple3<NavPointNeighbourLink, NavPointNeighbourLink, NavPointNeighbourLink> links = getCurrentLinks();
84 if (links == null) {
85
86
87 return new Vector3d(0,0,0);
88 }
89
90 NavPointNeighbourLink prevLink = links.getFirst();
91 NavPointNeighbourLink currLink = links.getSecond();
92 NavPointNeighbourLink nextLink = links.getThird();
93
94 if (SteeringManager.DEBUG) {
95 System.out.println("StickToPath: prevLink = " + prevLink);
96 System.out.println("StickToPath: currLink = " + currLink);
97 System.out.println("StickToPath: nextLink = " + nextLink);
98 }
99
100 if (currLink != null) {
101 return steerCurrLink(currLink, nextLink);
102 }
103
104
105 if (nextLink != null) {
106 return steerNextLink(nextLink);
107 }
108
109
110
111 return new Vector3d(0,0,0);
112 }
113
114 private Vector3d steerCurrLink(NavPointNeighbourLink currLink, NavPointNeighbourLink nextLink) {
115 Location projection = NavigationGraphHelper.projectPointToLinkLine(currLink, bot.getLocation());
116 double distance = NavigationGraphHelper.NAV_LINK_GET_DISTANCE.getDistance(currLink, bot.getLocation());
117
118 double ratio;
119
120 if (currLink.getCollisionR() <= 0) {
121 ratio = 0;
122 } else {
123 ratio = distance / (double)currLink.getCollisionR();
124 }
125
126 Location result = projection.sub(bot.getLocation()).scale(ratio).setZ(0);
127
128 if (SteeringManager.DEBUG) {
129 System.out.println("StickToPathSteer: HAS LINK");
130 System.out.println("StickToPathSteer: collision radius = " + currLink.getCollisionR());
131 System.out.println("StickToPathSteer: distance = " + distance);
132 System.out.println("StickToPathSteer: ratio = " + ratio);
133 System.out.println("StickToPathSteer: result = [" + result.x + ", " + result.y + ", " + result.z + "]");
134 System.out.println("StickToPathSteer: length = " + result.getLength());
135 }
136
137 return result.asVector3d();
138 }
139
140 private Vector3d steerNextLink(NavPointNeighbourLink nextLink) {
141 Location projection = nextLink.getFromNavPoint().getLocation();
142
143 Location result = projection.sub(bot.getLocation()).getNormalized().scale(50).setZ(0);
144
145 if (SteeringManager.DEBUG) {
146 System.out.println("StickToPathSteer: TARGETING NEXT LINK");
147 System.out.println("StickToPathSteer: result = [" + result.x + ", " + result.y + ", " + result.z + "]");
148 System.out.println("StickToPathSteer: length = " + result.getLength());
149 }
150
151 return result.asVector3d();
152 }
153
154 private void reset() {
155 this.pathFuture = null;
156 this.pathLocations = null;
157 this.pathNavPoints = null;
158 this.pathLinks = null;
159 }
160
161 private boolean hasPath() {
162 return this.pathFuture != null && this.pathLocations != null && this.pathNavPoints != null && this.pathLinks != null && this.pathLocations.size() > 0 && this.pathNavPoints.size() > 0;
163 }
164
165 private void ensurePath() {
166 if (this.pathFuture == properties.getPath()) {
167
168 return;
169 }
170
171
172 if (properties.getPath() == null) {
173 System.out.println("PATH IS NULL! Use stickToPathProperties.setPath() to provide a path before you use the steering!");
174 reset();
175 return;
176 }
177 if (!properties.getPath().isDone()) {
178 System.out.println("Waiting for the path...");
179 reset();
180 return;
181 }
182 if (properties.getPath().get() == null) {
183 System.out.println("Provided PATH is null, path does not exist? Or incorrect path future provided?");
184 reset();
185 return;
186 }
187 if (properties.getPath().get().size() == 0) {
188 System.out.println("Provided PATH is 0-sized! Path does not exist? Or incorrect path future provided?");
189 reset();
190 return;
191 }
192
193
194
195 this.pathFuture = properties.getPath();
196 this.pathLocations = getPath(this.pathFuture);
197 this.pathNavPoints = getNavPoints(this.pathLocations);
198 this.pathLinks = getLinks(this.pathNavPoints);
199 }
200
201 private List<NavPoint> getNavPoints(List<ILocated> pathLocations) {
202 if (pathLocations == null) {
203 return null;
204 }
205 List<NavPoint> result = new ArrayList<NavPoint>(pathLocations.size());
206 for (ILocated location : pathLocations) {
207 result.add(getNearestNavPoint(location, NAVPOINT_MAX_DISTANCE));
208 }
209 return result;
210 }
211
212 private List<NavPointNeighbourLink> getLinks(List<NavPoint> pathNavPoints) {
213 if (pathNavPoints == null) {
214 return null;
215 }
216 if (pathNavPoints.size() == 1) {
217 List<NavPointNeighbourLink> result = new ArrayList<NavPointNeighbourLink>(1);
218 result.add(null);
219 return result;
220 }
221
222 List<NavPointNeighbourLink> result = new ArrayList<NavPointNeighbourLink>();
223 NavPoint previous = pathNavPoints.get(0);
224 for (int i = 1; i < pathNavPoints.size(); ++i) {
225 NavPoint curr = pathNavPoints.get(i);
226 if (previous != null && curr != null) {
227 result.add(previous.getOutgoingEdges().get(curr.getId()));
228 } else {
229 result.add(null);
230 }
231 previous = curr;
232 }
233
234 return result;
235 }
236
237
238
239
240
241
242
243 public NavPoint getNearestNavPoint(ILocated target, double maxDistance) {
244 if (target == null || target.getLocation() == null) return null;
245 NavPoint result = DistanceUtils.getNearest(bot.getWorldView().getAll(NavPoint.class).values(), target);
246 if (result == null) return null;
247 if (target.getLocation().getDistance(result.getLocation()) > maxDistance) return null;
248 return result;
249 }
250
251
252
253
254
255
256 private Tuple3<NavPointNeighbourLink, NavPointNeighbourLink, NavPointNeighbourLink> getCurrentLinks() {
257 if (!hasPath()) return null;
258
259 NavPointNeighbourLink nearest = DistanceUtils.getNearest(pathLinks, bot.getLocation(), NavigationGraphHelper.NAV_LINK_GET_DISTANCE);
260
261 if (nearest == null) return null;
262
263 int index = pathLinks.indexOf(nearest);
264
265 if (NavigationGraphHelper.isPointProjectionOnLinkSegment(nearest, bot.getLocation())) {
266 return new Tuple3<NavPointNeighbourLink, NavPointNeighbourLink, NavPointNeighbourLink>(
267 getPathLink(index-1), getPathLink(index), getPathLink(index+1)
268 );
269 } else
270 if (NavigationGraphHelper.isPointProjectionBeforeLinkSegment(nearest, bot.getLocation())) {
271 return new Tuple3<NavPointNeighbourLink, NavPointNeighbourLink, NavPointNeighbourLink>(
272 getPathLink(index-2), getPathLink(index-1), getPathLink(index)
273 );
274 } else {
275
276 return new Tuple3<NavPointNeighbourLink, NavPointNeighbourLink, NavPointNeighbourLink>(
277 getPathLink(index), getPathLink(index+1), getPathLink(index+2)
278 );
279 }
280
281 }
282 private NavPointNeighbourLink getPathLink(int index) {
283 if (pathLinks == null) return null;
284 if (index < 0 || index >= pathLinks.size()) return null;
285 return pathLinks.get(index);
286 }
287
288 private List<ILocated> getPath(IPathFuture<ILocated> path) {
289 List<ILocated> locations = path.get();
290 if (locations == null) {
291 System.out.println("Provided PATH is null, have you provided correct path via stickToPathProperties.setPath() ?");
292 return null;
293 }
294 if (locations.size() == 0) {
295 System.out.println("Provided PATH has no elements, have you provided correct path via stickToPathProperties.setPath() ?");
296 return null;
297 }
298 return locations;
299 }
300
301 public void setProperties(SteeringProperties newProperties) {
302 if (!(newProperties instanceof StickToPathProperties)) throw new RuntimeException("newProperties is not of class StickToPathProperties, but " + newProperties.getClass());
303 this.properties = new StickToPathProperties((StickToPathProperties)newProperties);
304 }
305
306 public StickToPathProperties getProperties() {
307 return new StickToPathProperties(properties);
308 }
309
310 }
311