O --- O
libPEDSIM Documentation
[HOME] [EXAMPLES] [ECOSYSTEM] [DOCUMENTATION] [DOWNLOAD]
O --- O
ped_vector.cpp
1 //
2 // pedsim - A microscopic pedestrian simulation system.
3 // Copyright (c) by Christian Gloor
4 //
5 
6 #include "ped_vector.h"
7 
8 #include <cmath>
9 #include <string>
10 
11 #ifdef _WIN32
12 const double M_PI = 3.14159265359;
13 #endif
14 
15 
18 Ped::Tvector::Tvector() : x(0), y(0), z(0) {};
19 
20 
21 std::string Ped::Tvector::to_string() const {
22  return std::to_string(x) + "/" + std::to_string(y) + "/" + std::to_string(z);
23 }
24 
25 
28 double Ped::Tvector::length() const {
29  if ((x == 0) && (y == 0) && (z == 0)) return 0;
30  return sqrt(lengthSquared());
31 }
32 
33 
37  return x*x + y*y + z*z;
38 }
39 
40 
44  double len = length();
45 
46  // null vectors cannot be normalized
47  if (len == 0) return;
48 
49  x /= len;
50  y /= len;
51  z /= len;
52 }
53 
54 
58  double len = length();
59 
60  // null vectors cannot be normalized
61  if (len == 0) return Ped::Tvector();;
62 
63  return Ped::Tvector(x/len, y/len, z/len);
64 }
65 
66 
72 double Ped::Tvector::scalar(const Ped::Tvector &a, const Ped::Tvector &b) {
73  return acos( Ped::Tvector::dotProduct(a, b) / ( a.length() * b.length() ) );
74 }
75 
76 
83  return (a.x*b.x + a.y*b.y + a.z*b.z);
84 }
85 
86 
92  return Ped::Tvector(
93  a.y*b.z - a.z*b.y,
94  a.z*b.x - a.x*b.z,
95  a.x*b.y - a.y*b.x);
96 }
97 
98 
102 void Ped::Tvector::scale(double factor) {
103  x *= factor;
104  y *= factor;
105  z *= factor;
106 }
107 
108 
113 Ped::Tvector Ped::Tvector::scaled(double factor) const {
114  return Ped::Tvector(factor*x, factor*y, factor*z);
115 }
116 
117 // \warning: This is in 2D only!
118 Ped::Tvector Ped::Tvector::leftNormalVector() const {
119  return Ped::Tvector(-y, x);
120 }
121 
122 
123 // \warning: This is in 2D only!
124 Ped::Tvector Ped::Tvector::rightNormalVector() const {
125  return Ped::Tvector(y, -x);
126 }
127 
128 
129 double Ped::Tvector::polarRadius() const {
130  return length();
131 }
132 
133 
134 // \warning: This is in 2D only!
135 double Ped::Tvector::polarAngle() const {
136  return atan2(y, x);
137 }
138 
139 
140 // \warning: This is in 2D only!
141 double Ped::Tvector::angleTo(const Tvector &other) const {
142  double angleThis = polarAngle();
143  double angleOther = other.polarAngle();
144 
145  // compute angle
146  double diffAngle = angleOther - angleThis;
147  // → normalize angle
148  if (diffAngle > M_PI) diffAngle -= 2 * M_PI;
149  else if(diffAngle <= -M_PI) diffAngle += 2 * M_PI;
150 
151  return diffAngle;
152 }
153 
154 
155 Ped::Tvector Ped::Tvector::operator+(const Tvector& other) const {
156  return Ped::Tvector(
157  x + other.x,
158  y + other.y,
159  z + other.z);
160 }
161 
162 
163 Ped::Tvector Ped::Tvector::operator-(const Tvector& other) const {
164  return Ped::Tvector(
165  x - other.x,
166  y - other.y,
167  z - other.z);
168 }
169 
170 
171 Ped::Tvector Ped::Tvector::operator*(double factor) const {
172  return scaled(factor);
173 }
174 
175 
176 Ped::Tvector Ped::Tvector::operator/(double divisor) const {
177  return scaled(1/divisor);
178 }
179 
180 
181 Ped::Tvector& Ped::Tvector::operator+=(const Tvector& vectorIn) {
182  x += vectorIn.x;
183  y += vectorIn.y;
184  z += vectorIn.z;
185  return *this;
186 }
187 
188 
189 Ped::Tvector& Ped::Tvector::operator-=(const Tvector& vectorIn) {
190  x -= vectorIn.x;
191  y -= vectorIn.y;
192  z -= vectorIn.z;
193  return *this;
194 }
195 
196 
197 Ped::Tvector& Ped::Tvector::operator*=(double factor) {
198  scale(factor);
199  return *this;
200 }
201 
202 
203 Ped::Tvector& Ped::Tvector::operator*=(const Tvector& vectorIn) {
204  x *= vectorIn.x;
205  y *= vectorIn.y;
206  z *= vectorIn.z;
207  return *this;
208 }
209 
210 
211 Ped::Tvector& Ped::Tvector::operator/=(double divisor) {
212  scale(1/divisor);
213  return *this;
214 }
215 
216 
217 bool operator==(const Ped::Tvector& vector1In, const Ped::Tvector& vector2In) {
218  return (vector1In.x == vector2In.x)
219  && (vector1In.y == vector2In.y)
220  && (vector1In.z == vector2In.z);
221 }
222 
223 
224 bool operator!=(const Ped::Tvector& vector1In, const Ped::Tvector& vector2In) {
225  return (vector1In.x != vector2In.x)
226  || (vector1In.y != vector2In.y)
227  || (vector1In.z != vector2In.z);
228 }
229 
230 
231 Ped::Tvector operator+(const Ped::Tvector& vector1In, const Ped::Tvector& vector2In) {
232  return Ped::Tvector(
233  vector1In.x + vector2In.x,
234  vector1In.y + vector2In.y,
235  vector1In.z + vector2In.z);
236 }
237 
238 
239 Ped::Tvector operator-(const Ped::Tvector& vector1In, const Ped::Tvector& vector2In) {
240  return Ped::Tvector(
241  vector1In.x - vector2In.x,
242  vector1In.y - vector2In.y,
243  vector1In.z - vector2In.z);
244 }
245 
246 
247 Ped::Tvector operator-(const Ped::Tvector& vectorIn) {
248  return Ped::Tvector(
249  -vectorIn.x,
250  -vectorIn.y,
251  -vectorIn.z);
252 }
253 
254 
255 Ped::Tvector operator*(double factor, const Ped::Tvector& vector) {
256  return vector.scaled(factor);
257 }
258 
259 
264 bool Ped::Tvector::lineIntersection(const Ped::Tvector &p0, const Ped::Tvector &p1, const Ped::Tvector &p2, const Ped::Tvector &p3, Ped::Tvector *intersection) {
265  Ped::Tvector s1(p1.x - p0.x, p1.y - p0.y);
266  Ped::Tvector s2(p3.x - p2.x, p3.y - p2.y);
267 
268  double s = (-s1.y * (p0.x - p2.x) + s1.x * (p0.y - p2.y)) / (-s2.x * s1.y + s1.x * s2.y);
269  double t = ( s2.x * (p0.y - p2.y) - s2.y * (p0.x - p2.x)) / (-s2.x * s1.y + s1.x * s2.y);
270 
271  if (s >= 0 && s <= 1 && t >= 0 && t <= 1) { // intersection
272  if (intersection != NULL) {
273  intersection->x = p0.x + (t * s1.x);
274  intersection->y = p0.y + (t * s1.y);
275  }
276  return true;
277  }
278 
279  return false; // No intersection
280 }
281 
285 void Ped::Tvector::rotate(double theta) { // theta in rad.
286  double xt = x * cos(theta) - y * sin(theta);
287  double yt = x * sin(theta) + y * cos(theta);
288  x = xt; y = yt;
289 }
290 
294 Ped::Tvector Ped::Tvector::rotated(double theta) const { // theta in rad
295  return Ped::Tvector(x * cos(theta) - y * sin(theta), x * sin(theta) + y * cos(theta)); // let's hope the compiler reuses sin/cos
296 }
297 
void rotate(double theta)
Definition: ped_vector.cpp:285
static Tvector crossProduct(const Tvector &a, const Tvector &b)
Definition: ped_vector.cpp:91
Tvector scaled(double factor) const
Definition: ped_vector.cpp:113
double lengthSquared() const
Definition: ped_vector.cpp:36
Ped::Tvector rotated(double theta) const
Definition: ped_vector.cpp:294
Tvector normalized() const
Definition: ped_vector.cpp:57
void normalize()
Definition: ped_vector.cpp:43
static double scalar(const Tvector &a, const Tvector &b)
Definition: ped_vector.cpp:72
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
double length() const
Definition: ped_vector.cpp:28
static double dotProduct(const Tvector &a, const Tvector &b)
Definition: ped_vector.cpp:82
void scale(double factor)
Definition: ped_vector.cpp:102
O --- O

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