Issue 3048: Teach fix distance to use length on spline path (#3049)

* Issue 3048: Teach fix distance to use length on spline path

* Remove debug pragma

src/FileIO/FixDeriveDistance.cpp

@@ -24,8 +24,7 @@
 #include "HelpWhatsThis.h"
 #include <algorithm>
 #include <QVector>
-
-#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; i<ride->dataPoints().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);

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<SphericalTwoPointInterpolator>::Push(distance, point.toxyz());
-}
+}

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 <typename T, int T_count> 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<CalcSplineLengthBracketPair, s_worklistSize> 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<SphericalTwoPointInterpolator>