File indexing completed on 2024-04-28 07:38:11
0001 // SPDX-License-Identifier: LGPL-2.1-or-later 0002 // 0003 // SPDX-FileCopyrightText: 2011 Konrad Enzensberger <e.konrad@mpegcode.com> 0004 // SPDX-FileCopyrightText: 2011 Dennis Nienhüser <nienhueser@kde.org> 0005 // SPDX-FileCopyrightText: 2012 Bernhard Beschow <bbeschow@cs.tu-berlin.de> 0006 // 0007 0008 #include "RouteSimulationPositionProviderPlugin.h" 0009 0010 #include "MarbleModel.h" 0011 #include "routing/Route.h" 0012 #include "routing/RoutingManager.h" 0013 #include "routing/RoutingModel.h" 0014 #include "GeoDataAccuracy.h" 0015 0016 #include <QIcon> 0017 0018 namespace Marble 0019 { 0020 0021 namespace { 0022 qreal const c_frequency = 4.0; // Hz 0023 } 0024 0025 QString RouteSimulationPositionProviderPlugin::name() const 0026 { 0027 return tr( "Current Route Position Provider Plugin" ); 0028 } 0029 0030 QString RouteSimulationPositionProviderPlugin::nameId() const 0031 { 0032 return QStringLiteral("RouteSimulationPositionProviderPlugin"); 0033 } 0034 0035 QString RouteSimulationPositionProviderPlugin::guiString() const 0036 { 0037 return tr( "Current Route" ); 0038 } 0039 0040 QString RouteSimulationPositionProviderPlugin::version() const 0041 { 0042 return QStringLiteral("1.1"); 0043 } 0044 0045 QString RouteSimulationPositionProviderPlugin::description() const 0046 { 0047 return tr( "Simulates traveling along the current route." ); 0048 } 0049 0050 QString RouteSimulationPositionProviderPlugin::copyrightYears() const 0051 { 0052 return QStringLiteral("2011, 2012"); 0053 } 0054 0055 QVector<PluginAuthor> RouteSimulationPositionProviderPlugin::pluginAuthors() const 0056 { 0057 return QVector<PluginAuthor>() 0058 << PluginAuthor(QStringLiteral("Konrad Enzensberger"), QStringLiteral("e.konrad@mpegcode.com")) 0059 << PluginAuthor(QStringLiteral("Dennis Nienhüser"), QStringLiteral("nienhueser@kde.org")) 0060 << PluginAuthor(QStringLiteral("Bernhard Beschow"), QStringLiteral("bbeschow@cs.tu-berlin.de")); 0061 } 0062 0063 QIcon RouteSimulationPositionProviderPlugin::icon() const 0064 { 0065 return QIcon(); 0066 } 0067 0068 PositionProviderPlugin* RouteSimulationPositionProviderPlugin::newInstance() const 0069 { 0070 return new RouteSimulationPositionProviderPlugin( m_marbleModel ); 0071 } 0072 0073 PositionProviderStatus RouteSimulationPositionProviderPlugin::status() const 0074 { 0075 return m_status; 0076 } 0077 0078 GeoDataCoordinates RouteSimulationPositionProviderPlugin::position() const 0079 { 0080 return m_currentPositionWithNoise; 0081 } 0082 0083 GeoDataAccuracy RouteSimulationPositionProviderPlugin::accuracy() const 0084 { 0085 GeoDataAccuracy result; 0086 0087 // faked values 0088 result.level = GeoDataAccuracy::Detailed; 0089 result.horizontal = 10.0; 0090 result.vertical = 10.0; 0091 0092 return result; 0093 } 0094 0095 RouteSimulationPositionProviderPlugin::RouteSimulationPositionProviderPlugin(MarbleModel *marbleModel , QObject *parent) : 0096 PositionProviderPlugin(parent), 0097 m_marbleModel( marbleModel ), 0098 m_currentIndex( -2 ), 0099 m_status( PositionProviderStatusUnavailable ), 0100 m_currentDateTime(), 0101 m_speed( 0.0 ), 0102 m_direction( 0.0 ), 0103 m_directionWithNoise(0.0) 0104 { 0105 connect(&m_updateTimer, SIGNAL(timeout()), this, SLOT(update())); 0106 } 0107 0108 RouteSimulationPositionProviderPlugin::~RouteSimulationPositionProviderPlugin() 0109 { 0110 } 0111 0112 void RouteSimulationPositionProviderPlugin::initialize() 0113 { 0114 updateRoute(); 0115 connect(m_marbleModel->routingManager()->routingModel(), SIGNAL(currentRouteChanged()), this, SLOT(updateRoute())); 0116 } 0117 0118 bool RouteSimulationPositionProviderPlugin::isInitialized() const 0119 { 0120 return ( m_currentIndex > -2 ); 0121 } 0122 0123 qreal RouteSimulationPositionProviderPlugin::speed() const 0124 { 0125 return m_speed; 0126 } 0127 0128 qreal RouteSimulationPositionProviderPlugin::direction() const 0129 { 0130 return m_directionWithNoise; 0131 } 0132 0133 QDateTime RouteSimulationPositionProviderPlugin::timestamp() const 0134 { 0135 return m_currentDateTime; 0136 } 0137 0138 void RouteSimulationPositionProviderPlugin::updateRoute(){ 0139 m_currentIndex = -1; 0140 m_lineString = m_lineStringInterpolated = m_marbleModel->routingManager()->routingModel()->route().path(); 0141 m_speed = 0; //initialize speed to be around 25 m/s; 0142 bool const canWork = !m_lineString.isEmpty() || m_currentPosition.isValid(); 0143 if (canWork) { 0144 changeStatus(PositionProviderStatusAcquiring); 0145 m_updateTimer.start(1000.0 / c_frequency); 0146 } else { 0147 changeStatus(PositionProviderStatusUnavailable); 0148 m_updateTimer.stop(); 0149 } 0150 } 0151 0152 void RouteSimulationPositionProviderPlugin::update() 0153 { 0154 if (m_lineString.isEmpty() && m_currentPosition.isValid()) { 0155 m_currentPositionWithNoise = addNoise(m_currentPosition, accuracy()); 0156 changeStatus(PositionProviderStatusAvailable); 0157 emit positionChanged(position(), accuracy()); 0158 return; 0159 } 0160 0161 if ( m_currentIndex >= 0 && m_currentIndex < m_lineStringInterpolated.size() ) { 0162 changeStatus(PositionProviderStatusAvailable); 0163 GeoDataCoordinates newPosition = m_lineStringInterpolated.at( m_currentIndex ); 0164 const QDateTime newDateTime = QDateTime::currentDateTime(); 0165 qreal time= m_currentDateTime.msecsTo(newDateTime)/1000.0; 0166 if ( m_currentPosition.isValid() ) { 0167 //speed calculations 0168 //Max speed is set on points (m_lineStringInterpolated) based on formula. (max speed before points is calculated so the acceleration won't be exceeded) 0169 const qreal acceleration = 1.5; 0170 const qreal lookForwardDistance = 1000; 0171 qreal checkedDistance = m_currentPosition.sphericalDistanceTo(m_lineStringInterpolated.at(m_currentIndex))* m_marbleModel->planetRadius(); 0172 const qreal maxSpeed = 25; 0173 const qreal minSpeed = 2; 0174 qreal newSpeed = qMin((m_speed + acceleration*time), maxSpeed); 0175 for (int i=qMax(1,m_currentIndex); i<m_lineStringInterpolated.size()-1 && checkedDistance<lookForwardDistance; ++i) 0176 { 0177 qreal previousHeading = m_lineStringInterpolated.at( i-1 ).bearing( m_lineStringInterpolated.at( i ), GeoDataCoordinates::Degree, GeoDataCoordinates::FinalBearing ); 0178 qreal curveLength = 10;//we treat one point turn as a curve of length 10 0179 qreal angleSum = 0;//sum of turn angles in a curve 0180 for (int j=i+1; j<m_lineStringInterpolated.size() && curveLength<35; ++j) 0181 { 0182 qreal newHeading = m_lineStringInterpolated.at( j-1 ).bearing( m_lineStringInterpolated.at( j ), GeoDataCoordinates::Degree, GeoDataCoordinates::FinalBearing ); 0183 qreal differenceHeading = qAbs(previousHeading-newHeading);//angle of turn 0184 if(differenceHeading>180) { 0185 differenceHeading = 360 - differenceHeading; 0186 } 0187 angleSum +=differenceHeading; 0188 qreal maxSpeedAtTurn = qMax((1 - (static_cast<qreal>(angleSum/60.0/curveLength*10.0))*maxSpeed), minSpeed);//speed limit at turn 0189 if( checkedDistance<25 && maxSpeedAtTurn<newSpeed )//if we are near turn don't accelerate, if we will have to slow down 0190 newSpeed = qMin(newSpeed, qMax(m_speed,maxSpeedAtTurn)); 0191 // formulas: 0192 // s = Vc * t + a*t*t/2 0193 // V0 = Vc + a*t 0194 // V0 = maxCurrentSpeed 0195 // Vc = maxSpeedAtTurn 0196 // s = checkedDistance 0197 // a = acceleration 0198 qreal delta = maxSpeedAtTurn*maxSpeedAtTurn - 4.0*acceleration/2.0*(-checkedDistance);//delta = b*b-4*a*c 0199 qreal t = (-maxSpeedAtTurn+sqrt(delta))/(2.0*acceleration/2.0);//(-b+sqrt(delta))/(2*c) 0200 qreal maxCurrentSpeed = maxSpeedAtTurn + acceleration*t; 0201 newSpeed = qMin(newSpeed, maxCurrentSpeed); 0202 previousHeading = newHeading; 0203 curveLength += m_lineStringInterpolated.at(j - 1).sphericalDistanceTo(m_lineStringInterpolated.at(j)) * m_marbleModel->planetRadius(); 0204 } 0205 checkedDistance += m_lineStringInterpolated.at(i).sphericalDistanceTo(m_lineStringInterpolated.at(i + 1)) * m_marbleModel->planetRadius(); 0206 } 0207 m_speed=newSpeed; 0208 0209 //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. 0210 //If the function isn't called once exactly after a second, multiplying by the time will compensate for the error and maintain the speed. 0211 qreal fraction = m_speed*time/(m_currentPosition.sphericalDistanceTo(newPosition) * m_marbleModel->planetRadius()); 0212 0213 //Interpolate and find the next point to move to if needed. 0214 if(fraction>0 && fraction <1){ 0215 GeoDataCoordinates newPoint = m_currentPosition.interpolate(newPosition,fraction); 0216 newPosition=newPoint; 0217 } 0218 else if ( fraction > 1 ) { 0219 bool isCurrentIndexValid = true; 0220 while ( fraction > 1 ) { 0221 ++m_currentIndex; 0222 if ( m_currentIndex >= m_lineStringInterpolated.size() ) { 0223 isCurrentIndexValid = false; 0224 break; 0225 } 0226 0227 newPosition = m_lineStringInterpolated.at( m_currentIndex ); 0228 fraction = m_speed*time / (m_currentPosition.sphericalDistanceTo(newPosition) * m_marbleModel->planetRadius()); 0229 } 0230 0231 if ( isCurrentIndexValid ) { 0232 GeoDataCoordinates newPoint = m_currentPosition.interpolate( newPosition, fraction ); 0233 newPosition = newPoint; 0234 } 0235 } 0236 else 0237 { 0238 m_currentIndex++; 0239 } 0240 0241 m_direction = m_currentPosition.bearing( newPosition, GeoDataCoordinates::Degree, GeoDataCoordinates::FinalBearing ); 0242 m_directionWithNoise = addNoise(m_direction); 0243 } 0244 m_currentPosition = newPosition; 0245 m_currentPositionWithNoise = addNoise(m_currentPosition, accuracy()); 0246 m_currentDateTime = newDateTime; 0247 emit positionChanged( position(), accuracy() ); 0248 } 0249 else { 0250 // Repeat from start 0251 m_currentIndex = 0; 0252 m_lineStringInterpolated = m_lineString; 0253 m_currentPosition = GeoDataCoordinates(); //Reset the current position so that the simulation starts from the correct starting point. 0254 m_currentPositionWithNoise = GeoDataCoordinates(); 0255 m_speed = 0; 0256 changeStatus(PositionProviderStatusUnavailable); 0257 } 0258 } 0259 0260 GeoDataCoordinates RouteSimulationPositionProviderPlugin::addNoise(const Marble::GeoDataCoordinates &position, const Marble::GeoDataAccuracy &accuracy ) const 0261 { 0262 qreal randomBearing = static_cast<qreal>(qrand()) / (static_cast<qreal>(RAND_MAX/M_PI)); 0263 qreal randomDistance = static_cast<qreal>(qrand()) / (static_cast<qreal>(RAND_MAX/(accuracy.horizontal / 2.0 / m_marbleModel->planetRadius()))); 0264 return position.moveByBearing(randomBearing, randomDistance); 0265 } 0266 0267 qreal RouteSimulationPositionProviderPlugin::addNoise(qreal bearing) 0268 { 0269 qreal const maxBearingError = 30.0; 0270 return bearing + static_cast<qreal>(qrand()) / (static_cast<qreal>(RAND_MAX/maxBearingError/2.0)) - maxBearingError / 2.0; 0271 } 0272 0273 void RouteSimulationPositionProviderPlugin::changeStatus(PositionProviderStatus status) 0274 { 0275 if (m_status != status) { 0276 m_status = status; 0277 emit statusChanged(m_status); 0278 } 0279 } 0280 0281 } // namespace Marble 0282 0283 #include "moc_RouteSimulationPositionProviderPlugin.cpp"