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         196 : 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         196 :                                  bool rotate) :
      39         196 :     myPolygon(p),
      40         196 :     myCurrentTime(0),
      41         196 :     myLastUpdateTime(creationTime),
      42         196 :     animated(!timeSpan.empty()),
      43         196 :     looped(looped),
      44         196 :     tracking(trackedObject != nullptr),
      45         196 :     rotate(rotate),
      46         196 :     myTrackedObject(trackedObject),
      47         196 :     myTrackedObjectID(""),
      48             :     myTrackedObjectsInitialPositon(nullptr),
      49         196 :     myTrackedObjectsInitialAngle(-1),
      50             :     myOriginalShape(nullptr),
      51             :     myTimeSpan(nullptr),
      52             :     myAlphaSpan(nullptr),
      53         196 :     myVis(nullptr) {
      54             :     // Check for consistency
      55         196 :     if (animated) {
      56         272 :         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         136 :         myPrevTime = myTimeSpan->begin();
      68         136 :         myNextTime = ++myTimeSpan->begin();
      69             :     }
      70             : #ifdef DEBUG_DYNAMIC_SHAPES
      71             :     else  {
      72             :         assert(myAlphaSpan == nullptr);
      73             :     }
      74             : #endif
      75             : 
      76         196 :     myOriginalShape = std::unique_ptr<PositionVector>(new PositionVector(p->getShape()));
      77             : 
      78         196 :     if (tracking) {
      79             :         // Try initializing the tracked position (depends on whether object is already on the road)
      80         140 :         initTrackedPosition();
      81         140 :         myTrackedObjectID = myTrackedObject->getID();
      82             :     }
      83             : 
      84         196 :     if (!alphaSpan.empty()) {
      85         232 :         myAlphaSpan = std::unique_ptr<std::vector<double> >(new std::vector<double>(alphaSpan));
      86             :         assert(myAlphaSpan->size() >= 2);
      87             :         assert(myAlphaSpan->size() == myTimeSpan->size());
      88         116 :         myPrevAlpha = myAlphaSpan->begin();
      89         116 :         myNextAlpha = ++myAlphaSpan->begin();
      90             :     }
      91         196 : }
      92             : 
      93         392 : PolygonDynamics::~PolygonDynamics()
      94         392 : {}
      95             : 
      96             : 
      97             : SUMOTime
      98        4386 : PolygonDynamics::update(SUMOTime t) {
      99             : #ifdef DEBUG_DYNAMIC_SHAPES
     100             :     std::cout << t << " PolygonDynamics::update() for polygon '" << myPolygon->getID() << "'" << std::endl;
     101             : #endif
     102        4386 :     const double simtime = STEPS2TIME(t);
     103        4386 :     const double dt = simtime - myLastUpdateTime;
     104        4386 :     myLastUpdateTime = simtime;
     105             : 
     106        4386 :     SUMOTime ret = DELTA_T;
     107             : 
     108        4386 :     if (tracking) {
     109        4156 :         if (myTrackedObjectsInitialPositon == nullptr) {
     110             :             // Tracked object hasn't entered the network, until now.
     111             :             // Continuously try to obtain its initial position
     112          10 :             initTrackedPosition();
     113             :         }
     114        4156 :         if (myTrackedObjectsInitialPositon != nullptr) {
     115             :             // Initial position was initialized, relative tracking is possible
     116        4156 :             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        4156 :                 if (rotate) {
     125        4156 :                     const double relRotation = myTrackedObject->getAngle() - myTrackedObjectsInitialAngle;
     126        4156 :                     newShape.rotate2D(relRotation);
     127             : #ifdef DEBUG_DYNAMIC_SHAPES
     128             :                     std::cout << " Relative rotation wrt original rotation: " << relRotation << std::endl;
     129             : #endif
     130             :                 }
     131        4156 :                 newShape.add(objPos);
     132        4156 :                 myPolygon->setShape(newShape);
     133        4156 :             }
     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        4386 :     if (animated) {
     150             :         // Continue animation
     151        1150 :         myCurrentTime += dt;
     152        2742 :         while (myCurrentTime >= *myNextTime) {
     153             :             // step forward along time lines to appropriate anchor points
     154             :             ++myPrevTime;
     155             :             ++myNextTime;
     156         568 :             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         442 :                 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        1150 :         if (looped) {
     179         100 :             const bool resetAnimation = myCurrentTime >= *myNextTime;
     180             : #ifdef DEBUG_DYNAMIC_SHAPES
     181             :             if (resetAnimation) {
     182             :                 std::cout << " (resetting animation!)";
     183             :             }
     184             : #endif
     185         100 :             if (resetAnimation) {
     186             :                 // Reset animation time line to start, if appropriate
     187          40 :                 while (myCurrentTime >= *myNextTime) {
     188          20 :                     myCurrentTime -= *myNextTime;
     189             :                 }
     190          20 :                 myCurrentTime = MAX2(myCurrentTime, 0.0);
     191          20 :                 myPrevTime = myTimeSpan->begin();
     192          20 :                 myNextTime = ++myTimeSpan->begin();
     193          20 :                 if (myAlphaSpan != nullptr) {
     194          20 :                     myPrevAlpha = myAlphaSpan->begin();
     195          20 :                     myNextAlpha = ++myAlphaSpan->begin();
     196             :                 }
     197             :             }
     198             :         }
     199        1150 :         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        1044 :             if (*myNextTime - *myPrevTime != 0) {
     210        1044 :                 theta = (myCurrentTime - *myPrevTime) / (*myNextTime - *myPrevTime);
     211             :             }
     212             :         }
     213        1150 :         if (myAlphaSpan != nullptr) {
     214             :             // Interpolate values of properties
     215        1000 :             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        4386 :     return ret;
     226             : }
     227             : 
     228             : void
     229         150 : PolygonDynamics::initTrackedPosition() {
     230         150 :     const Position& objPos = myTrackedObject->getPosition();
     231             :     if (objPos != Position::INVALID) {
     232             :         // Initialize Position of tracked object
     233         140 :         myTrackedObjectsInitialPositon = std::unique_ptr<Position>(new Position(objPos));
     234         140 :         myTrackedObjectsInitialAngle = myTrackedObject->getAngle();
     235             :         // Store original polygon shape relative to the tracked object's original position
     236         140 :         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         150 : }
     242             : 
     243             : void
     244        1000 : PolygonDynamics::setAlpha(double alpha) {
     245        1000 :     int a = (int) alpha;
     246        1000 :     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        1000 : }