O --- O
libPEDSIM Documentation
[HOME] [EXAMPLES] [ECOSYSTEM] [DOCUMENTATION] [DOWNLOAD]
O --- O
ped_waypoint.cpp
1 //
2 // pedsim - A microscopic pedestrian simulation system.
3 // Copyright (c) by Christian Gloor
4 //
5 
6 #include "ped_waypoint.h"
7 
8 #include <cmath>
9 
10 
16 Ped::Twaypoint::Twaypoint(double px, double py, double pr) : Twaypoint() {
17  x = px;
18  y = py;
19  r = pr;
20 };
21 
22 
25 Ped::Twaypoint::Twaypoint() : x(0), y(0), r(1), type(Ped::Twaypoint::TYPE_NORMAL) {
26  static int staticid = 0;
27  id = staticid++;
28 };
29 
30 
35 
36 
44 Ped::Tvector Ped::Twaypoint::normalpoint(const Ped::Tvector& p, const Ped::Tvector& normalLineStart, const Ped::Tvector& normalLineEnd) const {
45  Ped::Tvector relativeEnd = normalLineEnd - normalLineStart;
46 
47  double lambda = (Tvector::dotProduct(p, relativeEnd) - Tvector::dotProduct(normalLineStart, relativeEnd)) / relativeEnd.lengthSquared();
48 
49  if (lambda <= 0)
50  return normalLineStart;
51  else if (lambda >= 1)
52  return normalLineEnd;
53  else
54  return normalLineStart + lambda*relativeEnd;
55 }
56 
57 
68 Ped::Tvector Ped::Twaypoint::normalpoint(double p1, double p2, double oc11, double oc12, double oc21, double oc22) const {
69  return normalpoint(Tvector(p1, p2), Tvector(oc11, oc12), Tvector(oc21, oc22));
70 }
71 
72 
81 Ped::Tvector Ped::Twaypoint::getForce(double agentX, double agentY, double fromx, double fromy, bool *reached) const {
82  if(type == Ped::Twaypoint::TYPE_NORMAL) {
83  Tvector diff(x - agentX, y - agentY);
84 
85  if(reached != NULL) {
86  if(diff.length() < r)
87  *reached = true;
88  else
89  *reached = false;
90  }
91  return diff.normalized();
92 
93 // Old code: (why is this so complicated?)
94 // Tvector diff(x - fromx, y - fromy);
95 // Tvector diffDirection = diff.normalized();
96 // Tvector scaledDiffDirection = r * diffDirection;
97 //
98 // double oc11 = x + scaledDiffDirection.x;
99 // double oc12 = y - scaledDiffDirection.y;
100 // double oc21 = x - scaledDiffDirection.x;
101 // double oc22 = y + scaledDiffDirection.y;
102 //
103 // Ped::Tvector pnormal = normalpoint(agentX, agentY, oc11, oc12, oc21, oc22);
104 // //TODO: use normalpoint(Tvector(p1, p2), Tvector(oc11, oc12), Tvector(oc21, oc22));
105 //
106 // Tvector pndistance(agentX - pnormal.x, agentY - pnormal.y);
107 // double pndist = pndistance.length();
108 //
109 // if(pndist == 0)
110 // return Ped::Tvector();
111 //
112 // if(reached != NULL) {
113 // if(pndist < 3)
114 // *reached = true;
115 // else
116 // *reached = false;
117 // }
118 // return -pndistance.normalized();
119  }
120  else if(type == Ped::Twaypoint::TYPE_POINT) {
121  Tvector diff(x - agentX, y - agentY);
122 
123  if(reached != NULL) {
124  if(diff.length() < r)
125  *reached = true;
126  else
127  *reached = false;
128  }
129  return diff.normalized();
130  }
131  else {
132  // unknown waypoint type
133  return Tvector();
134  }
135 }
double x
position of the waypoint
Definition: ped_waypoint.h:66
virtual Tvector getForce(double myx, double myy, double fromx, double fromy, bool *reached=NULL) const
double lengthSquared() const
Definition: ped_vector.cpp:36
double y
position of the waypoint
Definition: ped_waypoint.h:67
virtual Tvector normalpoint(const Tvector &p, const Tvector &obstacleStart, const Tvector &obstacleEnd) const
double r
position of the waypoint
Definition: ped_waypoint.h:68
Tvector normalized() const
Definition: ped_vector.cpp:57
virtual ~Twaypoint()
double length() const
Definition: ped_vector.cpp:28
static double dotProduct(const Tvector &a, const Tvector &b)
Definition: ped_vector.cpp:82
O --- O

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