X-Git-Url: https://git.street.me.uk/andy/viking.git/blobdiff_plain/c79f0206d3b01d52af7696498d905e509894f9c2..700b0908d71f64c9449ba372d0b9a8363257afb1:/src/viktrack.c

diff --git a/src/viktrack.c b/src/viktrack.c
index 5489081b..dc87516f 100644
--- a/src/viktrack.c
+++ b/src/viktrack.c
@@ -22,6 +22,9 @@
 #include <glib.h>
 #include <time.h>
 #include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
 #include "coords.h"
 #include "vikcoord.h"
 #include "viktrack.h"
@@ -29,9 +32,7 @@
 
 VikTrack *vik_track_new()
 {
-  VikTrack *tr = g_malloc ( sizeof ( VikTrack ) );
-  tr->trackpoints = NULL;
-  tr->comment = NULL;
+  VikTrack *tr = g_malloc0 ( sizeof ( VikTrack ) );
   tr->ref_count = 1;
   return tr;
 }
@@ -427,7 +428,7 @@ void vik_track_get_total_elevation_gain(const VikTrack *tr, gdouble *up, gdouble
 {
   gdouble diff;
   *up = *down = 0;
-  if ( tr->trackpoints )
+  if ( tr->trackpoints && VIK_TRACKPOINT(tr->trackpoints->data)->altitude != VIK_DEFAULT_ALTITUDE )
   {
     GList *iter = tr->trackpoints->next;
     while (iter)
@@ -439,21 +440,124 @@ void vik_track_get_total_elevation_gain(const VikTrack *tr, gdouble *up, gdouble
         *down -= diff;
       iter = iter->next;
     }
+  } else
+    *up = *down = VIK_DEFAULT_ALTITUDE;
+}
+
+typedef struct {
+  double a, b, c, d;
+} spline_coeff_t;
+
+void compute_spline(int n, double *x, double *f, spline_coeff_t *p) 
+{
+  double *h, *alpha, *B, *m;
+  int i;
+  int orig_n = n;
+  double new_x[3], new_f[3];
+  
+  if (n==0) return;
+  if (n==1) {
+    new_x[0] = x[0];
+    new_f[0] = f[0];
+    new_x[1] = x[0]+0.00001;
+    new_f[1] = f[0];
+    x = new_x;
+    f = new_f;
+    n = 3;
+  }
+  if (n==2) {
+    new_x[0] = x[0];
+    new_f[0] = f[0];
+    new_x[1] = x[1];
+    new_f[1] = f[1];
+    new_x[2] = x[1] + x[1]-x[0];
+    new_f[2] = f[1] + f[1]-f[0];
+    x = new_x;
+    f = new_f;
+    n = 3;
+  }
+  
+  /* we're solving a linear system of equations of the form Ax = B. 
+   * The matrix a is tridiagonal and consists of coefficients in 
+   * the h[] and alpha[] arrays. 
+   */
+
+  h = (double *)malloc(sizeof(double) * (n-1));
+  for (i=0; i<n-1; i++) {
+    h[i] = x[i+1]-x[i];
+  }
+
+  alpha = (double *)malloc(sizeof(double) * (n-2));
+  for (i=0; i<n-2; i++) {
+    alpha[i] = 2 * (h[i] + h[i+1]);
+  }
+
+  /* B[] is the vector on the right hand side of the equation */
+  B = (double *)malloc(sizeof(double) * (n-2));
+  for (i=0; i<n-2; i++) {
+    B[i] = 6 * ((f[i+2] - f[i+1])/h[i+1] - (f[i+1] - f[i])/h[i]);
+  }
+
+  /* Now solve the n-2 by n-2 system */
+  m = (double *)malloc(sizeof(double) * (n-2));
+  for (i=1; i<=n-3; i++) {
+    /*
+    d0 = alpha 0   
+    a0 = h1          
+    c0 = h1          
+
+      di = di - (ai-1 / di-1) * ci-1
+      bi = bi - (ai-1 / di-1) * bi-1
+      ;
+    */
+    alpha[i] = alpha[i] - (h[i]/alpha[i-1]) * h[i];
+    B[i] = B[i] - (h[i]/alpha[i-1]) * B[i-1];
+  }
+  /*  xn-3 = bn-3 / dn-3; */
+  m[n-3] = B[n-3]/alpha[n-3];
+  for (i=n-4; i>=0; i--) {
+    m[i] = (B[i]-h[i+1]*m[i+1])/alpha[i];
+  }
+
+  for (i=0; i<orig_n-1; i++) {
+    double mi, mi1;
+    mi = (i==(n-2)) ? 0 : m[i];
+    mi1 = (i==0) ? 0 : m[i-1];
+
+    p[i].a = f[i+1];
+    p[i].b = (f[i+1] - f[i]) / h[i]  +  h[i] * (2*mi + mi1) / 6;
+    p[i].c = mi/2;
+    p[i].d = (mi-mi1)/(6*h[i]);
   }
+
+  free(alpha);
+  free(B);
+  free(h);
+  free(m);
 }
 
+/* by Alex Foobarian */
 gdouble *vik_track_make_speed_map ( const VikTrack *tr, guint16 num_chunks )
 {
-  gdouble *pts;
-  gdouble duration, chunk_dur, t;
+  gdouble *v, *s, *t;
+  gdouble duration, chunk_dur, T, s_prev, s_now;
   time_t t1, t2;
-  int i;
+  int i, pt_count, numpts, spline;
+  GList *iter;
+  spline_coeff_t *p;
+  GList *mytr;
 
-  GList *iter = tr->trackpoints;
+  if ( ! tr->trackpoints )
+    return NULL;
 
   g_assert ( num_chunks < 16000 );
 
-  pts = g_malloc ( sizeof(gdouble) * num_chunks );
+#ifdef XXXXXXXXXXXXXXXXXX
+  iter = tr->trackpoints;
+  while (iter) {
+    
+  }
+#endif /*XXXXXXXXXXXXXXXXXX*/
 
   t1 = VIK_TRACKPOINT(tr->trackpoints->data)->timestamp;
   t2 = VIK_TRACKPOINT(g_list_last(tr->trackpoints)->data)->timestamp;
@@ -466,17 +570,180 @@ gdouble *vik_track_make_speed_map ( const VikTrack *tr, guint16 num_chunks )
     fprintf(stderr, "negative duration: unsorted trackpoint timestamps?\n");
     return NULL;
   }
-  
+  pt_count = vik_track_get_tp_count(tr);
+
+  v = g_malloc ( sizeof(gdouble) * num_chunks );
   chunk_dur = duration / num_chunks;
-  t = t1;
-  for (i = 0; i < num_chunks; i++, t+=chunk_dur) {
-    while (iter && VIK_TRACKPOINT(iter->data)->timestamp <= t) {
+
+  s = g_malloc(sizeof(double) * pt_count);
+  t = g_malloc(sizeof(double) * pt_count);
+  p = g_malloc(sizeof(spline_coeff_t) * (pt_count-1));
+
+  iter = tr->trackpoints->next;
+  numpts = 0;
+  s[0] = 0;
+  t[0] = VIK_TRACKPOINT(iter->prev->data)->timestamp;
+  numpts++;
+  while (iter) {
+    s[numpts] = s[numpts-1] + vik_coord_diff ( &(VIK_TRACKPOINT(iter->prev->data)->coord), &(VIK_TRACKPOINT(iter->data)->coord) );
+    t[numpts] = VIK_TRACKPOINT(iter->data)->timestamp;
+    numpts++;
+    iter = iter->next;
+  }
+
+  compute_spline(numpts, t, s, p);
+
+  /*
+  printf("Got spline\n");
+  for (i=0; i<numpts-1; i++) {
+    printf("a = %15f  b = %15f  c = %15f  d = %15f\n", p[i].a, p[i].b, p[i].c, p[i].d);
+  }
+  */
+
+  /* the spline gives us distances at chunk_dur intervals. from these,
+   * we obtain average speed in each interval.
+   */
+  spline = 0;
+  T = t[spline];
+  s_prev = 
+      p[spline].d * pow(T - t[spline+1], 3) + 
+      p[spline].c * pow(T - t[spline+1], 2) + 
+      p[spline].b * (T - t[spline+1]) + 
+      p[spline].a;
+  for (i = 0; i < num_chunks; i++, T+=chunk_dur) {
+    while (T > t[spline+1]) {
+      spline++;
+    }
+    s_now = 
+      p[spline].d * pow(T - t[spline+1], 3) + 
+      p[spline].c * pow(T - t[spline+1], 2) + 
+      p[spline].b * (T - t[spline+1]) + 
+      p[spline].a;
+    v[i] = (s_now - s_prev) / chunk_dur;
+    s_prev = s_now;
+    /* 
+     * old method of averages
+    v[i] = (s[spline+1]-s[spline])/(t[spline+1]-t[spline]);
+    */
+  }
+  g_free(s);
+  g_free(t);
+  g_free(p);
+  return v;
+}
+
+/* by Alex Foobarian */
+VikTrackpoint *vik_track_get_closest_tp_by_percentage_dist ( VikTrack *tr, gdouble reldist )
+{
+  gdouble dist = vik_track_get_length_including_gaps(tr) * reldist;
+  gdouble current_dist = 0.0;
+  gdouble current_inc = 0.0;
+  if ( tr->trackpoints )
+  {
+    GList *iter = tr->trackpoints->next;
+    GList *last_iter = NULL;
+    while (iter)
+    {
+      current_inc = vik_coord_diff ( &(VIK_TRACKPOINT(iter->data)->coord),
+                                     &(VIK_TRACKPOINT(iter->prev->data)->coord) );
+      current_dist += current_inc;
+      if ( current_dist >= dist )
+        break;
+      last_iter = iter;
       iter = iter->next;
     }
-    pts[i] = vik_coord_diff ( &(VIK_TRACKPOINT(iter->prev->data)->coord),
-					  &(VIK_TRACKPOINT(iter->data)->coord) ) / 
-      (gdouble)(VIK_TRACKPOINT(iter->data)->timestamp - VIK_TRACKPOINT(iter->prev->data)->timestamp);
+    if (!iter) /* passing the end the track */
+      return (last_iter ? last_iter->data : NULL);
+    /* we've gone past the dist already, was prev trackpoint closer? */
+    /* should do a vik_coord_average_weighted() thingy. */
+    if ( iter->prev && abs(current_dist-current_inc-dist) < abs(current_dist-dist) )
+      iter = iter->prev;
+
+    return VIK_TRACKPOINT(iter->data);
+
   }
+  return NULL;
+}
 
-  return pts;
+gboolean vik_track_get_minmax_alt ( const VikTrack *tr, gdouble *min_alt, gdouble *max_alt )
+{
+  *min_alt = 25000;
+  *max_alt = -5000;
+  if ( tr && tr->trackpoints && tr->trackpoints->data && (VIK_TRACKPOINT(tr->trackpoints->data)->altitude != VIK_DEFAULT_ALTITUDE) ) {
+    GList *iter = tr->trackpoints->next;
+    gdouble tmp_alt;
+    while (iter)
+    {
+      tmp_alt = VIK_TRACKPOINT(iter->data)->altitude;
+      if ( tmp_alt > *max_alt )
+        *max_alt = tmp_alt;
+      if ( tmp_alt < *min_alt )
+        *min_alt = tmp_alt;
+      iter = iter->next;
+    }
+    return TRUE;
+  }
+  return FALSE;
 }
+
+void vik_track_marshall ( VikTrack *tr, guint8 **data, guint *datalen)
+{
+  GList *tps;
+  GByteArray *b = g_byte_array_new();
+  guint len;
+  guint intp, ntp;
+
+  g_byte_array_append(b, (guint8 *)tr, sizeof(*tr));
+
+  /* we'll fill out number of trackpoints later */
+  intp = b->len;
+  g_byte_array_append(b, (guint8 *)&len, sizeof(len));
+
+  tps = tr->trackpoints;
+  ntp = 0;
+  while (tps) {
+    g_byte_array_append(b, (guint8 *)tps->data, sizeof(VikTrackpoint));
+    tps = tps->next;
+    ntp++;
+  }
+  *(guint *)(b->data + intp) = ntp;
+
+  len = (tr->comment) ? strlen(tr->comment)+1 : 0; 
+  g_byte_array_append(b, (guint8 *)&len, sizeof(len)); 
+  if (tr->comment) g_byte_array_append(b, (guint8 *)tr->comment, len);
+
+  *data = b->data;
+  *datalen = b->len;
+  g_byte_array_free(b, FALSE);
+}
+
+VikTrack *vik_track_unmarshall (guint8 *data, guint datalen)
+{
+  guint len;
+  VikTrack *new_tr = vik_track_new();
+  VikTrackpoint *new_tp;
+  guint ntp;
+  gint i;
+
+  /* only the visibility is needed */
+  new_tr->visible = ((VikTrack *)data)->visible;
+  data += sizeof(*new_tr);
+
+  ntp = *(guint *)data;
+  data += sizeof(ntp);
+
+  for (i=0; i<ntp; i++) {
+    new_tp = vik_trackpoint_new();
+    memcpy(new_tp, data, sizeof(*new_tp));
+    data += sizeof(*new_tp);
+    new_tr->trackpoints = g_list_append(new_tr->trackpoints, new_tp);
+  }
+
+  len = *(guint *)data;
+  data += sizeof(len);
+  if (len) {
+    new_tr->comment = g_strdup((gchar *)data);
+  }
+  return new_tr;
+}
+