Eclipse SUMO - Simulation of Urban MObility
PolygonDynamics.cpp
Go to the documentation of this file.
1 /****************************************************************************/
2 // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo
3 // Copyright (C) 2004-2019 German Aerospace Center (DLR) and others.
4 // This program and the accompanying materials
5 // are made available under the terms of the Eclipse Public License v2.0
6 // which accompanies this distribution, and is available at
7 // http://www.eclipse.org/legal/epl-v20.html
8 // SPDX-License-Identifier: EPL-2.0
9 /****************************************************************************/
15 // A polygon, which holds a timeSpan for displaying dynamic properties
16 /****************************************************************************/
17 
18 
19 #include "PolygonDynamics.h"
20 
21 #include <assert.h>
22 #include "utils/common/StdDefs.h"
23 #include "utils/common/SUMOTime.h"
25 
26 
27 //#define DEBUG_DYNAMIC_SHAPES
28 
30  SUMOPolygon* p,
31  SUMOTrafficObject* trackedObject,
32  const std::vector<double>& timeSpan,
33  const std::vector<double>& alphaSpan,
34  bool looped,
35  bool rotate) :
36  myPolygon(p),
37  myCurrentTime(0),
38  myLastUpdateTime(creationTime),
39  animated(!timeSpan.empty()),
40  looped(looped),
41  tracking(trackedObject != nullptr),
42  rotate(rotate),
43  myTrackedObject(trackedObject),
44  myTrackedObjectID(""),
45  myTrackedObjectsInitialPositon(nullptr),
46  myTrackedObjectsInitialAngle(-1),
47  myOriginalShape(nullptr),
48  myTimeSpan(nullptr),
49  myAlphaSpan(nullptr),
50  myVis(nullptr) {
51  // Check for consistency
52  if (animated) {
53  myTimeSpan = std::unique_ptr<std::vector<double> >(new std::vector<double>(timeSpan));
54  assert(myTimeSpan->size() >= 2);
55  assert((*myTimeSpan)[0] == 0.0);
56  assert(myAlphaSpan == nullptr || myAlphaSpan->size() >= 2);
57 #ifdef DEBUG_DYNAMIC_SHAPES
58  if (myTimeSpan->size() >= 2) {
59  for (unsigned int i = 1; i < myTimeSpan->size(); ++i) {
60  assert((*myTimeSpan)[i - 1] <= (*myTimeSpan)[i]);
61  }
62  }
63 #endif
64  myPrevTime = myTimeSpan->begin();
65  myNextTime = ++myTimeSpan->begin();
66  }
67 #ifdef DEBUG_DYNAMIC_SHAPES
68  else {
69  assert(myAlphaSpan == nullptr);
70  }
71 #endif
72 
73  myOriginalShape = std::unique_ptr<PositionVector>(new PositionVector(p->getShape()));
74 
75  if (tracking) {
76  // Try initializing the tracked position (depends on whether object is already on the road)
79  }
80 
81  if (!alphaSpan.empty()) {
82  myAlphaSpan = std::unique_ptr<std::vector<double> >(new std::vector<double>(alphaSpan));
83  assert(myAlphaSpan->size() >= 2);
84  assert(myAlphaSpan->size() == myTimeSpan->size());
85  myPrevAlpha = myAlphaSpan->begin();
86  myNextAlpha = ++myAlphaSpan->begin();
87  }
88 }
89 
91 {}
92 
93 
96 #ifdef DEBUG_DYNAMIC_SHAPES
97  std::cout << t << " PolygonDynamics::update() for polygon '" << myPolygon->getID() << "'" << std::endl;
98 #endif
99  const double simtime = STEPS2TIME(t);
100  const double dt = simtime - myLastUpdateTime;
101  myLastUpdateTime = simtime;
102 
103  SUMOTime ret = DELTA_T;
104 
105  if (tracking) {
106  if (myTrackedObjectsInitialPositon == nullptr) {
107  // Tracked object hasn't entered the network, until now.
108  // Continuously try to obtain its initial position
110  }
111  if (myTrackedObjectsInitialPositon != nullptr) {
112  // Initial position was initialized, relative tracking is possible
113  const Position& objPos = myTrackedObject->getPosition();
114  const bool onRoad = objPos != Position::INVALID;
115  if (onRoad) {
116 #ifdef DEBUG_DYNAMIC_SHAPES
117  std::cout << " Tracked object '" << myTrackedObject->getID() << "' is on the road. Tracked position=" << objPos << std::endl;
118 #endif
119  // Update polygon's shape
120  PositionVector newShape(*myOriginalShape);
121  if (rotate) {
122  const double relRotation = myTrackedObject->getAngle() - myTrackedObjectsInitialAngle;
123  newShape.rotate2D(relRotation);
124 #ifdef DEBUG_DYNAMIC_SHAPES
125  std::cout << " Relative rotation wrt original rotation: " << relRotation << std::endl;
126 #endif
127  }
128  newShape.add(objPos);
129  myPolygon->setShape(newShape);
130  }
131 #ifdef DEBUG_DYNAMIC_SHAPES
132  else {
133  // tracked object is off road
134  std::cout << " Tracked object '" << myTrackedObject->getID() << "' is off road." << std::endl;
135  }
136 #endif
137  }
138 #ifdef DEBUG_DYNAMIC_SHAPES
139  else {
140  // Initial position was not initialized, yet
141  std::cout << " Tracked object '" << myTrackedObject->getID() << "' hasn't entered the network since tracking was started." << std::endl;
142  }
143 #endif
144  }
145 
146  if (animated) {
147  // Continue animation
148  myCurrentTime += dt;
149  while (myCurrentTime >= *myNextTime) {
150  // step forward along time lines to appropriate anchor points
151  ++myPrevTime;
152  ++myNextTime;
153  if (myNextTime == myTimeSpan->end()) {
154  // Set iterators back to point to valid positions
155  --myPrevTime;
156  --myNextTime;
157  break;
158  } else {
159  // Forward corresponding iterators for property time lines
160  if (myAlphaSpan != nullptr) {
161  ++myPrevAlpha;
162  ++myNextAlpha;
163  }
164  }
165  }
166 
167  // Linear interpolation factor between previous and next time
168  double theta = 1.0;
169 #ifdef DEBUG_DYNAMIC_SHAPES
170  std::cout << " animation: dt=" << dt
171  << ", current animation time: " << myCurrentTime
172  << ", previous anchor time: " << *myPrevTime
173  << ", next anchor time: " << *myNextTime;
174 #endif
175  if (looped) {
176  const bool resetAnimation = myCurrentTime >= *myNextTime;
177 #ifdef DEBUG_DYNAMIC_SHAPES
178  if (resetAnimation) {
179  std::cout << " (resetting animation!)";
180  }
181 #endif
182  if (resetAnimation) {
183  // Reset animation time line to start, if appropriate
184  while (myCurrentTime >= *myNextTime) {
186  }
188  myPrevTime = myTimeSpan->begin();
189  myNextTime = ++myTimeSpan->begin();
190  if (myAlphaSpan != nullptr) {
191  myPrevAlpha = myAlphaSpan->begin();
192  myNextAlpha = ++myAlphaSpan->begin();
193  }
194  }
195  }
196  if (myCurrentTime >= *myNextTime) {
197  assert(!looped);
198  // Reached the end of the dynamics, indicate expiration by returning zero
199  // and set all properties to the final state (theta remains one)
200  ret = 0;
201 #ifdef DEBUG_DYNAMIC_SHAPES
202  std::cout << " (animation elapsed!)";
203 #endif
204  } else {
205  // Animation is still going on, schedule next update
206  if (*myNextTime - *myPrevTime != 0) {
207  theta = (myCurrentTime - *myPrevTime) / (*myNextTime - *myPrevTime);
208  }
209  }
210  if (myAlphaSpan != nullptr) {
211  // Interpolate values of properties
212  setAlpha(*myPrevAlpha + theta * (*myNextAlpha - *myPrevAlpha));
213 #ifdef DEBUG_DYNAMIC_SHAPES
214  std::cout << ", previous anchor alpha: " << *myPrevAlpha
215  << ", next anchor alpha: " << *myNextAlpha;
216 #endif
217  }
218 #ifdef DEBUG_DYNAMIC_SHAPES
219  std::cout << ", theta=" << theta << std::endl;
220 #endif
221  }
222  return ret;
223 }
224 
225 void
227  const Position& objPos = myTrackedObject->getPosition();
228  if (objPos != Position::INVALID) {
229  // Initialize Position of tracked object
230  myTrackedObjectsInitialPositon = std::unique_ptr<Position>(new Position(objPos));
232  // Store original polygon shape relative to the tracked object's original position
234 #ifdef DEBUG_DYNAMIC_SHAPES
235  std::cout << " Tracking object '" << myTrackedObject->getID() << "' at initial positon: " << *myTrackedObjectsInitialPositon << std::endl;
236 #endif
237  }
238 }
239 
240 void
242  int a = (int) alpha;
243  myPolygon->setShapeAlpha((unsigned char) a);
244 #ifdef DEBUG_DYNAMIC_SHAPES
245  std::cout << "\n DynamicPolygon::setAlpha() Converted alpha=" << alpha << " into myAlpha=" << a << std::endl;
246 #endif
247 }
long long int SUMOTime
Definition: SUMOTime.h:35
std::unique_ptr< Position > myTrackedObjectsInitialPositon
Initial position of the tracked object.
virtual const std::string & getID() const =0
Get the vehicle&#39;s ID.
virtual ~PolygonDynamics()
void setShapeAlpha(unsigned char alpha)
Sets a new alpha value.
Definition: Shape.h:137
std::vector< double >::const_iterator myPrevTime
Pointer to the next time points in timeSpan.
bool rotate
Whether this polygon should be rotated with the tracked object.
T MAX2(T a, T b)
Definition: StdDefs.h:80
SUMOTime DELTA_T
Definition: SUMOTime.cpp:35
const PositionVector & getShape() const
Returns whether the shape of the polygon.
Definition: SUMOPolygon.h:82
void initTrackedPosition()
Initialize the object&#39;s position.
const std::string & getID() const
Returns the id.
Definition: Named.h:77
double myCurrentTime
Current time.
bool tracking
Whether this polygon tracks an object.
bool looped
Whether animation should be looped.
virtual double getAngle() const =0
Returns the objects angle in degrees.
std::unique_ptr< std::vector< double > > myTimeSpan
Time points corresponding to the anchor values of the dynamic properties.
virtual void setShape(const PositionVector &shape)
Sets the shape of the polygon.
Definition: SUMOPolygon.h:121
std::vector< double >::const_iterator myNextTime
A point in 2D or 3D with translation and scaling methods.
Definition: Position.h:39
A list of positions.
bool animated
Whether this polygon is animated, i.e., whether timelines should be used to control properties...
std::vector< double >::const_iterator myNextAlpha
#define STEPS2TIME(x)
Definition: SUMOTime.h:57
double myLastUpdateTime
The last time the animation has been updated.
SUMOTime update(SUMOTime t)
Updates the polygon according to its timeSpan and follows the tracked object.
void rotate2D(double angle)
double myTrackedObjectsInitialAngle
Initial angle of the tracked object.
Representation of a vehicle or person.
SUMOTrafficObject * myTrackedObject
An object tracked by the shape, deletion by caller.
std::string myTrackedObjectID
void setAlpha(double alpha)
Sets the alpha value for the shape&#39;s color.
std::unique_ptr< PositionVector > myOriginalShape
the original shape of the polygon (in case of tracking another object, this is converted to relative ...
std::vector< double >::const_iterator myPrevAlpha
Pointer to the next alpha points in alphaSpan.
void add(double xoff, double yoff, double zoff)
SUMOPolygon * myPolygon
The polygon this dynamics acts upon.
std::unique_ptr< std::vector< double > > myAlphaSpan
Alpha values corresponding to.
static const Position INVALID
used to indicate that a position is valid
Definition: Position.h:285
virtual Position getPosition(const double offset=0) const =0
Return current position (x/y, cartesian)
PolygonDynamics(double creationTime, SUMOPolygon *p, SUMOTrafficObject *trackedObject, const std::vector< double > &timeSpan, const std::vector< double > &alphaSpan, bool looped, bool rotate)
Constructor that takes a SUMOPolygon and adds timelines for the properties to be modified dynamically...