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 }
PolygonDynamics::myAlphaSpan
std::unique_ptr< std::vector< double > > myAlphaSpan
Alpha values corresponding to.
Definition: PolygonDynamics.h:132
SUMOTrafficObject
Representation of a vehicle or person.
Definition: SUMOTrafficObject.h:48
SUMOTrafficObject::getPosition
virtual Position getPosition(const double offset=0) const =0
Return current position (x/y, cartesian)
PolygonDynamics.h
PolygonDynamics::~PolygonDynamics
virtual ~PolygonDynamics()
Definition: PolygonDynamics.cpp:90
SUMOTime.h
DELTA_T
SUMOTime DELTA_T
Definition: SUMOTime.cpp:35
Position::INVALID
static const Position INVALID
used to indicate that a position is valid
Definition: Position.h:285
PositionVector::rotate2D
void rotate2D(double angle)
Definition: PositionVector.cpp:1460
SUMOTrafficObject::getID
virtual const std::string & getID() const =0
Get the vehicle's ID.
SUMOPolygon::setShape
virtual void setShape(const PositionVector &shape)
Sets the shape of the polygon.
Definition: SUMOPolygon.h:121
SUMOTime
long long int SUMOTime
Definition: SUMOTime.h:35
PolygonDynamics::initTrackedPosition
void initTrackedPosition()
Initialize the object's position.
Definition: PolygonDynamics.cpp:226
PolygonDynamics::myCurrentTime
double myCurrentTime
Current time.
Definition: PolygonDynamics.h:89
PositionVector
A list of positions.
Definition: PositionVector.h:46
Shape::setShapeAlpha
void setShapeAlpha(unsigned char alpha)
Sets a new alpha value.
Definition: Shape.h:137
PolygonDynamics::myPrevTime
std::vector< double >::const_iterator myPrevTime
Pointer to the next time points in timeSpan.
Definition: PolygonDynamics.h:128
MAX2
T MAX2(T a, T b)
Definition: StdDefs.h:80
PositionVector::add
void add(double xoff, double yoff, double zoff)
Definition: PositionVector.cpp:609
PolygonDynamics::rotate
bool rotate
Whether this polygon should be rotated with the tracked object.
Definition: PolygonDynamics.h:105
PolygonDynamics::myTimeSpan
std::unique_ptr< std::vector< double > > myTimeSpan
Time points corresponding to the anchor values of the dynamic properties.
Definition: PolygonDynamics.h:124
PolygonDynamics::myNextAlpha
std::vector< double >::const_iterator myNextAlpha
Definition: PolygonDynamics.h:136
SUMOTrafficObject::getAngle
virtual double getAngle() const =0
Returns the objects angle in degrees.
STEPS2TIME
#define STEPS2TIME(x)
Definition: SUMOTime.h:57
PolygonDynamics::tracking
bool tracking
Whether this polygon tracks an object.
Definition: PolygonDynamics.h:102
PolygonDynamics::looped
bool looped
Whether animation should be looped.
Definition: PolygonDynamics.h:99
PolygonDynamics::myNextTime
std::vector< double >::const_iterator myNextTime
Definition: PolygonDynamics.h:129
Position
A point in 2D or 3D with translation and scaling methods.
Definition: Position.h:39
PolygonDynamics::myLastUpdateTime
double myLastUpdateTime
The last time the animation has been updated.
Definition: PolygonDynamics.h:92
PolygonDynamics::myTrackedObjectsInitialAngle
double myTrackedObjectsInitialAngle
Initial angle of the tracked object.
Definition: PolygonDynamics.h:115
PolygonDynamics::animated
bool animated
Whether this polygon is animated, i.e., whether timelines should be used to control properties.
Definition: PolygonDynamics.h:96
PolygonDynamics::myTrackedObjectID
std::string myTrackedObjectID
Definition: PolygonDynamics.h:109
SUMOPolygon::getShape
const PositionVector & getShape() const
Returns whether the shape of the polygon.
Definition: SUMOPolygon.h:82
SUMOPolygon
Definition: SUMOPolygon.h:47
PolygonDynamics::myTrackedObject
SUMOTrafficObject * myTrackedObject
An object tracked by the shape, deletion by caller.
Definition: PolygonDynamics.h:108
PolygonDynamics::myPolygon
SUMOPolygon * myPolygon
The polygon this dynamics acts upon.
Definition: PolygonDynamics.h:86
PolygonDynamics::myOriginalShape
std::unique_ptr< PositionVector > myOriginalShape
the original shape of the polygon (in case of tracking another object, this is converted to relative ...
Definition: PolygonDynamics.h:120
PolygonDynamics::update
SUMOTime update(SUMOTime t)
Updates the polygon according to its timeSpan and follows the tracked object.
Definition: PolygonDynamics.cpp:95
StdDefs.h
SUMOTrafficObject.h
PolygonDynamics::myPrevAlpha
std::vector< double >::const_iterator myPrevAlpha
Pointer to the next alpha points in alphaSpan.
Definition: PolygonDynamics.h:135
Named::getID
const std::string & getID() const
Returns the id.
Definition: Named.h:77
PolygonDynamics::setAlpha
void setAlpha(double alpha)
Sets the alpha value for the shape's color.
Definition: PolygonDynamics.cpp:241
PolygonDynamics::PolygonDynamics
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...
Definition: PolygonDynamics.cpp:29
PolygonDynamics::myTrackedObjectsInitialPositon
std::unique_ptr< Position > myTrackedObjectsInitialPositon
Initial position of the tracked object.
Definition: PolygonDynamics.h:112