[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.
This commit is contained in:
parent
52940bcfc3
commit
1a40fe3366
|
@ -13,6 +13,7 @@
|
||||||
|
|
||||||
|
|
||||||
#include "point3d.hh"
|
#include "point3d.hh"
|
||||||
|
#include "timestamp.hh"
|
||||||
#include "posutil.hh"
|
#include "posutil.hh"
|
||||||
#include "posexcept.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
|
* intermediate points; when the function returns, it is set to the
|
||||||
* actual step used.
|
* actual step used.
|
||||||
* @param[out] interpolated The vector in which the intermediate values
|
* @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
|
* 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,
|
void Point3D::interpolate(const Point3D &end, float &step_hint,
|
||||||
std::vector<Point3D> &interpolated) const
|
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 *** */
|
/* *** Operators *** */
|
||||||
|
|
||||||
|
|
|
@ -15,6 +15,8 @@
|
||||||
#ifndef _OWLPS_POSITIONING_POINT3D_HH_
|
#ifndef _OWLPS_POSITIONING_POINT3D_HH_
|
||||||
#define _OWLPS_POSITIONING_POINT3D_HH_
|
#define _OWLPS_POSITIONING_POINT3D_HH_
|
||||||
|
|
||||||
|
class Timestamp ;
|
||||||
|
|
||||||
#include "posutil.hh"
|
#include "posutil.hh"
|
||||||
|
|
||||||
#include <string>
|
#include <string>
|
||||||
|
@ -81,9 +83,14 @@ public:
|
||||||
|
|
||||||
/** @name Other operations */
|
/** @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,
|
void interpolate(const Point3D &end, float &step_hint,
|
||||||
std::vector<Point3D> &interpolated) const ;
|
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 */
|
/** @name Operators */
|
||||||
|
|
Loading…
Reference in New Issue