diff --git a/src/FileIO/FixDeriveDistance.cpp b/src/FileIO/FixDeriveDistance.cpp index 76336cbb0..bf8c5110e 100644 --- a/src/FileIO/FixDeriveDistance.cpp +++ b/src/FileIO/FixDeriveDistance.cpp @@ -24,8 +24,7 @@ #include "HelpWhatsThis.h" #include #include - -#define pi 3.14159265358979323846 +#include "LocationInterpolation.h" // Config widget used by the Preferences/Options config panes class FixDeriveDistance; @@ -36,6 +35,9 @@ class FixDeriveDistanceConfig : public DataProcessorConfig friend class ::FixDeriveDistance; protected: QHBoxLayout *layout; + + QCheckBox *useCubicSplines; + QLabel *bikeWeightLabel; QDoubleSpinBox *bikeWeight; QLabel *crrLabel; @@ -49,6 +51,10 @@ class FixDeriveDistanceConfig : public DataProcessorConfig layout = new QHBoxLayout(this); + useCubicSplines = new QCheckBox(tr("Use Cubic Splines"), this); + useCubicSplines->setCheckState(Qt::Checked); + layout->addWidget(useCubicSplines); + layout->setContentsMargins(0,0,0,0); setContentsMargins(0,0,0,0); @@ -66,10 +72,11 @@ class FixDeriveDistanceConfig : public DataProcessorConfig QString explain() { return(QString(tr("Derive distance based on " - "GPS position\n\n" - "Some unit doesn't record distance without " - "a speedometer but record position " - "(eg Timex Cycle Trainer)\n\n"))); + "ridefile's GPS locations\n\n" + "This process will populate distance information (and override existing distance information if present.)" + "The cubic splines processing estimates distance across polynomial curve, " + "otherwise this feature will compute geometric arc distance between ride points." + "\n\n"))); } }; @@ -101,7 +108,7 @@ class FixDeriveDistance : public DataProcessor { static bool FixDeriveDistanceAdded = DataProcessorFactory::instance().registerProcessor(QString("Estimate Distance Values"), new FixDeriveDistance()); double _deg2rad(double deg) { - return (deg * pi / 180); + return (deg * M_PI / 180); } bool @@ -110,23 +117,89 @@ FixDeriveDistance::postProcess(RideFile *ride, DataProcessorConfig *config=0, QS Q_UNUSED(config) Q_UNUSED(op) - // if its already there do nothing ! - if (ride->areDataPresent()->km) return false; + bool fUseCubicSplines = ((FixDeriveDistanceConfig*)(config))->useCubicSplines->isChecked(); + bool fUseSpeedAndTime = false; - // no dice if we don't have alt and speed - if (!ride->areDataPresent()->lat) return false; + GeoPointInterpolator gpi; + int ii = 0; // interpolator index + double cubicDistanceKM = 0.0; + + double distanceFromSpeedTime = 0.0; + double lastSecs = 0.0; + bool fHasSpeedTime = (ride->areDataPresent()->kph && ride->areDataPresent()->secs); + + if (fUseSpeedAndTime && !fHasSpeedTime) + return false; + + // if its already there do nothing ! + //if (ride->areDataPresent()->km) return false; + + // no dice if we don't have any gps location information. + if (!fUseSpeedAndTime && !ride->areDataPresent()->lat) + return false; + + bool fHasAlt = ride->areDataPresent()->alt; // apply the change ride->command->startLUW("Estimate Distance"); - if (ride->areDataPresent()->lat) { + { double km = 0.0; double lastLat = 0; double lastLon = 0; for (int i=0; idataPoints().count(); i++) { + RideFilePoint *p = ride->dataPoints()[i]; + // Compute distance using cubic splines. + while (gpi.WantsInput(i)) { + + // If bracket is present, always sum its length before any push. + // This maintains cubicDistanceKM as distance to current bracket + // start. + double d0, d1; + if (gpi.GetBracket(d0, d1)) { + cubicDistanceKM += (gpi.SplineLength(d0, d1) / 1000.0); + } + + if (ii >= ride->dataPoints().count()) { + // Past end of ride points, continue pushing final point. + RideFilePoint *pii = ride->dataPoints()[ii-1]; + geolocation geo(pii->lat, pii->lon, fHasAlt ? pii->alt : 0.0); + if (geo.IsReasonableGeoLocation()) { + gpi.Push(ii, geo); + //gpi.NotifyInputComplete(); + } + } else { + // Use index for distance, since we just use it as an enumeration + RideFilePoint *pii = ride->dataPoints()[ii]; + geolocation geo(pii->lat, pii->lon, fHasAlt ? pii->alt : 0.0); + if (geo.IsReasonableGeoLocation()) { + gpi.Push(ii, geo); + ii++; + } + } + } + + // Compute distance using speed and time (has high variance.) + // Currently computed because it is interesting to see, but no + // option enabled to apply it. + double distDelta = 0.0; + if (fHasSpeedTime) + { + double secs = p->secs; + double kph = p->kph; + + double secDelta = secs - lastSecs; + distDelta = secDelta * kph / 3600; + + distanceFromSpeedTime += distDelta; + + lastSecs = secs; + } + + // Compute distance using geometric arc length between points. double _theta, _dist; _theta = lastLon - p->lon; if (lastLat == 0.0 || lastLon == 0.0 || p->lat == 0.0 || p->lon == 0.0 || (_theta == 0.0 && (lastLat - p->lat) == 0.0)) { @@ -141,7 +214,20 @@ FixDeriveDistance::postProcess(RideFile *ride, DataProcessorConfig *config=0, QS lastLat = p->lat; lastLon = p->lon; - ride->command->setPointValue(i, RideFile::km, km); + // Write distance into ride file. + double distanceKM = km; + + // Select distance estimate to use + if (fUseSpeedAndTime) { + distanceKM = distanceFromSpeedTime; + } else if (fUseCubicSplines) { + distanceKM = cubicDistanceKM; + } else { + distanceKM = km; + } + + // Apply selected distance estimate to ride file + ride->command->setPointValue(i, RideFile::km, distanceKM); } ride->setDataPresent(ride->km, true); diff --git a/src/FileIO/LocationInterpolation.cpp b/src/FileIO/LocationInterpolation.cpp index caf56f3ed..6ed39de34 100644 --- a/src/FileIO/LocationInterpolation.cpp +++ b/src/FileIO/LocationInterpolation.cpp @@ -21,7 +21,7 @@ static double radianstodegrees(double r) { return r * (360.0 / (2 * M_PI)); } static double degreestoradians(double d) { return d * (2 * M_PI / 360.0); } -xyz geolocation::toxyz() +xyz geolocation::toxyz() const { // Approach developed by: // @@ -52,7 +52,7 @@ xyz geolocation::toxyz() return(xyz(dx, dy, dz)); //Return x, y, z in ECEF } -geolocation xyz::togeolocation() +geolocation xyz::togeolocation() const { // Approach developed by: // (Olson, D.K. (1996). @@ -246,4 +246,4 @@ geolocation GeoPointInterpolator::Interpolate(double distance) void GeoPointInterpolator::Push(double distance, geolocation point) { DistancePointInterpolator::Push(distance, point.toxyz()); -} +} \ No newline at end of file diff --git a/src/FileIO/LocationInterpolation.h b/src/FileIO/LocationInterpolation.h index ef3714253..80cad0fa6 100644 --- a/src/FileIO/LocationInterpolation.h +++ b/src/FileIO/LocationInterpolation.h @@ -81,7 +81,11 @@ struct xyz : public v3 // qDebug() << "< X:" << x() << " y:" << y() << " z:" << z() << " >"; //} - geolocation togeolocation(); + double DistanceFrom(const xyz& from) const { + return this->subtract(from).magnitude(); + } + + geolocation togeolocation() const; }; struct geolocation : v3 @@ -102,7 +106,23 @@ struct geolocation : v3 // qDebug() << "< Lat:" << Lat() << " Long:" << Long() << " Alt:" << Alt() << " >"; //} - xyz toxyz(); + xyz toxyz() const; + + double DistanceFrom(const geolocation& from) const { + xyz x0 = from.toxyz(); + xyz x1 = this->toxyz(); + + double dist = x1.subtract(x0).magnitude(); + + return dist; + } + + bool IsReasonableGeoLocation () const { + return (this->Lat() && this->Lat() >= double(-90) && this->Lat() <= double(90) && + this->Long() && this->Long() >= double(-180) && this->Long() <= double(180) && + this->Alt() >= -1000 && this->Alt() < 10000); + } + }; // Class to wrap classic game spherical interpolation @@ -512,6 +532,141 @@ public: return m_Interpolator.Interpolate(frac); } + +private: + + struct CalcSplineLengthBracketPair + { + double d0, d1; + + CalcSplineLengthBracketPair() {} + CalcSplineLengthBracketPair(double a0, double a1) : d0(a0), d1(a1) {} + }; + + // A static sized (static sized/frame allocatable) worklist for pushing and popping stuffs. + template class CalcSplineLengthWorklist + { + static_assert(T_count > 0 && T_count <= 64, "Worklist element count must be within 1 and 64"); + + T m_worklist[T_count]; + int m_idx; // points to first unused element in worklist + + public: + + CalcSplineLengthWorklist() : m_idx(0) {} + + unsigned EmptySlots() const { + return T_count - m_idx; + } + + bool Push(T rT) { + if (m_idx >= T_count) + return false; + + m_worklist[m_idx] = rT; + m_idx++; + return true; + } + + bool Pop(T& rT) { + if (m_idx == 0) return false; + + m_idx--; + rT = m_worklist[m_idx]; + return true; + } + }; + +public: + + // + // SplineLength: Estimate path distance across bracketed spline range. + // + // Estimation is necessary here because there is no closed form for length of cubic spline. + // + // Cubic spline has 4 control points. The interpolation is only valid within the middle 2 points. + // I call these middle two points the 'bracket'. + // + // Method takes two distances within the bracket and estimates the length of the spline that is + // interpolated between those two interpolated points. + // + // Estimation technique I use : + // + // 1 Break provided distance range into 3 equidistant distance ranges (4 distances) + // 2 Interpolate location for each of those 4 locations + // 3 Compute 'linear distance': the geometric distance from first to last location. + // 4 Compute 'quad distance': the sum of geometric distance between those 4 locations. + // 5 Examine ratio of linear distance / quad distance. + // a If below threshold accumulate the summed quad distance + // b If above threshold then push the 3 ranges onto worklist and goto 1 + // + double SplineLength(double d0, double d1, double thresholdLimit = 0.000001) + { + double bracketStart, bracketEnd; + + // Ensure: + // - bracket exists + // - requested range is within bracket + // - range is ordered + if (!this->GetBracket(bracketStart, bracketEnd) + || d0 < bracketStart + || d0 > bracketEnd + || d1 < bracketStart + || d1 > bracketEnd + || d0 > d1) + { + return 0.0; + } + + double finalLength = 0.0; + + static const unsigned s_worklistSize = 32; + CalcSplineLengthWorklist worklist; + + // Push initial range for processing. + if (!worklist.Push(CalcSplineLengthBracketPair(d0, d1))) + return 0.0; + + CalcSplineLengthBracketPair workitem; + while (worklist.Pop(workitem)) { + d0 = workitem.d0; + d1 = workitem.d1; + + // Step 1 + const double quarterspan = (d1 - d0) / 4; + const double inter0 = d0 + quarterspan; + const double inter1 = d0 + quarterspan + quarterspan; + + // Step 2 + const xyz pm1 = this->Interpolate(d0); + const xyz p0 = this->Interpolate(inter0); + const xyz p1 = this->Interpolate(inter1); + const xyz p2 = this->Interpolate(d1); + + // Step 3 + double linearDistance = p2.DistanceFrom(pm1); + if (linearDistance == 0.0) + break; + + // Step 4 + double quadDistance = p2.DistanceFrom(p1) + p1.DistanceFrom(p0) + p0.DistanceFrom(pm1); + + // Step 5 + double difference = fabs(quadDistance / linearDistance) - 1.0; + + // 5A: Settle for quaddistance if threshold met or no more room on worklist. + if (difference < thresholdLimit || worklist.EmptySlots() < 3) { + finalLength += quadDistance; + } else { + // 5B: otherwise push the 3 new subsegments onto worklist. + worklist.Push(CalcSplineLengthBracketPair(d0, inter0)); + worklist.Push(CalcSplineLengthBracketPair(inter0, inter1)); + worklist.Push(CalcSplineLengthBracketPair(inter1, d1)); + } + } + + return finalLength; + } }; class GeoPointInterpolator : public DistancePointInterpolator