O --- O
libPEDSIM Documentation
[HOME] [EXAMPLES] [ECOSYSTEM] [DOCUMENTATION] [DOWNLOAD]
O --- O
ped_agent.cpp
1 //
2 // pedsim - A microscopic pedestrian simulation system.
3 // Copyright (c) by Christian Gloor
4 //
5 
6 #include "ped_agent.h"
7 #include "ped_waypoint.h"
8 #include "ped_scene.h"
9 #include "ped_obstacle.h"
10 
11 #include <cmath>
12 #include <algorithm>
13 #include <random>
14 #include <iostream>
15 
16 using namespace std;
17 
18 
19 default_random_engine generator;
20 
21 
25  static int staticid = 0;
26  id = staticid++;
27  p.x = 0;
28  p.y = 0;
29  p.z = 0;
30  v.x = 0;
31  v.y = 0;
32  v.z = 0;
33  type = 0;
34 
35  destination = NULL;
36  lastdestination = NULL;
37  follow = -1;
38  mlLookAhead = false;
39  scene = NULL;
40 
41  // assign random maximal speed in m/s
42  // normal distribution (mean 1.2, std 0.2)
43  normal_distribution<double> distribution(1.2, 0.2);
44  vmax = distribution(generator);
45 
46  factorsocialforce = 2.1;
47  factorobstacleforce = 1.0; // parameter based on plausible pedsim output, not real measurement!
48  factordesiredforce = 1.0;
49  factorlookaheadforce = 1.0;
50 
51  obstacleForceSigma = 0.8;
52 
53  agentRadius = 0.2;
54 
55  relaxationTime = 0.5;
56 
57  waypointbehavior = BEHAVIOR_CIRCULAR;
58 }
59 
60 
64 }
65 
66 
74  scene = s;
75 }
76 
82  return scene;
83 };
84 
85 
95  waypoints.push_back(wp);
96  destination = waypoints.front();
97 }
98 
99 
100 bool Ped::Tagent::removeWaypoint(const Twaypoint *wp) {
101  // unset references
102  if (destination == wp)
103  destination = NULL;
104  if (lastdestination == wp)
105  lastdestination = NULL;
106 
107  // remove waypoint from list of destinations
108  bool removed = false;
109  for (int i = waypoints.size(); i > 0; --i) {
110  Twaypoint* currentWaypoint = waypoints.front();
111  waypoints.pop_front();
112  if (currentWaypoint != wp) {
113  waypoints.push_back(currentWaypoint);
114  removed = true;
115  }
116  }
117 
118  return removed;
119 }
120 
121 
122 void Ped::Tagent::clearWaypoints() {
123  // unset references
124  destination = NULL;
125  lastdestination = NULL;
126 
127  // remove all references to the waypoints
128  // note: don't delete waypoints, because the scene is responsible
129  for (int i = waypoints.size(); i > 0; --i) {
130  waypoints.pop_front();
131  }
132 }
133 
134 /*
135 void Ped::Tagent::removeAgentFromNeighbors(const Ped::Tagent* agentIn) {
136  // search agent in neighbors, and remove him
137  set<const Ped::Tagent*>::iterator foundNeighbor = neighbors.find(agentIn);
138  if (foundNeighbor != neighbors.end()) {
139  neighbors.erase(foundNeighbor);
140  }
141 }
142 */ // no longer required
143 
150  follow = id;
151 }
152 
153 
159  return follow;
160 }
161 
162 
167 void Ped::Tagent::setVmax(double pvmax) {
168  vmax = pvmax;
169 }
170 
176  return vmax;
177 }
178 
179 
186 void Ped::Tagent::setPosition(double px, double py, double pz) {
187  p.x = px;
188  p.y = py;
189  p.z = pz;
190 }
191 
192 
198  factorsocialforce = f;
199 }
200 
201 
207  factorobstacleforce = f;
208 }
209 
210 
216  factordesiredforce = f;
217 }
218 
219 
225  factorlookaheadforce = f;
226 }
227 
228 
237 
238  // following behavior
239  if (follow >= 0) {
240  Tagent* followedAgent = scene->agents.at(follow);
241  Twaypoint newDestination(followedAgent->getPosition().x, followedAgent->getPosition().y, 0);
242  newDestination.settype(Ped::Twaypoint::TYPE_POINT);
243  Ped::Tvector ef = newDestination.getForce(p.x, p.y, 0, 0);
244  desiredDirection = Ped::Tvector(followedAgent->getPosition().x, followedAgent->getPosition().y);
245 
246  // walk with full speed if nothing else affects me
247  return vmax * ef;
248  }
249 
250  // waypoint management (fetch new destination if available)
251  if ((destination == NULL) && (!waypoints.empty())) {
252  destination = waypoints.front();
253  waypoints.pop_front();
254 
255  if (waypointbehavior == Ped::Tagent::BEHAVIOR_CIRCULAR) {
256  waypoints.push_back(destination);
257  }
258  }
259 
260  // Agent has reached last waypoint, or there never was one.
261  // if ((destination != NULL) && (waypoints.empty())) {
262  // destination = NULL;
263  // }
264 
265  // if there is no destination, don't move
266  if (destination == NULL) {
267  // desiredDirection = Ped::Tvector();
268  // Tvector antiMove = -v / relaxationTime;
269  // return antiMove;
270  // not shure about these lines above
271  desiredDirection = Ped::Tvector(0, 0, 0);
272  }
273 
274  bool reached = false;
275  if (destination != NULL) {
276  if (lastdestination == NULL) {
277  // create a temporary destination of type point, since no normal from last dest is available
278  Twaypoint tempDestination(destination->getx(), destination->gety(), destination->getr());
279  tempDestination.settype(Ped::Twaypoint::TYPE_POINT);
280  desiredDirection = tempDestination.getForce(p.x,
281  p.y,
282  0,
283  0,
284  &reached);
285  } else {
286  desiredDirection = destination->getForce(p.x,
287  p.y,
288  lastdestination->getx(),
289  lastdestination->gety(),
290  &reached);
291  }
292  }
293 
294  // mark destination as reached for next time step
295  if ((destination != NULL) && (reached == true)) {
296  lastdestination = destination;
297  destination = NULL;
298  }
299 
300  // Compute force. This translates to "I want to move into that
301  // direction at the maximum speed"
302  Tvector force = desiredDirection.normalized() * vmax;
303  // cout << force.to_string() << endl;
304 
305  return force;
306 }
307 
308 
316 Ped::Tvector Ped::Tagent::socialForce(const set<const Ped::Tagent*> &neighbors) {
317  // define relative importance of position vs velocity vector
318  // (set according to Moussaid-Helbing 2009)
319  const double lambdaImportance = 2.0;
320 
321  // define speed interaction
322  // (set according to Moussaid-Helbing 2009)
323  const double gamma = 0.35;
324 
325  // define speed interaction
326  // (set according to Moussaid-Helbing 2009)
327  const double n = 2;
328 
329  // define angular interaction
330  // (set according to Moussaid-Helbing 2009)
331  const double n_prime = 3;
332 
333  Tvector force;
334  for (const Ped::Tagent* other: neighbors) {
335  // don't compute social force to yourself
336  if (other->id == id) continue;
337 
338  // compute difference between both agents' positions
339  Tvector diff = other->p - p;
340 
341  // skip futher computation if they are too far away from each
342  // other. Should speed up things.
343  if (diff.lengthSquared() > 64.0) continue; // val to high --chgloor 20160630
344 
345  Tvector diffDirection = diff.normalized();
346 
347  // compute difference between both agents' velocity vectors
348  // Note: the agent-other-order changed here
349  Tvector velDiff = v - other->v;
350 
351  // compute interaction direction t_ij
352  Tvector interactionVector = lambdaImportance * velDiff + diffDirection;
353  double interactionLength = interactionVector.length();
354  Tvector interactionDirection = interactionVector / interactionLength;
355 
356  // compute angle theta (between interaction and position difference vector)
357  double theta = interactionDirection.angleTo(diffDirection);
358  int thetaSign = (theta == 0) ? (0) : (theta / abs(theta));
359 
360  // compute model parameter B = gamma * ||D||
361  double B = gamma * interactionLength;
362 
363  // According to paper, this should be the sum of the two forces...
364 // force += -exp(-diff.length()/B)
365 // * (exp(-pow(n_prime*B*theta,2)) * interactionDirection
366 // + exp(-pow(n*B*theta,2)) * interactionDirection.leftNormalVector());
367 
368  double forceVelocityAmount = -exp(-diff.length()/B - (n_prime*B*theta)*(n_prime*B*theta));
369  double forceAngleAmount = -thetaSign * exp(-diff.length()/B - (n*B*theta)*(n*B*theta));
370 
371  Tvector forceVelocity = forceVelocityAmount * interactionDirection;
372  Tvector forceAngle = forceAngleAmount * interactionDirection.leftNormalVector();
373 
374  force += forceVelocity + forceAngle;
375  }
376 
377 // Old code: (didn't follow papers)
378 // const double maxDistance = 10.0;
379 // const double maxDistSquared = maxDistance*maxDistance;
380 //
381 // Ped::Tvector force;
382 // for(set<const Ped::Tagent*>::iterator iter = neighbors.begin(); iter!=neighbors.end(); ++iter) {
383 // const Ped::Tagent* other = *iter;
384 //
385 // // don't compute social force to yourself
386 // if (other->id == id)
387 // continue;
388 //
389 // // quick distance check
390 // Ped::Tvector diff = other->p - p;
391 // if ((abs(diff.x) < maxDistance)
392 // && (abs(diff.y) < maxDistance)) {
393 // double dist2 = diff.lengthSquared();
394 //
395 // // ignore too small forces
396 // if (dist2 < maxDistSquared) {
397 // double expdist = exp(-sqrt(dist2)/socialForceSigma);
398 // force += -expdist * diff;
399 // }
400 // }
401 // }
402 
403  return force;
404 }
405 
406 
411 Ped::Tvector Ped::Tagent::obstacleForce(const set<const Ped::Tagent*> &neighbors) {
412  // obstacle which is closest only
413  Ped::Tvector minDiff;
414  double minDistanceSquared = INFINITY;
415 
416  for (const Tobstacle* obstacle : scene->obstacles) {
417  Ped::Tvector closestPoint = obstacle->closestPoint(p);
418  Ped::Tvector diff = p - closestPoint;
419  double distanceSquared = diff.lengthSquared(); // use squared distance to avoid computing square root
420  if (distanceSquared < minDistanceSquared) {
421  minDistanceSquared = distanceSquared;
422  minDiff = diff;
423  }
424  }
425 
426  double distance = sqrt(minDistanceSquared) - agentRadius;
427  double forceAmount = exp(-distance/obstacleForceSigma);
428  return forceAmount * minDiff.normalized();
429 }
430 
431 
439 Ped::Tvector Ped::Tagent::lookaheadForce(Ped::Tvector e, const set<const Ped::Tagent*> &neighbors) {
440  const double pi = 3.14159265;
441  int lookforwardcount = 0;
442  for (set<const Ped::Tagent*>::iterator iter = neighbors.begin(); iter!=neighbors.end(); ++iter) {
443  const Ped::Tagent* other = *iter;
444 
445  // don't compute this force for the agent himself
446  if (other->id == id) continue;
447 
448  double distancex = other->p.x - p.x;
449  double distancey = other->p.y - p.y;
450  double dist2 = (distancex * distancex + distancey * distancey); // 2D
451  if (dist2 < 400) { // look ahead feature
452  double at2v = atan2(-e.x, -e.y); // was vx, vy --chgloor 2012-01-15
453  double at2d = atan2(-distancex, -distancey);
454  double at2v2 = atan2(-other->v.x, -other->v.y);
455  double s = at2d - at2v;
456  if (s > pi) s -= 2*pi;
457  if (s < -pi) s += 2*pi;
458  double vv = at2v - at2v2;
459  if (vv > pi) vv -= 2*pi;
460  if (vv < -pi) vv += 2*pi;
461  if (abs(vv) > 2.5) { // opposite direction
462  if ((s < 0) && (s > -0.3)) // position vor mir, in meine richtung
463  lookforwardcount--;
464  if ((s > 0) && (s < 0.3))
465  lookforwardcount++;
466  }
467  }
468  }
469 
470  Ped::Tvector lf;
471  if (lookforwardcount < 0) {
472  lf.x = 0.5f * e.y; // was vx, vy --chgloor 2012-01-15
473  lf.y = 0.5f * -e.x;
474  }
475  if (lookforwardcount > 0) {
476  lf.x = 0.5f * -e.y;
477  lf.y = 0.5f * e.x;
478  }
479  return lf;
480 }
481 
482 
489 Ped::Tvector Ped::Tagent::myForce(Ped::Tvector e, const set<const Ped::Tagent*> &neighbors) {
490  Ped::Tvector lf;
491  return lf;
492 }
493 
499  const double neighborhoodRange = 20.0;
500  auto neighbors = scene->getNeighbors(p.x, p.y, neighborhoodRange);
501 
502  desiredforce = desiredForce();
503  if (factorlookaheadforce > 0) lookaheadforce = lookaheadForce(desiredDirection, neighbors);
504  if (factorsocialforce > 0) socialforce = socialForce(neighbors);
505  if (factorobstacleforce > 0) obstacleforce = obstacleForce(neighbors);
506  myforce = myForce(desiredDirection, neighbors);
507 }
508 
509 
519 void Ped::Tagent::move(double h) {
520  // internal position update = actual move
521  // p = p + v * h;
522  Tvector p_desired = p + v * h;
523 
524 
525  Ped::Tvector intersection;
526  bool has_intersection = false;
527  for (auto obstacle : scene->getAllObstacles()) {
528  Ped::Tvector intersection;
529  // Ped::Tvector surface = obstacle->getEndPoint() - obstacle->getStartPoint();
530  // Ped::Tvector vd = surface.leftNormalVector().normalized() * 0.30; // min radius of agent
531 
532  // // walls left and right
533  // if (Ped::Tvector::lineIntersection(p, p_desired, obstacle->getStartPoint()-vd, obstacle->getEndPoint()-vd, &intersection) == 1) {
534  // p_desired = intersection - (v*h).normalized()*0.1;
535  // }
536  // if (Ped::Tvector::lineIntersection(p, p_desired, obstacle->getStartPoint()+vd, obstacle->getEndPoint()+vd, &intersection) == 1) {
537  // p_desired = intersection - (v*h).normalized()*0.1;
538  // }
539 
540  // // caps
541  // if (Ped::Tvector::lineIntersection(p, p_desired, obstacle->getStartPoint()-vd, obstacle->getStartPoint()+vd, &intersection) == 1) {
542  // p_desired = intersection - (v*h).normalized()*0.1;
543  // }
544  // if (Ped::Tvector::lineIntersection(p, p_desired, obstacle->getEndPoint()-vd, obstacle->getEndPoint()+vd, &intersection) == 1) {
545  // p_desired = intersection - (v*h).normalized()*0.1;
546  // }
547 
548  if (Ped::Tvector::lineIntersection(p, p_desired, obstacle->getStartPoint(), obstacle->getEndPoint(), &intersection) == 1) {
549  p_desired = intersection - (v*h).normalized()*0.1;
550  }
551  }
552 
553  p = p_desired; // update my position
554 
555 
556  // weighted sum of all forces --> acceleration
557  a = factordesiredforce * desiredforce
558  + factorsocialforce * socialforce
559  + factorobstacleforce * obstacleforce
560  + factorlookaheadforce * lookaheadforce
561  + myforce;
562 
563  // calculate the new velocity
564  v = 0.5 * v + a * h; // prob rather (0.5 / h) * v
565 
566  // don't exceed maximal speed
567  if (v.length() > vmax) v = v.normalized() * vmax;
568 
569  // notice scene of movement
570  scene->moveAgent(this);
571 }
void setfactordesiredforce(double f)
Definition: ped_agent.cpp:215
Tscene * getscene()
Definition: ped_agent.cpp:81
void setVmax(double vmax)
Definition: ped_agent.cpp:167
virtual Tvector socialForce(const set< const Ped::Tagent * > &neighbors)
Definition: ped_agent.cpp:316
void setFollow(int id)
Definition: ped_agent.cpp:149
Tvector p
current position of the agent
Definition: ped_agent.h:108
virtual Tvector lookaheadForce(Tvector desired, const set< const Ped::Tagent * > &neighbors)
Definition: ped_agent.cpp:439
void setfactorobstacleforce(double f)
Definition: ped_agent.cpp:206
int getFollow() const
Definition: ped_agent.cpp:158
double getVmax()
Definition: ped_agent.cpp:175
void addWaypoint(Twaypoint *wp)
Definition: ped_agent.cpp:94
double lengthSquared() const
Definition: ped_vector.cpp:36
virtual Tvector obstacleForce(const set< const Ped::Tagent * > &neighbors)
Definition: ped_agent.cpp:411
virtual void computeForces()
Definition: ped_agent.cpp:498
void setfactorsocialforce(double f)
Definition: ped_agent.cpp:197
void setPosition(double px, double py, double pz)
Definition: ped_agent.cpp:186
virtual Tvector desiredForce()
Definition: ped_agent.cpp:236
virtual Tvector closestPoint(double p1, double p2) const
virtual void move(double stepSizeIn)
Definition: ped_agent.cpp:519
Tvector normalized() const
Definition: ped_vector.cpp:57
virtual Tvector myForce(Tvector desired, const set< const Ped::Tagent * > &neighbors)
Definition: ped_agent.cpp:489
int id
agent number
Definition: ped_agent.h:107
void setfactorlookaheadforce(double f)
Definition: ped_agent.cpp:224
virtual ~Tagent()
Definition: ped_agent.cpp:63
static bool lineIntersection(const Ped::Tvector &p0, const Ped::Tvector &p1, const Ped::Tvector &p2, const Ped::Tvector &p3, Ped::Tvector *intersection)
Definition: ped_vector.cpp:264
Tvector v
velocity of the agent
Definition: ped_agent.h:109
double length() const
Definition: ped_vector.cpp:28
void setscene(Tscene *s)
Definition: ped_agent.cpp:73
O --- O

⁠(c) Christian Gloor [ c|h|g|l|o|o|r|@|s|i|l|m|a|r|i|l|.|o|r|g| ]