Marble

RouteSimulationPositionProviderPlugin.cpp
1 // SPDX-License-Identifier: LGPL-2.1-or-later
2 //
3 // SPDX-FileCopyrightText: 2011 Konrad Enzensberger <[email protected]>
4 // SPDX-FileCopyrightText: 2011 Dennis Nienhüser <[email protected]>
5 // SPDX-FileCopyrightText: 2012 Bernhard Beschow <[email protected]>
6 //
7 
8 #include "RouteSimulationPositionProviderPlugin.h"
9 
10 #include "MarbleModel.h"
11 #include "routing/Route.h"
12 #include "routing/RoutingManager.h"
13 #include "routing/RoutingModel.h"
14 #include "GeoDataAccuracy.h"
15 
16 #include <QIcon>
17 
18 namespace Marble
19 {
20 
21 namespace {
22  qreal const c_frequency = 4.0; // Hz
23 }
24 
25 QString RouteSimulationPositionProviderPlugin::name() const
26 {
27  return tr( "Current Route Position Provider Plugin" );
28 }
29 
30 QString RouteSimulationPositionProviderPlugin::nameId() const
31 {
32  return QStringLiteral("RouteSimulationPositionProviderPlugin");
33 }
34 
35 QString RouteSimulationPositionProviderPlugin::guiString() const
36 {
37  return tr( "Current Route" );
38 }
39 
40 QString RouteSimulationPositionProviderPlugin::version() const
41 {
42  return QStringLiteral("1.1");
43 }
44 
45 QString RouteSimulationPositionProviderPlugin::description() const
46 {
47  return tr( "Simulates traveling along the current route." );
48 }
49 
50 QString RouteSimulationPositionProviderPlugin::copyrightYears() const
51 {
52  return QStringLiteral("2011, 2012");
53 }
54 
55 QVector<PluginAuthor> RouteSimulationPositionProviderPlugin::pluginAuthors() const
56 {
57  return QVector<PluginAuthor>()
58  << PluginAuthor(QStringLiteral("Konrad Enzensberger"), QStringLiteral("[email protected]"))
59  << PluginAuthor(QStringLiteral("Dennis Nienhüser"), QStringLiteral("[email protected]"))
60  << PluginAuthor(QStringLiteral("Bernhard Beschow"), QStringLiteral("[email protected]"));
61 }
62 
63 QIcon RouteSimulationPositionProviderPlugin::icon() const
64 {
65  return QIcon();
66 }
67 
68 PositionProviderPlugin* RouteSimulationPositionProviderPlugin::newInstance() const
69 {
70  return new RouteSimulationPositionProviderPlugin( m_marbleModel );
71 }
72 
73 PositionProviderStatus RouteSimulationPositionProviderPlugin::status() const
74 {
75  return m_status;
76 }
77 
78 GeoDataCoordinates RouteSimulationPositionProviderPlugin::position() const
79 {
80  return m_currentPositionWithNoise;
81 }
82 
83 GeoDataAccuracy RouteSimulationPositionProviderPlugin::accuracy() const
84 {
85  GeoDataAccuracy result;
86 
87  // faked values
88  result.level = GeoDataAccuracy::Detailed;
89  result.horizontal = 10.0;
90  result.vertical = 10.0;
91 
92  return result;
93 }
94 
95 RouteSimulationPositionProviderPlugin::RouteSimulationPositionProviderPlugin(MarbleModel *marbleModel , QObject *parent) :
96  PositionProviderPlugin(parent),
97  m_marbleModel( marbleModel ),
98  m_currentIndex( -2 ),
99  m_status( PositionProviderStatusUnavailable ),
100  m_currentDateTime(),
101  m_speed( 0.0 ),
102  m_direction( 0.0 ),
103  m_directionWithNoise(0.0)
104 {
105  connect(&m_updateTimer, SIGNAL(timeout()), this, SLOT(update()));
106 }
107 
108 RouteSimulationPositionProviderPlugin::~RouteSimulationPositionProviderPlugin()
109 {
110 }
111 
112 void RouteSimulationPositionProviderPlugin::initialize()
113 {
114  updateRoute();
115  connect(m_marbleModel->routingManager()->routingModel(), SIGNAL(currentRouteChanged()), this, SLOT(updateRoute()));
116 }
117 
118 bool RouteSimulationPositionProviderPlugin::isInitialized() const
119 {
120  return ( m_currentIndex > -2 );
121 }
122 
123 qreal RouteSimulationPositionProviderPlugin::speed() const
124 {
125  return m_speed;
126 }
127 
128 qreal RouteSimulationPositionProviderPlugin::direction() const
129 {
130  return m_directionWithNoise;
131 }
132 
133 QDateTime RouteSimulationPositionProviderPlugin::timestamp() const
134 {
135  return m_currentDateTime;
136 }
137 
138 void RouteSimulationPositionProviderPlugin::updateRoute(){
139  m_currentIndex = -1;
140  m_lineString = m_lineStringInterpolated = m_marbleModel->routingManager()->routingModel()->route().path();
141  m_speed = 0; //initialize speed to be around 25 m/s;
142  bool const canWork = !m_lineString.isEmpty() || m_currentPosition.isValid();
143  if (canWork) {
144  changeStatus(PositionProviderStatusAcquiring);
145  m_updateTimer.start(1000.0 / c_frequency);
146  } else {
147  changeStatus(PositionProviderStatusUnavailable);
148  m_updateTimer.stop();
149  }
150 }
151 
152 void RouteSimulationPositionProviderPlugin::update()
153 {
154  if (m_lineString.isEmpty() && m_currentPosition.isValid()) {
155  m_currentPositionWithNoise = addNoise(m_currentPosition, accuracy());
156  changeStatus(PositionProviderStatusAvailable);
157  emit positionChanged(position(), accuracy());
158  return;
159  }
160 
161  if ( m_currentIndex >= 0 && m_currentIndex < m_lineStringInterpolated.size() ) {
162  changeStatus(PositionProviderStatusAvailable);
163  GeoDataCoordinates newPosition = m_lineStringInterpolated.at( m_currentIndex );
164  const QDateTime newDateTime = QDateTime::currentDateTime();
165  qreal time= m_currentDateTime.msecsTo(newDateTime)/1000.0;
166  if ( m_currentPosition.isValid() ) {
167  //speed calculations
168  //Max speed is set on points (m_lineStringInterpolated) based on formula. (max speed before points is calculated so the acceleration won't be exceeded)
169  const qreal acceleration = 1.5;
170  const qreal lookForwardDistance = 1000;
171  qreal checkedDistance = m_currentPosition.sphericalDistanceTo(m_lineStringInterpolated.at(m_currentIndex))* m_marbleModel->planetRadius();
172  const qreal maxSpeed = 25;
173  const qreal minSpeed = 2;
174  qreal newSpeed = qMin((m_speed + acceleration*time), maxSpeed);
175  for (int i=qMax(1,m_currentIndex); i<m_lineStringInterpolated.size()-1 && checkedDistance<lookForwardDistance; ++i)
176  {
177  qreal previousHeading = m_lineStringInterpolated.at( i-1 ).bearing( m_lineStringInterpolated.at( i ), GeoDataCoordinates::Degree, GeoDataCoordinates::FinalBearing );
178  qreal curveLength = 10;//we treat one point turn as a curve of length 10
179  qreal angleSum = 0;//sum of turn angles in a curve
180  for (int j=i+1; j<m_lineStringInterpolated.size() && curveLength<35; ++j)
181  {
182  qreal newHeading = m_lineStringInterpolated.at( j-1 ).bearing( m_lineStringInterpolated.at( j ), GeoDataCoordinates::Degree, GeoDataCoordinates::FinalBearing );
183  qreal differenceHeading = qAbs(previousHeading-newHeading);//angle of turn
184  if(differenceHeading>180) {
185  differenceHeading = 360 - differenceHeading;
186  }
187  angleSum +=differenceHeading;
188  qreal maxSpeedAtTurn = qMax((1 - (static_cast<qreal>(angleSum/60.0/curveLength*10.0))*maxSpeed), minSpeed);//speed limit at turn
189  if( checkedDistance<25 && maxSpeedAtTurn<newSpeed )//if we are near turn don't accelerate, if we will have to slow down
190  newSpeed = qMin(newSpeed, qMax(m_speed,maxSpeedAtTurn));
191  // formulas:
192  // s = Vc * t + a*t*t/2
193  // V0 = Vc + a*t
194  // V0 = maxCurrentSpeed
195  // Vc = maxSpeedAtTurn
196  // s = checkedDistance
197  // a = acceleration
198  qreal delta = maxSpeedAtTurn*maxSpeedAtTurn - 4.0*acceleration/2.0*(-checkedDistance);//delta = b*b-4*a*c
199  qreal t = (-maxSpeedAtTurn+sqrt(delta))/(2.0*acceleration/2.0);//(-b+sqrt(delta))/(2*c)
200  qreal maxCurrentSpeed = maxSpeedAtTurn + acceleration*t;
201  newSpeed = qMin(newSpeed, maxCurrentSpeed);
202  previousHeading = newHeading;
203  curveLength += m_lineStringInterpolated.at(j - 1).sphericalDistanceTo(m_lineStringInterpolated.at(j)) * m_marbleModel->planetRadius();
204  }
205  checkedDistance += m_lineStringInterpolated.at(i).sphericalDistanceTo(m_lineStringInterpolated.at(i + 1)) * m_marbleModel->planetRadius();
206  }
207  m_speed=newSpeed;
208 
209  //Assume the car's moving at m_speed m/s. The distance moved will be speed*time which is equal to the speed of the car if time is equal to one.
210  //If the function isn't called once exactly after a second, multiplying by the time will compensate for the error and maintain the speed.
211  qreal fraction = m_speed*time/(m_currentPosition.sphericalDistanceTo(newPosition) * m_marbleModel->planetRadius());
212 
213  //Interpolate and find the next point to move to if needed.
214  if(fraction>0 && fraction <1){
215  GeoDataCoordinates newPoint = m_currentPosition.interpolate(newPosition,fraction);
216  newPosition=newPoint;
217  }
218  else if ( fraction > 1 ) {
219  bool isCurrentIndexValid = true;
220  while ( fraction > 1 ) {
221  ++m_currentIndex;
222  if ( m_currentIndex >= m_lineStringInterpolated.size() ) {
223  isCurrentIndexValid = false;
224  break;
225  }
226 
227  newPosition = m_lineStringInterpolated.at( m_currentIndex );
228  fraction = m_speed*time / (m_currentPosition.sphericalDistanceTo(newPosition) * m_marbleModel->planetRadius());
229  }
230 
231  if ( isCurrentIndexValid ) {
232  GeoDataCoordinates newPoint = m_currentPosition.interpolate( newPosition, fraction );
233  newPosition = newPoint;
234  }
235  }
236  else
237  {
238  m_currentIndex++;
239  }
240 
241  m_direction = m_currentPosition.bearing( newPosition, GeoDataCoordinates::Degree, GeoDataCoordinates::FinalBearing );
242  m_directionWithNoise = addNoise(m_direction);
243  }
244  m_currentPosition = newPosition;
245  m_currentPositionWithNoise = addNoise(m_currentPosition, accuracy());
246  m_currentDateTime = newDateTime;
247  emit positionChanged( position(), accuracy() );
248  }
249  else {
250  // Repeat from start
251  m_currentIndex = 0;
252  m_lineStringInterpolated = m_lineString;
253  m_currentPosition = GeoDataCoordinates(); //Reset the current position so that the simulation starts from the correct starting point.
254  m_currentPositionWithNoise = GeoDataCoordinates();
255  m_speed = 0;
256  changeStatus(PositionProviderStatusUnavailable);
257  }
258 }
259 
260 GeoDataCoordinates RouteSimulationPositionProviderPlugin::addNoise(const Marble::GeoDataCoordinates &position, const Marble::GeoDataAccuracy &accuracy ) const
261 {
262  qreal randomBearing = static_cast<qreal>(qrand()) / (static_cast<qreal>(RAND_MAX/M_PI));
263  qreal randomDistance = static_cast<qreal>(qrand()) / (static_cast<qreal>(RAND_MAX/(accuracy.horizontal / 2.0 / m_marbleModel->planetRadius())));
264  return position.moveByBearing(randomBearing, randomDistance);
265 }
266 
267 qreal RouteSimulationPositionProviderPlugin::addNoise(qreal bearing)
268 {
269  qreal const maxBearingError = 30.0;
270  return bearing + static_cast<qreal>(qrand()) / (static_cast<qreal>(RAND_MAX/maxBearingError/2.0)) - maxBearingError / 2.0;
271 }
272 
273 void RouteSimulationPositionProviderPlugin::changeStatus(PositionProviderStatus status)
274 {
275  if (m_status != status) {
276  m_status = status;
277  emit statusChanged(m_status);
278  }
279 }
280 
281 } // namespace Marble
282 
283 #include "moc_RouteSimulationPositionProviderPlugin.cpp"
A 3d point representation.
QDateTime currentDateTime()
QMetaObject::Connection connect(const QObject *sender, const char *signal, const QObject *receiver, const char *method, Qt::ConnectionType type)
Q_SCRIPTABLE CaptureState status()
Binds a QML item to a specific geodetic location in screen coordinates.
GeoDataCoordinates moveByBearing(qreal bearing, qreal distance) const
Returns the coordinates of the resulting point after moving this point according to the distance and ...
void update(Part *part, const QByteArray &data, qint64 dataSize)
bool isValid() const const
This file is part of the KDE documentation.
Documentation copyright © 1996-2023 The KDE developers.
Generated on Mon Sep 25 2023 03:50:20 by doxygen 1.8.17 written by Dimitri van Heesch, © 1997-2006

KDE's Doxygen guidelines are available online.