[Positioner] Point3D: new interpolation function

Add Point3D::interpolate(point, speed, duration), that returns a single
interpolated point computed according to speed and duration of the
mobile's movement.

owlps-positioner/point3d.cc

@@ -13,6 +13,7 @@
 
 
 #include "point3d.hh"
+#include "timestamp.hh"
 #include "posutil.hh"
 #include "posexcept.hh"
 
@@ -145,9 +146,10 @@ void Point3D::rotate_2d(const Point3D &c, float angle)
  * intermediate points; when the function returns, it is set to the
  * actual step used.
  * @param[out] interpolated The vector in which the intermediate values
- * will be stored. The two extremities are not included in this vector,
+ * will be added. The two extremities are not included in this vector,
  * but of course the calling function can add values prior and after the
- * function call.
+ * function call (existing data is not erased by this function, only new
+ * points are added at the end of the vector).
  */
 void Point3D::interpolate(const Point3D &end, float &step_hint,
                           std::vector<Point3D> &interpolated) const
@@ -192,6 +194,64 @@ void Point3D::interpolate(const Point3D &end, float &step_hint,
 }
 
 
+/**
+ * This function computes the coordinates of the point that would be
+ * reached by a mobile terminal traveling during `time` at `speed` km/h,
+ * starting from the current point in direction of the point `end`;
+ * these coordinates are returned. In case the speed of the terminal
+ * would allow it to travel further than `end`, `end` is returned.
+ *
+ * @param[in] end Coordinate of the furthest point, in direction of which
+ * the mobile is travelling.
+ * @param[in] speed The maximal speed at which the mobile can travel, in
+ * km/h.
+ * @param[in] duration The travel time of the mobile.
+ *
+ * @returns The coordinates of the furthest point between the current
+ * point and `end` that can be reached by the mobile.
+ * @returns `end` if the mobile could travel further than `end`.
+ */
+Point3D Point3D::interpolate(const Point3D &end, const float speed,
+                             const Timestamp &duration) const
+{
+  float dist = distance(end) ; // Distance between the two extremities
+  double speed_mps = speed * 1000 / 3600 ; // Speed in m/s
+  // Duration in seconds:
+  double duration_s = static_cast<uint64_t>(duration) / 1000.0 ;
+  // Distance covered by the mobile during `duration` at `speed`:
+  float dist_covered = speed_mps * duration_s ;
+
+  /* If the mobile is fast enough to reach `end`, return `end` */
+  if (dist_covered >= dist)
+    return end ;
+
+  /* Set up the tweener */
+  // Individual tweeners for X, Y and Z
+  double tweened_x = x, tweened_y = y, tweened_z = z ;
+  claw::tween::single_tweener
+  interpolator_x(tweened_x, end.x, dist,
+                 claw::tween::easing_linear::ease_in) ;
+  claw::tween::single_tweener
+  interpolator_y(tweened_y, end.y, dist,
+                 claw::tween::easing_linear::ease_in) ;
+  claw::tween::single_tweener
+  interpolator_z(tweened_z, end.z, dist,
+                 claw::tween::easing_linear::ease_in) ;
+  // Consolidated tweener for both X and Y
+  claw::tween::tweener_group interpolator ;
+  interpolator.insert(interpolator_x) ;
+  interpolator.insert(interpolator_y) ;
+  interpolator.insert(interpolator_z) ;
+
+  /* Compute the interpolated value */
+  assert(! interpolator.is_finished()) ;
+  // Update tweened_x, tweened_y, tweened_z
+  interpolator.update(dist_covered) ;
+  // Return the interpolated coordinates
+  return Point3D(tweened_x, tweened_y, tweened_z) ;
+}
+
+
 
 /* *** Operators *** */
 

owlps-positioner/point3d.hh

@@ -15,6 +15,8 @@
 #ifndef _OWLPS_POSITIONING_POINT3D_HH_
 #define _OWLPS_POSITIONING_POINT3D_HH_
 
+class Timestamp ;
+
 #include "posutil.hh"
 
 #include <string>
@@ -81,9 +83,14 @@ public:
 
   /** @name Other operations */
   //@{
-  /// Interpolates the coordinates of a Point3D
+  /// Interpolates the coordinates of a Point3D to compute intermediate
+  /// coordinates between two points
   void interpolate(const Point3D &end, float &step_hint,
                    std::vector<Point3D> &interpolated) const ;
+  /// Interpolates the coordinates of a Point3D according to the speed of
+  /// the mobile
+  Point3D interpolate(const Point3D &end, const float speed,
+                      const Timestamp &duration) const ;
   //@}
 
   /** @name Operators */