329 lines
8.9 KiB
C++
329 lines
8.9 KiB
C++
/*
|
|
* This file is part of the Owl Positioning System (OwlPS) project.
|
|
* It is subject to the copyright notice and license terms in the
|
|
* COPYRIGHT.t2t file found in the top-level directory of this
|
|
* distribution and at
|
|
* https://code.lm7.fr/mcy/owlps/src/master/COPYRIGHT.t2t
|
|
* No part of the OwlPS Project, including this file, may be copied,
|
|
* modified, propagated, or distributed except according to the terms
|
|
* contained in the COPYRIGHT.t2t file; the COPYRIGHT.t2t file must be
|
|
* distributed along with this file, either separately or by replacing
|
|
* this notice by the COPYRIGHT.t2t file's contents.
|
|
*/
|
|
|
|
|
|
#include "point3d.hh"
|
|
#include "timestamp.hh"
|
|
#include "posutil.hh"
|
|
#include "posexcept.hh"
|
|
|
|
#include <sstream>
|
|
|
|
// For interpolate()
|
|
#include <claw/tween/tweener_group.hpp>
|
|
#include <claw/tween/single_tweener.hpp>
|
|
#include <claw/tween/easing/easing_linear.hpp>
|
|
|
|
using namespace std ;
|
|
|
|
|
|
|
|
/* *** Constructors *** */
|
|
|
|
|
|
Point3D::Point3D(const string &source)
|
|
{
|
|
float pos[3] ;
|
|
istringstream iss(source) ;
|
|
|
|
for (int i = 0 ; i < 2 ; ++i)
|
|
{
|
|
iss >> pos[i] ;
|
|
if (iss.get() != ';')
|
|
throw malformed_input_data(
|
|
"Point3D(string): cannot extract coordinates!") ;
|
|
}
|
|
iss >> pos[2] ;
|
|
|
|
set_coordinates(pos) ;
|
|
}
|
|
|
|
|
|
|
|
/* *** Distance operations *** */
|
|
|
|
|
|
/**
|
|
* The distance is not square rooted after the computation, in order
|
|
* to optimise comparisons.
|
|
*/
|
|
float Point3D::square_distance_2d(const Point3D &source) const
|
|
{
|
|
return
|
|
(x - source.x) * (x - source.x) +
|
|
(y - source.y) * (y - source.y) ;
|
|
}
|
|
|
|
|
|
/**
|
|
* The distance is not square rooted after the computation, in order
|
|
* to optimise comparisons.
|
|
*/
|
|
float Point3D::square_distance(const Point3D &source) const
|
|
{
|
|
return
|
|
(x - source.x) * (x - source.x) +
|
|
(y - source.y) * (y - source.y) +
|
|
(z - source.z) * (z - source.z) ;
|
|
}
|
|
|
|
|
|
/**
|
|
* A, B and C are three points, A being the current Point3D (*this).
|
|
* This function computes the angle BÂC, in two dimensions (i.e. the Z
|
|
* coordinate of the points is ignored).
|
|
*
|
|
* If the points are aligned, the angle returned is always 0° (and not
|
|
* 180°, even in the case where A is on [BC]).
|
|
*
|
|
* @returns The angle BÂC, in the interval [0, 180[ degrees.
|
|
*/
|
|
double Point3D::angle_2d(const Point3D &b, const Point3D &c) const
|
|
{
|
|
double
|
|
sq_ab = square_distance_2d(b),
|
|
sq_ac = square_distance_2d(c),
|
|
sq_bc = b.square_distance_2d(c) ;
|
|
|
|
if (sq_ab == 0 || sq_ac == 0 || sq_bc == 0)
|
|
return 0 ;
|
|
|
|
double ab = sqrt(sq_ab) ;
|
|
double ac = sqrt(sq_ac) ;
|
|
|
|
double cos_bac = (sq_ab + sq_ac - sq_bc) / (2 * ab * ac) ;
|
|
double bac = acos(cos_bac) ;
|
|
|
|
return PosUtil::rad2deg(bac) ;
|
|
}
|
|
|
|
|
|
/**
|
|
* #x and #y are updated with the rotated image of the point.
|
|
*
|
|
* Note that as this functions does a 2D rotation, the c.z is ignored,
|
|
* and so is this->x.
|
|
*/
|
|
void Point3D::rotate_2d(const Point3D &c, float angle)
|
|
{
|
|
angle = PosUtil::deg2rad(angle) ;
|
|
|
|
float new_x = cos(angle) * (x-c.x) - sin(angle) * (y-c.y) + c.x ;
|
|
float new_y = sin(angle) * (x-c.x) + cos(angle) * (y-c.y) + c.y ;
|
|
|
|
x = new_x ;
|
|
y = new_y ;
|
|
}
|
|
|
|
|
|
|
|
/* *** Other operations *** */
|
|
|
|
|
|
/**
|
|
* This function computes the intermediate (linear) values between the
|
|
* current point and the point `end`.
|
|
*
|
|
* The number of steps is function of `step_hint` and the distance
|
|
* between the two points. `step_hint`, as its name indicates, is a hint
|
|
* of the desired distance between two intermediate coordinates. It is
|
|
* adjusted by the function to distribute equally the intermediate
|
|
* coordinates between the two extremities.
|
|
*
|
|
* @param[in] end Coordinate of the last extremity.
|
|
* @param[in,out] step_hint The desired approximate distance between two
|
|
* 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 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 (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
|
|
{
|
|
/**
|
|
* Example of step computation: if the distance between the two
|
|
* extremities (`*this` and `end`) is 12.42 m and `step_hint` is set
|
|
* to 2 m, the number of steps is 12 / 2 == 6, and the actual distance
|
|
* between two intermediate coordinates is 12.42 / 6 == 2.07 m.
|
|
*/
|
|
float dist = distance(end) ;
|
|
int nb_steps = dist / step_hint ;
|
|
step_hint = dist / nb_steps ;
|
|
|
|
/* 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 interpolated values */
|
|
do
|
|
{
|
|
// Update tweened_x, tweened_y, tweened_z
|
|
interpolator.update(step_hint);
|
|
// Add the new coordinates to the vector
|
|
interpolated.push_back(Point3D(tweened_x, tweened_y, tweened_z)) ;
|
|
}
|
|
while (! interpolator.is_finished()) ;
|
|
}
|
|
|
|
|
|
/**
|
|
* 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 *** */
|
|
|
|
|
|
Point3D& Point3D::operator=(const Point3D &source)
|
|
{
|
|
if (this == &source)
|
|
return *this ;
|
|
|
|
x = source.x ;
|
|
y = source.y ;
|
|
z = source.z ;
|
|
|
|
return *this ;
|
|
}
|
|
|
|
|
|
bool Point3D::operator==(const Point3D &source) const
|
|
{
|
|
if (this == &source)
|
|
return true ;
|
|
|
|
return
|
|
x == source.x &&
|
|
y == source.y &&
|
|
z == source.z ;
|
|
}
|
|
|
|
|
|
bool Point3D::operator<(const Point3D &source) const
|
|
{
|
|
if (x < source.x)
|
|
return true ;
|
|
if (x > source.x)
|
|
return false ;
|
|
|
|
if (y < source.y)
|
|
return true ;
|
|
if (y > source.y)
|
|
return false ;
|
|
|
|
if (z < source.z)
|
|
return true ;
|
|
|
|
return false ;
|
|
}
|
|
|
|
|
|
Point3D::operator std::string(void) const
|
|
{
|
|
ostringstream oss ;
|
|
oss << *this ;
|
|
return oss.str() ;
|
|
}
|
|
|
|
|
|
/**
|
|
* @returns `true` if either #x, #y or #z is non-zero.
|
|
* @returns `false` if #x, #y and #z are defined to 0.
|
|
*/
|
|
Point3D::operator bool(void) const
|
|
{
|
|
return
|
|
x != 0 ||
|
|
y != 0 ||
|
|
z != 0 ;
|
|
}
|
|
|
|
|
|
|
|
ostream& operator<<(ostream &os, const Point3D &p)
|
|
{
|
|
os << "(" << p.x << ";" << p.y << ";" << p.z << ")" ;
|
|
return os ;
|
|
}
|