Marble

RouteSimulationPositionProviderPlugin.cpp
1// SPDX-License-Identifier: LGPL-2.1-or-later
2//
3// SPDX-FileCopyrightText: 2011 Konrad Enzensberger <e.konrad@mpegcode.com>
4// SPDX-FileCopyrightText: 2011 Dennis Nienhüser <nienhueser@kde.org>
5// SPDX-FileCopyrightText: 2012 Bernhard Beschow <bbeschow@cs.tu-berlin.de>
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
18namespace Marble
19{
20
21namespace {
22 qreal const c_frequency = 4.0; // Hz
23}
24
25QString RouteSimulationPositionProviderPlugin::name() const
26{
27 return tr( "Current Route Position Provider Plugin" );
28}
29
30QString RouteSimulationPositionProviderPlugin::nameId() const
31{
32 return QStringLiteral("RouteSimulationPositionProviderPlugin");
33}
34
35QString RouteSimulationPositionProviderPlugin::guiString() const
36{
37 return tr( "Current Route" );
38}
39
40QString RouteSimulationPositionProviderPlugin::version() const
41{
42 return QStringLiteral("1.1");
43}
44
45QString RouteSimulationPositionProviderPlugin::description() const
46{
47 return tr( "Simulates traveling along the current route." );
48}
49
50QString RouteSimulationPositionProviderPlugin::copyrightYears() const
51{
52 return QStringLiteral("2011, 2012");
53}
54
55QVector<PluginAuthor> RouteSimulationPositionProviderPlugin::pluginAuthors() const
56{
58 << PluginAuthor(QStringLiteral("Konrad Enzensberger"), QStringLiteral("e.konrad@mpegcode.com"))
59 << PluginAuthor(QStringLiteral("Dennis Nienhüser"), QStringLiteral("nienhueser@kde.org"))
60 << PluginAuthor(QStringLiteral("Bernhard Beschow"), QStringLiteral("bbeschow@cs.tu-berlin.de"));
61}
62
63QIcon RouteSimulationPositionProviderPlugin::icon() const
64{
65 return QIcon();
66}
67
68PositionProviderPlugin* RouteSimulationPositionProviderPlugin::newInstance() const
69{
70 return new RouteSimulationPositionProviderPlugin( m_marbleModel );
71}
72
73PositionProviderStatus RouteSimulationPositionProviderPlugin::status() const
74{
75 return m_status;
76}
77
78GeoDataCoordinates RouteSimulationPositionProviderPlugin::position() const
79{
80 return m_currentPositionWithNoise;
81}
82
83GeoDataAccuracy 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
95RouteSimulationPositionProviderPlugin::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
108RouteSimulationPositionProviderPlugin::~RouteSimulationPositionProviderPlugin()
109{
110}
111
112void RouteSimulationPositionProviderPlugin::initialize()
113{
114 updateRoute();
115 connect(m_marbleModel->routingManager()->routingModel(), SIGNAL(currentRouteChanged()), this, SLOT(updateRoute()));
116}
117
118bool RouteSimulationPositionProviderPlugin::isInitialized() const
119{
120 return ( m_currentIndex > -2 );
121}
122
123qreal RouteSimulationPositionProviderPlugin::speed() const
124{
125 return m_speed;
126}
127
128qreal RouteSimulationPositionProviderPlugin::direction() const
129{
130 return m_directionWithNoise;
131}
132
133QDateTime RouteSimulationPositionProviderPlugin::timestamp() const
134{
135 return m_currentDateTime;
136}
137
138void 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
152void 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
260GeoDataCoordinates 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
267qreal 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
273void 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"
This file contains the headers for MarbleModel.
A 3d point representation.
GeoDataCoordinates moveByBearing(qreal bearing, qreal distance) const
Returns the coordinates of the resulting point after moving this point according to the distance and ...
Q_SCRIPTABLE CaptureState status()
void update(Part *part, const QByteArray &data, qint64 dataSize)
Binds a QML item to a specific geodetic location in screen coordinates.
QDateTime currentDateTime()
bool isValid() const const
T qobject_cast(QObject *object)
QString tr(const char *sourceText, const char *disambiguation, int n)
QFuture< ArgsType< Signal > > connect(Sender *sender, Signal signal)
This file is part of the KDE documentation.
Documentation copyright © 1996-2024 The KDE developers.
Generated on Fri Jun 21 2024 12:00:07 by doxygen 1.10.0 written by Dimitri van Heesch, © 1997-2006

KDE's Doxygen guidelines are available online.