View Javadoc

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   * Steering that makes sure the bot won't get out of the "naviagation edge corridor".
31   *
32   * @author Jimmy
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     	 * CURRENT path future.
46     	 */
47  	private IPathFuture<ILocated> pathFuture;
48  
49  	/**
50  	 * CURRENT path locations list.
51  	 */
52  	private List<ILocated> pathLocations;
53  	
54  	/**
55  	 * CURRENT navigation points, if applies (may contain NULLs!).
56  	 */
57  	private List<NavPoint> pathNavPoints;
58  	
59  	/**
60  	 * CURRENT navigation links, index 0 is for links between pathNavPoints.get(0) and pathNavPoints.get(1)
61  	 */
62  	private List<NavPointNeighbourLink> pathLinks;
63  
64      public StickToPathSteer(UT2004Bot bot) {
65          this.bot = bot;
66      }
67  
68      /**
69       * Moves the bot around the given pathFuture.
70       */
71      @Override
72      public Vector3d run(Vector3d scaledActualVelocity, RefBoolean wantsToGoFaster, RefBoolean wantsToStop, RefLocation focus) {
73      	
74      	// ENSURE PATH EXISTENCE
75      	ensurePath();
76      	
77      	if (!hasPath()) {
78      		// NO PATH
79      		// => we cannot steer
80      		return new Vector3d(0,0,0);
81      	}
82      	
83      	Tuple3<NavPointNeighbourLink, NavPointNeighbourLink, NavPointNeighbourLink> links = getCurrentLinks();
84      	if (links == null) {
85      		// INVALID LINKS
86      		// => we cannot steer
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     	// currLink == null
105     	if (nextLink != null) {
106     		return steerNextLink(nextLink);
107     	}
108     	
109     	// NO LINK INFORMATION
110     	// => we cannot steer
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     		// ALREADY INITIALIZED TO CORRECT PATH
168     		return;
169     	}
170     	
171     	// CHECK PATH EXISTENCE
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     	// PATH EXIST
194     	// => initialize
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 	 * Returns nearest {@link NavPoint} to some 'target' no further than 'maxDistance' from the bot.
239 	 * @param target
240 	 * @param maxDistance
241 	 * @return
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      * Returns PREVIOUS, CURRENT, NEXT neighbour links we're following.
254      * @return
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     		// after
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