Line data Source code
1 : /****************************************************************************/
2 : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3 : // Copyright (C) 2004-2024 German Aerospace Center (DLR) and others.
4 : // This program and the accompanying materials are made available under the
5 : // terms of the Eclipse Public License 2.0 which is available at
6 : // https://www.eclipse.org/legal/epl-2.0/
7 : // This Source Code may also be made available under the following Secondary
8 : // Licenses when the conditions for such availability set forth in the Eclipse
9 : // Public License 2.0 are satisfied: GNU General Public License, version 2
10 : // or later which is available at
11 : // https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
12 : // SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
13 : /****************************************************************************/
14 : /// @file PolygonDynamics.cpp
15 : /// @author Leonhard Luecken
16 : /// @date Apr 2019
17 : ///
18 : // A polygon, which holds a timeSpan for displaying dynamic properties
19 : /****************************************************************************/
20 :
21 :
22 : #include "PolygonDynamics.h"
23 :
24 : #include <assert.h>
25 : #include "utils/common/StdDefs.h"
26 : #include "utils/common/SUMOTime.h"
27 : #include "utils/vehicle/SUMOTrafficObject.h"
28 :
29 :
30 : //#define DEBUG_DYNAMIC_SHAPES
31 :
32 156 : PolygonDynamics::PolygonDynamics(double creationTime,
33 : SUMOPolygon* p,
34 : SUMOTrafficObject* trackedObject,
35 : const std::vector<double>& timeSpan,
36 : const std::vector<double>& alphaSpan,
37 : bool looped,
38 156 : bool rotate) :
39 156 : myPolygon(p),
40 156 : myCurrentTime(0),
41 156 : myLastUpdateTime(creationTime),
42 156 : animated(!timeSpan.empty()),
43 156 : looped(looped),
44 156 : tracking(trackedObject != nullptr),
45 156 : rotate(rotate),
46 156 : myTrackedObject(trackedObject),
47 156 : myTrackedObjectID(""),
48 : myTrackedObjectsInitialPositon(nullptr),
49 156 : myTrackedObjectsInitialAngle(-1),
50 : myOriginalShape(nullptr),
51 : myTimeSpan(nullptr),
52 : myAlphaSpan(nullptr),
53 156 : myVis(nullptr) {
54 : // Check for consistency
55 156 : if (animated) {
56 218 : myTimeSpan = std::unique_ptr<std::vector<double> >(new std::vector<double>(timeSpan));
57 : assert(myTimeSpan->size() >= 2);
58 : assert((*myTimeSpan)[0] == 0.0);
59 : assert(myAlphaSpan == nullptr || myAlphaSpan->size() >= 2);
60 : #ifdef DEBUG_DYNAMIC_SHAPES
61 : if (myTimeSpan->size() >= 2) {
62 : for (unsigned int i = 1; i < myTimeSpan->size(); ++i) {
63 : assert((*myTimeSpan)[i - 1] <= (*myTimeSpan)[i]);
64 : }
65 : }
66 : #endif
67 109 : myPrevTime = myTimeSpan->begin();
68 109 : myNextTime = ++myTimeSpan->begin();
69 : }
70 : #ifdef DEBUG_DYNAMIC_SHAPES
71 : else {
72 : assert(myAlphaSpan == nullptr);
73 : }
74 : #endif
75 :
76 156 : myOriginalShape = std::unique_ptr<PositionVector>(new PositionVector(p->getShape()));
77 :
78 156 : if (tracking) {
79 : // Try initializing the tracked position (depends on whether object is already on the road)
80 111 : initTrackedPosition();
81 111 : myTrackedObjectID = myTrackedObject->getID();
82 : }
83 :
84 156 : if (!alphaSpan.empty()) {
85 186 : myAlphaSpan = std::unique_ptr<std::vector<double> >(new std::vector<double>(alphaSpan));
86 : assert(myAlphaSpan->size() >= 2);
87 : assert(myAlphaSpan->size() == myTimeSpan->size());
88 93 : myPrevAlpha = myAlphaSpan->begin();
89 93 : myNextAlpha = ++myAlphaSpan->begin();
90 : }
91 156 : }
92 :
93 312 : PolygonDynamics::~PolygonDynamics()
94 468 : {}
95 :
96 :
97 : SUMOTime
98 3434 : PolygonDynamics::update(SUMOTime t) {
99 : #ifdef DEBUG_DYNAMIC_SHAPES
100 : std::cout << t << " PolygonDynamics::update() for polygon '" << myPolygon->getID() << "'" << std::endl;
101 : #endif
102 3434 : const double simtime = STEPS2TIME(t);
103 3434 : const double dt = simtime - myLastUpdateTime;
104 3434 : myLastUpdateTime = simtime;
105 :
106 3434 : SUMOTime ret = DELTA_T;
107 :
108 3434 : if (tracking) {
109 3249 : if (myTrackedObjectsInitialPositon == nullptr) {
110 : // Tracked object hasn't entered the network, until now.
111 : // Continuously try to obtain its initial position
112 8 : initTrackedPosition();
113 : }
114 3249 : if (myTrackedObjectsInitialPositon != nullptr) {
115 : // Initial position was initialized, relative tracking is possible
116 3249 : const Position& objPos = myTrackedObject->getPosition();
117 : const bool onRoad = objPos != Position::INVALID;
118 : if (onRoad) {
119 : #ifdef DEBUG_DYNAMIC_SHAPES
120 : std::cout << " Tracked object '" << myTrackedObject->getID() << "' is on the road. Tracked position=" << objPos << std::endl;
121 : #endif
122 : // Update polygon's shape
123 : PositionVector newShape(*myOriginalShape);
124 3249 : if (rotate) {
125 3249 : const double relRotation = myTrackedObject->getAngle() - myTrackedObjectsInitialAngle;
126 3249 : newShape.rotate2D(relRotation);
127 : #ifdef DEBUG_DYNAMIC_SHAPES
128 : std::cout << " Relative rotation wrt original rotation: " << relRotation << std::endl;
129 : #endif
130 : }
131 3249 : newShape.add(objPos);
132 3249 : myPolygon->setShape(newShape);
133 3249 : }
134 : #ifdef DEBUG_DYNAMIC_SHAPES
135 : else {
136 : // tracked object is off road
137 : std::cout << " Tracked object '" << myTrackedObject->getID() << "' is off road." << std::endl;
138 : }
139 : #endif
140 : }
141 : #ifdef DEBUG_DYNAMIC_SHAPES
142 : else {
143 : // Initial position was not initialized, yet
144 : std::cout << " Tracked object '" << myTrackedObject->getID() << "' hasn't entered the network since tracking was started." << std::endl;
145 : }
146 : #endif
147 : }
148 :
149 3434 : if (animated) {
150 : // Continue animation
151 921 : myCurrentTime += dt;
152 2196 : while (myCurrentTime >= *myNextTime) {
153 : // step forward along time lines to appropriate anchor points
154 : ++myPrevTime;
155 : ++myNextTime;
156 455 : if (myNextTime == myTimeSpan->end()) {
157 : // Set iterators back to point to valid positions
158 : --myPrevTime;
159 : --myNextTime;
160 : break;
161 : } else {
162 : // Forward corresponding iterators for property time lines
163 354 : if (myAlphaSpan != nullptr) {
164 : ++myPrevAlpha;
165 : ++myNextAlpha;
166 : }
167 : }
168 : }
169 :
170 : // Linear interpolation factor between previous and next time
171 : double theta = 1.0;
172 : #ifdef DEBUG_DYNAMIC_SHAPES
173 : std::cout << " animation: dt=" << dt
174 : << ", current animation time: " << myCurrentTime
175 : << ", previous anchor time: " << *myPrevTime
176 : << ", next anchor time: " << *myNextTime;
177 : #endif
178 921 : if (looped) {
179 80 : const bool resetAnimation = myCurrentTime >= *myNextTime;
180 : #ifdef DEBUG_DYNAMIC_SHAPES
181 : if (resetAnimation) {
182 : std::cout << " (resetting animation!)";
183 : }
184 : #endif
185 80 : if (resetAnimation) {
186 : // Reset animation time line to start, if appropriate
187 32 : while (myCurrentTime >= *myNextTime) {
188 16 : myCurrentTime -= *myNextTime;
189 : }
190 16 : myCurrentTime = MAX2(myCurrentTime, 0.0);
191 16 : myPrevTime = myTimeSpan->begin();
192 16 : myNextTime = ++myTimeSpan->begin();
193 16 : if (myAlphaSpan != nullptr) {
194 16 : myPrevAlpha = myAlphaSpan->begin();
195 16 : myNextAlpha = ++myAlphaSpan->begin();
196 : }
197 : }
198 : }
199 921 : if (myCurrentTime >= *myNextTime) {
200 : assert(!looped);
201 : // Reached the end of the dynamics, indicate expiration by returning zero
202 : // and set all properties to the final state (theta remains one)
203 : ret = 0;
204 : #ifdef DEBUG_DYNAMIC_SHAPES
205 : std::cout << " (animation elapsed!)";
206 : #endif
207 : } else {
208 : // Animation is still going on, schedule next update
209 836 : if (*myNextTime - *myPrevTime != 0) {
210 836 : theta = (myCurrentTime - *myPrevTime) / (*myNextTime - *myPrevTime);
211 : }
212 : }
213 921 : if (myAlphaSpan != nullptr) {
214 : // Interpolate values of properties
215 801 : setAlpha(*myPrevAlpha + theta * (*myNextAlpha - *myPrevAlpha));
216 : #ifdef DEBUG_DYNAMIC_SHAPES
217 : std::cout << ", previous anchor alpha: " << *myPrevAlpha
218 : << ", next anchor alpha: " << *myNextAlpha;
219 : #endif
220 : }
221 : #ifdef DEBUG_DYNAMIC_SHAPES
222 : std::cout << ", theta=" << theta << std::endl;
223 : #endif
224 : }
225 3434 : return ret;
226 : }
227 :
228 : void
229 119 : PolygonDynamics::initTrackedPosition() {
230 119 : const Position& objPos = myTrackedObject->getPosition();
231 : if (objPos != Position::INVALID) {
232 : // Initialize Position of tracked object
233 111 : myTrackedObjectsInitialPositon = std::unique_ptr<Position>(new Position(objPos));
234 111 : myTrackedObjectsInitialAngle = myTrackedObject->getAngle();
235 : // Store original polygon shape relative to the tracked object's original position
236 111 : myOriginalShape->sub(*myTrackedObjectsInitialPositon);
237 : #ifdef DEBUG_DYNAMIC_SHAPES
238 : std::cout << " Tracking object '" << myTrackedObject->getID() << "' at initial positon: " << *myTrackedObjectsInitialPositon << std::endl;
239 : #endif
240 : }
241 119 : }
242 :
243 : void
244 801 : PolygonDynamics::setAlpha(double alpha) {
245 801 : int a = (int) alpha;
246 801 : myPolygon->setShapeAlpha((unsigned char) a);
247 : #ifdef DEBUG_DYNAMIC_SHAPES
248 : std::cout << "\n DynamicPolygon::setAlpha() Converted alpha=" << alpha << " into myAlpha=" << a << std::endl;
249 : #endif
250 801 : }
|
