1 package Steerings;
2
3 import SteeringStuff.SteeringManager;
4 import SteeringStuff.SteeringTools;
5 import SteeringStuff.RefBoolean;
6 import SteeringProperties.PeopleAvoidanceProperties;
7 import SteeringProperties.SteeringProperties;
8 import cz.cuni.amis.pogamut.base3d.worldview.object.Location;
9 import cz.cuni.amis.pogamut.unreal.communication.messages.UnrealId;
10 import javax.vecmath.Vector3d;
11 import cz.cuni.amis.pogamut.ut2004.bot.impl.UT2004Bot;
12 import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.Player;
13 import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.Self;
14 import SteeringStuff.ISteering;
15 import java.util.Collection;
16 import java.util.Random;
17 import javax.vecmath.Tuple3d;
18 import javax.vecmath.Vector2d;
19
20
21
22
23
24
25 public class PeopleAvoidanceSteer implements ISteering {
26
27
28 private UT2004Bot botself;
29
30
31 private int repulsiveForce;
32
33 private int distanceFromOtherPeople;
34
35 private boolean circumvention;
36
37 private double projection;
38 private boolean deceleration = true;
39 private boolean acceleration = false;
40
41 private static int TICK_PARTS = 5;
42
43 Random random = new Random();
44
45
46
47
48 public PeopleAvoidanceSteer(UT2004Bot bot) {
49 botself = bot;
50 }
51
52
53 public Vector3d run(Vector3d scaledActualVelocity, RefBoolean wantsToGoFaster, RefBoolean wantsToStop, Location focus)
54 {
55
56 Vector3d actualVelocity = botself.getVelocity().getVector3d();
57
58 Vector3d nextVelocity = new Vector3d(0,0,0);
59
60
61 Vector3d otherBotToFollower;
62
63
64 UnrealId myId = botself.getWorldView().getSingle(Self.class).getId();
65
66 wantsToGoFaster.setValue(true);
67
68
69 Collection<Player> col = botself.getWorldView().getAllVisible(Player.class).values();
70
71 for (Player p : col) {
72 if (p.getId() != myId) {
73 if (deceleration || acceleration) {
74 Vector3d notPushVector = notPushPartner(scaledActualVelocity, p, wantsToGoFaster);
75 nextVelocity.add((Tuple3d) notPushVector);
76 } else if (circumvention) {
77 Vector3d goRoundVector = goRoundPartner(p);
78 nextVelocity.add((Tuple3d) goRoundVector);
79 }
80 if(p.getLocation().getDistance(botself.getLocation()) < distanceFromOtherPeople) {
81 Vector3d botToPlayer = new Vector3d(p.getLocation().x - botself.getLocation().x, p.getLocation().y - botself.getLocation().y, 0);
82 if (actualVelocity.angle(botToPlayer) < Math.PI/2) {
83 otherBotToFollower = new Vector3d(botself.getLocation().x - p.getLocation().x, botself.getLocation().y - p.getLocation().y, botself.getLocation().z - p.getLocation().z);
84 otherBotToFollower.normalize();
85 otherBotToFollower.scale(repulsiveForce * (distanceFromOtherPeople - botself.getLocation().getDistance(p.getLocation())) / distanceFromOtherPeople);
86 if (SteeringManager.DEBUG) System.out.println("Bot "+p.getName()+" je moc blizko ==> odpudivá síla "+otherBotToFollower.length());
87 wantsToGoFaster.setValue(false);
88 nextVelocity.add((Tuple3d) otherBotToFollower);
89 }
90 }
91 }
92 }
93 return nextVelocity;
94 }
95
96 private Vector3d goRoundPartner(Player player) {
97 Vector3d result = new Vector3d(0,0,0);
98 Location myActualLocation = botself.getLocation();
99 Vector3d myVelocity = botself.getVelocity().getVector3d();
100 Location hisActualLocation = player.getLocation();
101 Vector3d hisVelocity = player.getVelocity().getVector3d();
102 Location myNextLocation = new Location();
103 Location hisNextLocation = new Location();
104 double collisionTime = -1;
105 for(int t=0;t <= projection*TICK_PARTS;t++){
106 double time = ((double)t)/TICK_PARTS;
107 myNextLocation = getLocationAfterTime(myActualLocation, myVelocity, time);
108 hisNextLocation = getLocationAfterTime(hisActualLocation, hisVelocity, time);
109 if (myNextLocation.getDistance(hisNextLocation) <= distanceFromOtherPeople) {
110 collisionTime = time;
111 break;
112 }
113 }
114 if (collisionTime != -1) {
115 double ourNextDistance = myNextLocation.getDistance(hisNextLocation);
116 Vector3d myNextLocationToHis = new Vector3d(hisNextLocation.x - myNextLocation.x, hisNextLocation.y - myNextLocation.y, hisNextLocation.z - myNextLocation.z);
117 double ourNextAngle = myNextLocationToHis.angle(myVelocity);
118
119 Vector3d turningVector;
120 double koefA, koefB;
121 boolean turnLeft;
122
123
124
125 if (ourNextAngle == 0) {
126 turnLeft = random.nextBoolean();
127 if (SteeringManager.DEBUG) {
128 System.out.println("Partner exactly front collision. "+turnLeft);
129 }
130 koefA = 1;
131 koefB = getKoefB(collisionTime);
132 } else {
133 koefA = getKoefA(ourNextAngle, ourNextDistance);
134 koefB = getKoefB(collisionTime);
135 turnLeft = !SteeringTools.pointIsLeftFromTheVector(myVelocity, myNextLocationToHis);
136 if (SteeringManager.DEBUG) System.out.println("Partner nearly front collision. " + turnLeft);
137 if (SteeringManager.DEBUG) System.out.println("Distance " + ourNextDistance + " koefA " + koefA + " koefB " + koefB);
138 }
139 turningVector = SteeringTools.getTurningVector2(botself.getVelocity().getVector3d(), turnLeft);
140 turningVector.normalize();
141 turningVector.scale(2*repulsiveForce * koefA * koefB);
142 if (SteeringManager.DEBUG) System.out.println("Turning vector " + turningVector.length());
143 result.add(turningVector);
144 }
145 return result;
146 }
147
148
149 private double getKoefA(double angle, double distance) {
150 return Math.max(getKoefA1(angle), getKoefA2(distance));
151 }
152
153
154
155 private double getKoefA1(double angle) {
156 return Math.max(0, (Math.PI/2 - angle) / Math.PI/2);
157 }
158
159
160
161 private double getKoefA2(double distance) {
162 return Math.max(0, (distanceFromOtherPeople - distance) / distanceFromOtherPeople);
163 }
164
165
166
167 private double getKoefB(double ticks) {
168 double koef;
169 if (projection > 0) {
170 koef = (projection - ticks)/(projection - 1);
171 } else {
172 koef = 1;
173 }
174 return koef;
175 }
176
177 private Location getLocationAfterTime(Location start, Vector3d velocity, double time) {
178 return new Location(start.x + time*velocity.x, start.y + time*velocity.y, start.z);
179 }
180
181
182
183 private Vector3d notPushPartner(Vector3d botsVelocity, Player player, RefBoolean wantsToGoFaster) {
184 Vector3d myVelo = botself.getVelocity().getVector3d();
185 Vector3d hisVelo = player.getVelocity().getVector3d();
186 Location myLoc = botself.getLocation();
187 Location hisLoc = player.getLocation();
188 Vector2d vInterSec = SteeringTools.getIntersectionOld(new Vector2d(myLoc.x, myLoc.y), new Vector2d(myVelo.x, myVelo.y), new Vector2d(hisLoc.x, hisLoc.y), new Vector2d(hisVelo.x, hisVelo.y));
189 Vector3d result = new Vector3d();
190 boolean noForce = true;
191 if (vInterSec != null) {
192 Location locInterSec = new Location(vInterSec.x, vInterSec.y, myLoc.z);
193 double myDist = locInterSec.getDistance(myLoc);
194 double hisDist = locInterSec.getDistance(hisLoc);
195 double myTime = myDist / myVelo.length();
196 double hisTime = hisDist / hisVelo.length();
197 double minTime = Math.min(myTime, hisTime);
198 Location myNewLoc = new Location(myLoc.x + myVelo.x * minTime, myLoc.y + myVelo.y * minTime, myLoc.z);
199 Location hisNewLoc = new Location(hisLoc.x + hisVelo.x * minTime, hisLoc.y + hisVelo.y * minTime, hisLoc.z);
200 double newLocsDiff = myNewLoc.getDistance(hisNewLoc);
201
202
203 double far_distance = projection*myVelo.length();
204 double far_distance2 = Math.max(far_distance,distanceFromOtherPeople+1);
205 if (myDist <= far_distance2 && newLocsDiff < 2*distanceFromOtherPeople) {
206 double koefA = (far_distance2 - myDist) / (far_distance2 - distanceFromOtherPeople);
207 koefA = Math.min(koefA, 1);
208 double koefB = ( 2*distanceFromOtherPeople - newLocsDiff) / (2*distanceFromOtherPeople);
209 if (myTime < hisTime && acceleration) {
210 if (SteeringManager.DEBUG) System.out.println("We speed up: koefA "+koefA+" koefB "+koefB);
211 noForce = false;
212 result = getBiggerVelocity(botsVelocity, 3*koefA*koefB, false, wantsToGoFaster);
213 } else if (myTime > hisTime && deceleration) {
214 if (SteeringManager.DEBUG) System.out.println("We slow down: koefA "+koefA+" koefB "+koefB);
215 noForce = false;
216 result = getBiggerVelocity(botsVelocity, koefA*koefB, true, wantsToGoFaster);
217 } else if (myTime == hisTime) {
218 boolean slowDown = random.nextBoolean();
219 if (SteeringManager.DEBUG) System.out.println("Random --> We slow down "+slowDown+" koefA "+koefA+" koefB "+koefB);
220 noForce = false;
221 result = getBiggerVelocity(botsVelocity, 5, slowDown, wantsToGoFaster);
222 }
223 }
224 }
225 if (noForce && circumvention) {
226 result = goRoundPartner(player);
227 }
228 if (SteeringManager.DEBUG) System.out.println("pushing force: " + result.length());
229 return result;
230 }
231
232 private Vector3d getBiggerVelocity(Vector3d velocity, double scale, boolean negate, RefBoolean wantsToGoFaster) {
233 Vector3d result = new Vector3d(velocity.x, velocity.y, velocity.z);
234 if (negate) {
235 result.negate();
236 wantsToGoFaster.setValue(false);
237 }
238 result.scale(scale);
239 return result;
240 }
241
242 public void setProperties(SteeringProperties newProperties) {
243 this.repulsiveForce = ((PeopleAvoidanceProperties)newProperties).getRepulsiveForce();
244 this.distanceFromOtherPeople = ((PeopleAvoidanceProperties)newProperties).getDistanceFromOtherPeople();
245 this.circumvention = ((PeopleAvoidanceProperties)newProperties).isCircumvention();
246 this.deceleration = ((PeopleAvoidanceProperties)newProperties).isDeceleration();
247 this.acceleration = ((PeopleAvoidanceProperties)newProperties).isAcceleration();
248 this.projection = ((PeopleAvoidanceProperties)newProperties).getProjection();
249 }
250
251 public PeopleAvoidanceProperties getProperties() {
252 PeopleAvoidanceProperties properties = new PeopleAvoidanceProperties();
253 properties.setRepulsiveForce(repulsiveForce);
254 properties.setDistanceFromOtherPeople(distanceFromOtherPeople);
255 properties.setCircumvention(circumvention);
256 properties.setDeceleration(deceleration);
257 properties.setAcceleration(acceleration);
258 properties.setProjection(projection);
259 return properties;
260 }
261 }