diff --git a/owlps-positioner/point3d.cc b/owlps-positioner/point3d.cc index 666caec..66f688a 100644 --- a/owlps-positioner/point3d.cc +++ b/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 &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(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 *** */ diff --git a/owlps-positioner/point3d.hh b/owlps-positioner/point3d.hh index 56f77c0..c4380e7 100644 --- a/owlps-positioner/point3d.hh +++ b/owlps-positioner/point3d.hh @@ -15,6 +15,8 @@ #ifndef _OWLPS_POSITIONING_POINT3D_HH_ #define _OWLPS_POSITIONING_POINT3D_HH_ +class Timestamp ; + #include "posutil.hh" #include @@ -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 &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 */