View Javadoc

1   package Steerings;
2   
3   import SocialSteeringsBeta.RefLocation;
4   import SteeringStuff.SteeringManager;
5   import SteeringStuff.SteeringTools;
6   import SteeringStuff.RefBoolean;
7   import SteeringProperties.SteeringProperties;
8   import SteeringProperties.WallFollowingProperties;
9   import SteeringStuff.IRaysFlagChanged;
10  import cz.cuni.amis.pogamut.base3d.worldview.object.Location;
11  import javax.vecmath.Vector3d;
12  import cz.cuni.amis.pogamut.ut2004.bot.impl.UT2004Bot;
13  import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.AutoTraceRay;
14  import SteeringStuff.ISteering;
15  import SteeringStuff.RaycastingManager;
16  import SteeringStuff.SteeringRay;
17  import SteeringStuff.SteeringType;
18  import cz.cuni.amis.pogamut.ut2004.utils.UnrealUtils;
19  import java.util.HashMap;
20  import java.util.LinkedList;
21  import java.util.Random;
22  import java.util.concurrent.ExecutionException;
23  import java.util.concurrent.Future;
24  import java.util.logging.Level;
25  import java.util.logging.Logger;
26  import javax.vecmath.Tuple3d;
27  
28  
29  /**
30   * A class for providing wall following steering to bots via raycasting.
31   *
32   * @author Marki
33   */
34  public class WallFollowingSteer implements ISteering, IRaysFlagChanged {
35      /** This steering needs botself, raycasting manager. */
36      private UT2004Bot botself;
37      private RaycastingManager rayManager;
38  
39      private int wallForce;
40      /** Řád síly - 1-10. */
41      private int orderOfTheForce;
42      /**Steering properties: */
43      private double attractiveForceWeight;
44      /**Steering properties: */
45      private double repulsiveForceWeight;
46      /**Steering properties: */
47      private double convexEdgesForceWeight;
48      /**Steering properties: */
49      private double concaveEdgesForceWeight;
50      /**Steering properties: */
51      private boolean justMySide;
52      /**Steering properties: */
53      private boolean specialDetection;
54      /**Steering properties: */
55      private boolean frontCollisions;
56      
57  
58      /** ISteering properties:  distance from the wall - how far from the wall should the bot go.*/
59      int DISTANCE_FROM_THE_WALL = 166;
60      //How big is the value of the bot's counter when leaving a wall. (For so many ticks he is turning.)
61      private static final int DEFAULTCOUNTER = 10;
62      //Tento parametr je asi na pytel, páč to s ním stejně moc dobře nefunguje. Takže je jako konstanta a asi ho smažu.
63      private boolean goFast = false;
64  
65      //Possible states of the bot - whether he's following nothing, the left hand, or the right hand.
66      enum State { NOTHING, LEFT, RIGHT };
67      
68      
69      //The bot's state.
70      State state;
71  
72      //Counter of bot's memory, when leaving a wall (90° turns etc.).
73      int counter;
74  
75      //Help variable to remember, when we were turning beacause of the convex edge --> we will diminish the force concave edges.
76      boolean convexTurning;
77      
78      private boolean raysReady;
79      private HashMap<String, Future<AutoTraceRay>> myFutureRays;
80  
81      // Constants for rays' ids
82      protected static final String NLEFTFRONT = "wleftfront";
83      protected static final String NLEFT = "wleft";
84      protected static final String NRIGHTFRONT = "wrightfront";
85      protected static final String NRIGHT = "wright";
86      protected static final String NFRONT = "wfront";
87  
88      Random random = new Random();
89      
90      /**
91       * Bot's rays.
92       */
93      AutoTraceRay nleft, nright, nleftfront, nrightfront, nfront;
94  
95      /**
96       * Lengths of bot's rays. Short rays are supposed for side rays, long rays for nfront rays.
97       */
98      int shortSideRayLength;
99      int shortSideFrontRayLength;
100     int longSideRayLength;
101     int longSideFrontRayLength;
102     int shortFrontRayLength;
103     int longFrontRayLength;
104 
105 
106     /**
107      *
108      * @param bot Instance of the steered bot.
109      * @param rayManager Raycasting manager of the steered bot.
110      * @param newProperties Sets the steering properties.
111      */
112     public WallFollowingSteer(UT2004Bot bot, RaycastingManager rayManager, SteeringProperties newProperties) {
113         botself = bot;
114         this.rayManager = rayManager;
115         setProperties(newProperties);   //This is not necessary.
116         state = State.NOTHING;
117         counter = 0;
118         convexTurning = false;
119         prepareRays();
120     }
121 
122     /**
123      *
124      * @param bot Instance of the steered bot.
125      * @param rayManager Raycasting manager of the steered bot.
126      */
127     public WallFollowingSteer(UT2004Bot bot, RaycastingManager rayManager) {
128         botself = bot;
129         this.rayManager = rayManager;
130         setProperties(new WallFollowingProperties());   //This is not necessary.
131         state = State.NOTHING;
132         counter = 0;
133         convexTurning = false;
134         prepareRays();
135     }
136 
137     //<editor-fold defaultstate="collapsed" desc="Rays">
138 
139     //Prepares rays for the bot, removes any old rays and sets new ones.
140     private void prepareRays() {
141 
142         /*Délky postranních paprsků jsou 8 a 12. Délky šikmých se vynásobí 2 a předního se vynásobí odmocninou(3).*/
143         int shortLength = 8;
144         int longLength = 12;
145 
146         shortSideRayLength = (int) (UnrealUtils.CHARACTER_COLLISION_RADIUS * shortLength * DISTANCE_FROM_THE_WALL / 166f);        //8
147         longSideRayLength = (int) (UnrealUtils.CHARACTER_COLLISION_RADIUS * longLength * DISTANCE_FROM_THE_WALL / 166f);        //12
148         shortSideFrontRayLength = (int) (UnrealUtils.CHARACTER_COLLISION_RADIUS * shortLength * 2 * DISTANCE_FROM_THE_WALL / 166f);  //20
149         longSideFrontRayLength = (int) (UnrealUtils.CHARACTER_COLLISION_RADIUS * longLength * 2 * DISTANCE_FROM_THE_WALL / 166f);   //30
150         shortFrontRayLength = (int) (UnrealUtils.CHARACTER_COLLISION_RADIUS * shortLength * Math.sqrt(3) * DISTANCE_FROM_THE_WALL / 166f);      //18
151         longFrontRayLength = (int) (UnrealUtils.CHARACTER_COLLISION_RADIUS * longLength * Math.sqrt(3) * DISTANCE_FROM_THE_WALL / 166f);       //27
152         
153         //Five rays are created.
154         LinkedList<SteeringRay> rayList = new LinkedList<SteeringRay>();
155         rayList.add(new SteeringRay(NLEFT, new Vector3d(0, -1, 0), longSideRayLength));
156         rayList.add(new SteeringRay(NLEFTFRONT, new Vector3d(Math.sqrt(3), -1, 0), longSideFrontRayLength));
157         rayList.add(new SteeringRay(NRIGHTFRONT, new Vector3d(Math.sqrt(3), 1, 0), longSideFrontRayLength));
158         rayList.add(new SteeringRay(NRIGHT, new Vector3d(0, 1, 0), longSideRayLength));
159         rayList.add(new SteeringRay(NFRONT, new Vector3d(1, 0, 0), longFrontRayLength));
160         rayManager.addRays(SteeringType.WALL_FOLLOWING, rayList, this);
161         raysReady = false;
162         //System.out.println("Rays wall preparation end.");
163     }
164 
165 
166     public void flagRaysChanged() {
167         myFutureRays = rayManager.getMyFutureRays(SteeringType.WALL_FOLLOWING);
168         raysReady = false;
169         listenToRays();
170     }
171 
172     private void listenToRays() {
173         for(String rayId : myFutureRays.keySet()) {
174             Future<AutoTraceRay> fr = myFutureRays.get(rayId);
175             if (fr.isDone()) {
176                 //System.out.println("Ray done."+rayId);
177                 try {
178                     if (rayId.equals(NLEFTFRONT)) {
179                         nleftfront = fr.get();
180                     } else if (rayId.equals(NRIGHTFRONT)) {
181                         nrightfront = fr.get();
182                     } else if (rayId.equals(NLEFT)) {
183                         nleft = fr.get();
184                     } else if (rayId.equals(NRIGHT)) {
185                         nright = fr.get();
186                     } else if (rayId.equals(NFRONT)) {
187                         nfront = fr.get();
188                     }
189                     raysReady = true;
190                 } catch (InterruptedException ex) {
191                     Logger.getLogger(ObstacleAvoidanceSteer.class.getName()).log(Level.SEVERE, null, ex);
192                 } catch (ExecutionException ex) {
193                     Logger.getLogger(ObstacleAvoidanceSteer.class.getName()).log(Level.SEVERE, null, ex);
194                 }
195             } else {
196                 //System.out.println("Ray isn't done."+rayId);
197             }
198         }
199     }
200     //</editor-fold>
201     
202     /**
203      * When called, the bot starts steering, when possible, he walks straight, he steers away from obstacles though, when necessary.
204      * If the isn't a wall, he goes straight forward.
205      */
206     @Override
207     public Vector3d run(Vector3d scaledActualVelocity, RefBoolean wantsToGoFaster, RefBoolean wantsToStop, RefLocation focus) {
208 
209         //<editor-fold defaultstate="collapsed" desc="Set and init variables">
210 
211         wantsToGoFaster.setValue(false);
212         //Supposed velocity in the next tick of logic, after applying various steering forces to the bot. If no sensor is active, it will stay as the current velocity.
213         Vector3d nextVelocity = new Vector3d(0,0,0);
214 
215         if (!raysReady) {
216             listenToRays();
217             return nextVelocity;
218         }
219         
220         //A vector from the point where one of bot's rays crosses the wall to the bot.
221         Vector3d botToHitLocation;
222 
223         //Normal to the point, where bot's ray intersect with wall.
224         Vector3d normal;
225         
226         //Normal vector is multiplied by this when applied to the bot.
227         double multiplier;
228 
229         //The bot's velocity is received.
230         Vector3d actualVelocity = botself.getVelocity().getVector3d();
231 
232         //Whether any of the former short rays is triggered - i.e. whether the bot is close to any wall. If not, he may walk at normal speed.
233         boolean shortrays = false;
234 
235         //Whether sensors signalize the collision. (Computed in the run())
236         boolean sensornLeft = false;
237         boolean sensornRight = false;
238         boolean sensornLeftFront = false;
239         boolean sensornRightFront = false;
240         boolean sensornFront = false;
241         
242         //The bot checks his sensors
243         if (nleftfront != null)
244             sensornLeftFront = nleftfront.isResult();
245         if (nrightfront != null)
246             sensornRightFront = nrightfront.isResult();
247         if (nleft != null)
248             sensornLeft = nleft.isResult();
249         if (nright != null)
250             sensornRight = nright.isResult();
251         if (nfront != null)
252             sensornFront = nfront.isResult();
253 
254         //</editor-fold>
255 
256         //<editor-fold defaultstate="collapsed" desc="Set state">
257 
258         //The bot may change his state.
259         if (state == State.NOTHING) {
260             //If the bot is in NOTHING state and he touches the wall with both of his negative side rays
261             //(pleft and pleftront or pright and prightfront), he switches to the appropriate state.
262             if (sensornLeft && sensornLeftFront) {
263                 state = State.LEFT;
264                 counter = DEFAULTCOUNTER;
265             } else if (sensornRight && sensornRightFront) {
266                 state = State.RIGHT;
267                 counter = DEFAULTCOUNTER;
268             } else if (sensornFront) {
269                 if (frontCollisions) {
270                     botToHitLocation = new Vector3d(nfront.getHitLocation().x - botself.getLocation().x, nfront.getHitLocation().y - botself.getLocation().y, 0);
271                     normal = nfront.getHitNormal();
272                     if (Math.PI - normal.angle(actualVelocity) < Math.PI/5) {   //Nějak téměř kolmo na zeď.
273                         boolean turnLeft;
274                         if (normal.angle(actualVelocity) == 180) {
275                             turnLeft = random.nextBoolean();
276                             if (SteeringManager.DEBUG) {
277                                 System.out.println("Wall exactly front collision.");
278                             }
279                         } else {
280                             turnLeft = SteeringTools.pointIsLeftFromTheVector(actualVelocity, normal);
281                             if (SteeringManager.DEBUG) {
282                                 System.out.println("Wall nearly front collision.");
283                             }
284                         }
285                         if (SteeringManager.DEBUG) {
286                             System.out.println("We turn left " + turnLeft);
287                         }
288                         Vector3d turn = SteeringTools.getTurningVector(actualVelocity, turnLeft);
289                         turn.normalize();
290                         turn.scale(0.5);
291                         normal.add(turn);
292                         normal.normalize();
293                         multiplier = Math.pow(((longFrontRayLength - botToHitLocation.length()) * 2f / longFrontRayLength), orderOfTheForce);
294                         normal.scale(multiplier * wallForce);
295                         nextVelocity.add((Tuple3d) normal);
296                         return nextVelocity;
297                     }
298                 }
299             }
300         } else if (state == State.LEFT) {
301             if (sensornLeft && sensornLeftFront) {//The bot walks alongside the wall, he replenishes his counter.
302                 counter = DEFAULTCOUNTER;
303             }
304         } else if (state == State.RIGHT) {
305             if (sensornRight && sensornRightFront) {//The bot walks alongside the wall, he replenishes his counter.
306                 counter = DEFAULTCOUNTER;
307             }
308         }
309         //</editor-fold>
310 
311         //<editor-fold defaultstate="collapsed" desc="Just my side parameter">
312 
313         if (justMySide) {
314             if (state.equals(State.LEFT)) {
315                 sensornRight = false;
316                 sensornRightFront = false;
317             } else if (state.equals(State.RIGHT)) {
318                 sensornLeft = false;
319                 sensornLeftFront = false;
320             }
321         }
322         //</editor-fold>
323 
324         //<editor-fold defaultstate="collapsed" desc="Setting the wantsToGoFaster value.">
325 
326         if (goFast) {
327             if (sensornFront) { //If wall is in front of the bot, he shouldn't go faster.
328                 wantsToGoFaster.setValue(false);
329             } else {
330                 wantsToGoFaster.setValue(true);
331             }
332         } else {
333             if (!shortrays) {   //If nothing is signalling repulsively, the bot can return to its normal speed.
334                 wantsToGoFaster.setValue(true);
335             } else {
336                 wantsToGoFaster.setValue(false);
337             }
338         }
339         //</editor-fold>
340 
341         //<editor-fold defaultstate="collapsed" desc="Main ray forces of the wall">
342 
343         /*
344          *All sensors are checked respectively. When any of them signalizes a hit, a vector is added to nextvelocity.
345          *The vector added is derived from normal vector of the wall, such that when the ray is half in the wall (see multiplier),
346          *the steering power is as powerful as walking power at maximal velocity.
347          */
348         if (sensornLeft) {
349             //The force, which hauls the bot towards to the wall.
350             botToHitLocation = new Vector3d(nleft.getHitLocation().x - botself.getLocation().x, nleft.getHitLocation().y - botself.getLocation().y, 0);
351             normal = nleft.getHitNormal();
352             multiplier = Math.pow(1 - (longSideRayLength - botToHitLocation.length()) / longSideRayLength, orderOfTheForce);
353             normal.scale(attractiveForceWeight * multiplier * wallForce);
354             nextVelocity.sub((Tuple3d) normal);
355             //if (SteeringManager.DEBUG) System.out.println("left to "+normal.length());
356 
357             //When closer, the bot is pushed away from the wall by another force.
358             if (botToHitLocation.length() < shortSideRayLength) {
359                 shortrays = true;
360                 multiplier = Math.pow(((shortSideRayLength - botToHitLocation.length()) * 2f / shortSideRayLength), orderOfTheForce);
361                 normal.normalize();
362                 normal.scale(repulsiveForceWeight * multiplier * wallForce);
363                 nextVelocity.add((Tuple3d) normal);
364                 //if (SteeringManager.DEBUG) System.out.println("left from "+normal.length());
365             }
366         }
367 
368         if (sensornRight) {
369             //The force, which hauls the bot towards to the wall.
370             botToHitLocation = new Vector3d(nright.getHitLocation().x - botself.getLocation().x, nright.getHitLocation().y - botself.getLocation().y, 0);
371             normal = nright.getHitNormal();
372             multiplier = Math.pow(1 - (longSideRayLength - botToHitLocation.length()) / longSideRayLength, orderOfTheForce);
373             normal.scale(attractiveForceWeight * multiplier * wallForce);
374             nextVelocity.sub((Tuple3d) normal);
375             //if (SteeringManager.DEBUG) System.out.println("right to "+normal.length());
376 
377             //When closer, the bot is pushed away from the wall by another force.
378             if (botToHitLocation.length() < shortSideRayLength) {
379                 shortrays = true;
380                 multiplier = Math.pow(((shortSideRayLength - botToHitLocation.length()) * 2f / shortSideRayLength), orderOfTheForce);
381                 normal.normalize();
382                 normal.scale(repulsiveForceWeight * multiplier * wallForce);
383                 nextVelocity.add((Tuple3d) normal);
384                 //if (SteeringManager.DEBUG) System.out.println("right from "+normal.length());
385             }
386         }
387         if (sensornLeftFront) {
388             //The force, which hauls the bot towards to the wall.
389             botToHitLocation = new Vector3d(nleftfront.getHitLocation().x - botself.getLocation().x, nleftfront.getHitLocation().y - botself.getLocation().y, 0);
390             normal = nleftfront.getHitNormal();
391             multiplier = Math.pow(1 - (longSideFrontRayLength - botToHitLocation.length()) / longSideFrontRayLength, orderOfTheForce);
392             normal.scale(attractiveForceWeight * multiplier * wallForce);
393             nextVelocity.sub((Tuple3d) normal);
394             //if (SteeringManager.DEBUG) System.out.println("left-front to "+normal.length());
395 
396             //When closer, the bot is pushed away from the wall by another force.
397             if (botToHitLocation.length() < shortSideFrontRayLength) {
398                 shortrays = true;
399                 multiplier = Math.pow(((shortSideFrontRayLength - botToHitLocation.length()) * 2f / shortSideFrontRayLength), orderOfTheForce);
400                 normal.normalize();
401                 normal.scale(repulsiveForceWeight * multiplier * wallForce);
402                 nextVelocity.add((Tuple3d) normal);
403                 //if (SteeringManager.DEBUG) System.out.println("left-front from "+normal.length());
404             }
405 
406         }
407         if (sensornRightFront) {
408             //The force, which hauls the bot towards to the wall.
409             botToHitLocation = new Vector3d(nrightfront.getHitLocation().x - botself.getLocation().x, nrightfront.getHitLocation().y - botself.getLocation().y, 0);
410             normal = nrightfront.getHitNormal();
411             multiplier = Math.pow(1 - (longSideFrontRayLength - botToHitLocation.length()) / longSideFrontRayLength, orderOfTheForce);
412             normal.scale(attractiveForceWeight * multiplier * wallForce);
413             nextVelocity.sub((Tuple3d) normal);
414             //if (SteeringManager.DEBUG) System.out.println("right-front to "+normal.length());
415 
416             //When closer, the bot is pushed away from the wall by another force.
417             if (botToHitLocation.length() < shortSideFrontRayLength) {
418                 shortrays = true;
419                 multiplier = Math.pow(((shortSideFrontRayLength - botToHitLocation.length()) * 2f / shortSideFrontRayLength), orderOfTheForce);
420                 normal.normalize();
421                 normal.scale(repulsiveForceWeight * multiplier * wallForce);
422                 nextVelocity.add((Tuple3d) normal);
423                 //if (SteeringManager.DEBUG) System.out.println("right-front from "+normal.length());
424             }
425         }
426         if (sensornFront) {
427             //No attractive force in this case.
428             botToHitLocation = new Vector3d(nfront.getHitLocation().x - botself.getLocation().x, nfront.getHitLocation().y - botself.getLocation().y, 0);
429             normal = nfront.getHitNormal();
430             //multiplier = Math.pow(1 - (longSideFrontRayLength - botToHitLocation.length()) / longSideFrontRayLength, orderOfTheForce);
431             //normal.scale(multiplier * MAGNITUDE_OF_THE_FORCE);
432             //nextVelocity.sub((Tuple3d) normal);
433 
434             //When closer, the bot is pushed away from the wall by another force.
435             if (botToHitLocation.length() < shortFrontRayLength) {
436                 shortrays = true;
437                 multiplier = Math.pow(((shortFrontRayLength - botToHitLocation.length()) * 2f / shortFrontRayLength), orderOfTheForce);
438                 normal.normalize();
439                 normal.scale(repulsiveForceWeight * 3 * multiplier * wallForce);    //3 krát, aby to mělo větší vliv.
440                 //if (SteeringManager.DEBUG) System.out.println("Hey front "+normal.length());
441                 nextVelocity.add((Tuple3d) normal);
442             }
443         }
444         //</editor-fold>
445         
446         boolean nextTurning = false;
447         
448         //<editor-fold defaultstate="collapsed" desc="Convex edges">
449         //System.out.println("Convex turning. "+convexTurning);
450         if (specialDetection) {
451             if (state.equals(State.LEFT)) {
452                 if ( (sensornFront && (convexTurning || (Math.abs(nfront.getHitNormal().angle(actualVelocity) - Math.PI) < Math.PI/4)) ) ||
453                    ( (sensornLeftFront && (Math.abs(nleftfront.getHitNormal().angle(actualVelocity) - Math.PI) < Math.PI/2) && convexTurning) ||  //Pokud jsme se začali točit, tak se točíme tak dlouho, až jdeme rovnoběžně se zdí.
454                      (sensornLeftFront && (Math.abs(nleftfront.getHitNormal().angle(actualVelocity) - Math.PI) < Math.PI/4)) || //Může se stát, že přední paprsek ten roh mine, ale přesto jdeme kolmo na zeď.
455                      (sensornLeft      && (Math.abs(nleft.getHitNormal().angle(actualVelocity)      - Math.PI) < Math.PI/2) && convexTurning) ) ) {
456                     if (SteeringManager.DEBUG) System.out.println("Left convex edge. " + SteeringTools.radiansToDegrees(nleftfront.getHitNormal().angle(actualVelocity)));
457                     if (SteeringManager.DEBUG) {
458                         if (sensornFront) System.out.println("Left convex edge front collision. " + SteeringTools.radiansToDegrees(nfront.getHitNormal().angle(actualVelocity)));
459                         else if (sensornLeftFront) System.out.println("Left convex edge side front. " + SteeringTools.radiansToDegrees(nleftfront.getHitNormal().angle(actualVelocity)));
460                         else System.out.println("Left convex edge side. " + SteeringTools.radiansToDegrees(nleft.getHitNormal().angle(actualVelocity)));
461                     }
462                     //Kdykoliv když naráží přední paprsek - či pokud jsme se začali točit a zeď levého paprsku je na nás buď kolmá, či její normála svírá s naší velocity 135° - 225°.
463                     nextTurning = true;
464                     double k = 1;
465                     if (sensornFront) {
466                         k = nfront.getHitNormal().angle(actualVelocity) / Math.PI; //Jestliže jsou vektory přímo opačné, bude úhel PI a k=1. Čím je úhel menší, tím bude k menší.
467                     }
468                     else if (sensornLeftFront) {
469                         k = 0.6*nleftfront.getHitNormal().angle(actualVelocity) / Math.PI;
470                     }
471                     else {
472                         k = 0.3*nleft.getHitNormal().angle(actualVelocity) / Math.PI;
473                     }
474                     if (SteeringManager.DEBUG) {
475                         System.out.println("Turning vector. " + k*SteeringTools.getTurningVector2(actualVelocity, false).length());
476                     }
477                     Vector3d turningVector = SteeringTools.getTurningVector2(actualVelocity, false);
478                     turningVector.scale(k*convexEdgesForceWeight);
479                     //nextVelocity.add(turningVector);
480                     //nextVelocity = turningVector;
481                     counter = DEFAULTCOUNTER;
482                     convexTurning = nextTurning;
483                     return turningVector;
484                     //return nextVelocity;
485                 }
486             } else if (state.equals(State.RIGHT)) {
487                 if ( (sensornFront && (convexTurning || (Math.abs(nfront.getHitNormal().angle(actualVelocity) - Math.PI) < Math.PI/4)) ) ||
488                    ( (sensornRightFront && (Math.abs(nrightfront.getHitNormal().angle(actualVelocity) - Math.PI) < Math.PI/2) && convexTurning) ||
489                      (sensornRightFront && (Math.abs(nrightfront.getHitNormal().angle(actualVelocity) - Math.PI) < Math.PI/4)) || //Může se stát, že přední paprsek ten roh mine, ale přesto jdeme kolmo na zeď.
490                      (sensornRight      && (Math.abs(nright.getHitNormal().angle(actualVelocity)      - Math.PI) < Math.PI/2) && convexTurning) ) ) {
491                     if (SteeringManager.DEBUG) {
492                         if (sensornFront) System.out.println("Right convex edge front collision. " + SteeringTools.radiansToDegrees(nfront.getHitNormal().angle(actualVelocity)));
493                         else if (sensornRightFront) System.out.println("Right convex edge side front. " + SteeringTools.radiansToDegrees(nrightfront.getHitNormal().angle(actualVelocity)));
494                         else System.out.println("Right convex edge side. " + SteeringTools.radiansToDegrees(nright.getHitNormal().angle(actualVelocity)));
495                     }
496                     nextTurning = true;
497                     double k = 1;
498                     if (sensornFront) {
499                         k = nfront.getHitNormal().angle(actualVelocity) / Math.PI; //Jestliže jsou vektory přímo opačné, bude úhel PI a k=1. Čím je úhel menší, tím bude k menší.
500                     }
501                     else if (sensornRightFront) {
502                         k = 0.6*nrightfront.getHitNormal().angle(actualVelocity) / Math.PI;
503                     }
504                     else {
505                         k = 0.3*nright.getHitNormal().angle(actualVelocity) / Math.PI;
506                     }
507                     if (SteeringManager.DEBUG) {
508                         System.out.println("Turning vector. " + k*SteeringTools.getTurningVector2(actualVelocity, true).length());
509                     }
510                     Vector3d turningVector = SteeringTools.getTurningVector2(actualVelocity, true);
511                     turningVector.scale(k*convexEdgesForceWeight);
512                     //nextVelocity.add(turningVector);
513                     //nextVelocity = turningVector;
514                     counter = DEFAULTCOUNTER;
515                     convexTurning = nextTurning;
516                     return turningVector;
517                     //return nextVelocity;
518                 }
519             }
520         } else {    //Basic solution of the convex edges. Convex edge <==> the front and sideFront ray hits.
521                     //If the convex edge is detected, the exactly turning vector is returned.
522             if (state == State.LEFT) {
523                 if (sensornLeftFront && sensornFront) {    //This means the concave edge. We must turn right.
524                     //if (counter > 0) {
525                     //counter--;
526                     Vector3d turningVector = SteeringTools.getTurningVector2(actualVelocity, false);
527                     turningVector.scale(convexEdgesForceWeight*0.8);
528                     return turningVector;
529                     /*} else {
530                         state = State.NOTHING;
531                     }*/
532                 }
533             } else if (state == State.RIGHT) {
534                 if (sensornRightFront && sensornFront) {    //This means the concave edge. We must turn left.
535                     //if (counter > 0) {
536                     //counter--;
537                     Vector3d turningVector = SteeringTools.getTurningVector2(actualVelocity, true);
538                     turningVector.scale(convexEdgesForceWeight*0.8);
539                     return turningVector;
540                     /*} else {
541                         state = State.NOTHING;
542                     }*/
543                 } 
544             }
545         }
546        
547 
548         //</editor-fold>
549 
550         //<editor-fold defaultstate="collapsed" desc="Concave edges">
551 
552         //If the concave edge is detected, the exactly turning vector is returned.
553 
554         if (state == State.LEFT) {
555             if (!sensornLeft && !sensornLeftFront) {//He left the wall completely, no sensor on the left side works. That is for turning around >=90°(& <=270° :-)) angles when he needs to "remember" that he should turn.
556                 if (SteeringManager.DEBUG) System.out.println("counter "+counter);
557                 if (counter > 0) {
558                     counter--;
559                     if (SteeringManager.DEBUG) System.out.println("Left concave edge. ");
560                     Vector3d turningVector = SteeringTools.getTurningVector2(actualVelocity, true);
561                     if (convexTurning) turningVector.scale(0.5);
562                     turningVector.scale(concaveEdgesForceWeight*0.8);
563                     return turningVector;
564                 } else {
565                     state = State.NOTHING;
566                 }
567             }
568         } else if (state == State.RIGHT) {
569             if (!sensornRight && !sensornRightFront) {//He left the wall completely, no sensor on the left side works. That is for turning around >=90°(& <=270° :-)) angles when he needs to "remember" that he should turn.            {
570                 if (SteeringManager.DEBUG) System.out.println("counter "+counter);
571                 if (counter > 0) {
572                     counter--;
573                     if (SteeringManager.DEBUG) System.out.println("Right concave edge. ");
574                     Vector3d turningVector = SteeringTools.getTurningVector2(actualVelocity, false);
575                     if (convexTurning) turningVector.scale(0.5);
576                     turningVector.scale(concaveEdgesForceWeight*0.8);
577                     return turningVector;
578                     //nextVelocity.add(SteeringTools.getTurningVector(actualVelocity, false));
579                     //nextVelocity = SteeringTools.getTurningVector(actualVelocity, false);
580                 } else {
581                     state = State.NOTHING;
582                 }
583             }
584         }
585         //</editor-fold>
586 
587         convexTurning = nextTurning;
588         return nextVelocity;
589     }
590 
591     @Override
592     public void setProperties(SteeringProperties newProperties) {
593         this.wallForce = ((WallFollowingProperties)newProperties).getWallForce();
594         this.orderOfTheForce = ((WallFollowingProperties)newProperties).getOrderOfTheForce();
595         this.attractiveForceWeight = ((WallFollowingProperties)newProperties).getAttractiveForceWeight();
596         this.repulsiveForceWeight = ((WallFollowingProperties)newProperties).getRepulsiveForceWeight();
597         this.concaveEdgesForceWeight = ((WallFollowingProperties)newProperties).getConcaveEdgesForceWeight();
598         this.convexEdgesForceWeight = ((WallFollowingProperties)newProperties).getConvexEdgesForceWeight();
599         this.justMySide = ((WallFollowingProperties)newProperties).isJustMySide();
600         this.specialDetection = ((WallFollowingProperties)newProperties).isSpecialDetection();
601         this.frontCollisions = ((WallFollowingProperties)newProperties).isFrontCollisions();
602     }
603 
604     public WallFollowingProperties getProperties() {
605         WallFollowingProperties properties = new WallFollowingProperties();
606         properties.setWallForce(wallForce);
607         properties.setOrderOfTheForce(orderOfTheForce);
608         properties.setAttractiveForceWeight(attractiveForceWeight);
609         properties.setRepulsiveForceWeight(repulsiveForceWeight);
610         properties.setConcaveEdgesForceWeight(concaveEdgesForceWeight);
611         properties.setConvexEdgesForceWeight(convexEdgesForceWeight);
612         properties.setJustMySide(justMySide);
613         properties.setSpecialDetection(specialDetection);
614         properties.setFrontCollisions(frontCollisions);
615         return properties;
616     }
617 }