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