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