2012-02-06 16:44:09 +01:00
|
|
|
/*
|
|
|
|
* This file is part of the Owl Positioning System (OwlPS).
|
|
|
|
* OwlPS is a project of the University of Franche-Comté
|
|
|
|
* (Université de Franche-Comté), France.
|
|
|
|
*/
|
|
|
|
|
|
|
|
|
2009-12-07 16:03:53 +01:00
|
|
|
#include "point3d.hh"
|
2011-07-13 15:00:54 +02:00
|
|
|
#include "posutil.hh"
|
2011-04-27 18:47:56 +02:00
|
|
|
#include "posexcept.hh"
|
2009-12-07 16:03:53 +01:00
|
|
|
|
2010-02-04 18:46:23 +01:00
|
|
|
#include <sstream>
|
2010-06-07 15:09:22 +02:00
|
|
|
#include <boost/functional/hash.hpp>
|
2009-12-07 16:03:53 +01:00
|
|
|
|
2011-04-27 18:47:56 +02:00
|
|
|
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) ;
|
|
|
|
}
|
|
|
|
|
2009-12-07 16:03:53 +01:00
|
|
|
|
|
|
|
|
2010-01-16 12:23:07 +01:00
|
|
|
/* *** Distance operations *** */
|
2009-12-07 16:03:53 +01:00
|
|
|
|
|
|
|
|
2012-04-27 15:28:55 +02:00
|
|
|
/**
|
|
|
|
* 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) ;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
2010-01-16 12:23:07 +01:00
|
|
|
/**
|
|
|
|
* The distance is not square rooted after the computation, in order
|
|
|
|
* to optimise comparisons.
|
2009-12-07 16:03:53 +01:00
|
|
|
*/
|
2010-02-03 13:40:27 +01:00
|
|
|
float Point3D::square_distance(const Point3D &source) const
|
2009-12-07 16:03:53 +01:00
|
|
|
{
|
|
|
|
return
|
2010-02-03 13:40:27 +01:00
|
|
|
(x - source.x) * (x - source.x) +
|
|
|
|
(y - source.y) * (y - source.y) +
|
|
|
|
(z - source.z) * (z - source.z) ;
|
2009-12-07 16:03:53 +01:00
|
|
|
}
|
|
|
|
|
|
|
|
|
2011-07-13 15:00:54 +02:00
|
|
|
/**
|
|
|
|
* A, B and C are three points, A being the current Point3D (*this).
|
2012-04-27 15:30:25 +02:00
|
|
|
* This function computes the angle BÂC, in two dimensions (i.e. the Z
|
|
|
|
* coordinate of the points is ignored).
|
|
|
|
*
|
2011-07-18 13:35:02 +02:00
|
|
|
* If the points are aligned, the angle returned is always 0° (and not
|
2012-04-27 15:30:25 +02:00
|
|
|
* 180°, even in the case where A is on [BC]).
|
|
|
|
*
|
|
|
|
* @returns The angle BÂC, in the interval [0, 180[ degrees.
|
2011-07-13 15:00:54 +02:00
|
|
|
*/
|
2012-04-27 15:30:25 +02:00
|
|
|
double Point3D::angle_2d(const Point3D &b, const Point3D &c) const
|
2011-07-13 15:00:54 +02:00
|
|
|
{
|
2011-07-18 13:35:02 +02:00
|
|
|
double
|
2012-04-27 15:30:25 +02:00
|
|
|
sq_ab = square_distance_2d(b),
|
|
|
|
sq_ac = square_distance_2d(c),
|
|
|
|
sq_bc = b.square_distance_2d(c) ;
|
2011-07-13 15:00:54 +02:00
|
|
|
|
2011-07-18 13:35:02 +02:00
|
|
|
if (sq_ab == 0 || sq_ac == 0 || sq_bc == 0)
|
|
|
|
return 0 ;
|
|
|
|
|
|
|
|
double
|
|
|
|
ab = sqrt(sq_ab),
|
|
|
|
ac = sqrt(sq_ac) ;
|
|
|
|
|
2011-07-13 15:00:54 +02:00
|
|
|
double cos_bac = (sq_ab + sq_ac - sq_bc) / (2 * ab * ac) ;
|
|
|
|
double bac = acos(cos_bac) ;
|
|
|
|
|
|
|
|
return PosUtil::rad2deg(bac) ;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
2012-04-30 09:58:30 +02:00
|
|
|
/**
|
|
|
|
* #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 ;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
2009-12-07 16:03:53 +01:00
|
|
|
|
2010-01-16 12:23:07 +01:00
|
|
|
/* *** Operators *** */
|
2009-12-07 16:03:53 +01:00
|
|
|
|
|
|
|
|
2011-07-30 20:38:42 +02:00
|
|
|
Point3D& Point3D::operator=(const Point3D &source)
|
2009-12-07 16:03:53 +01:00
|
|
|
{
|
2010-02-03 13:40:27 +01:00
|
|
|
if (this == &source)
|
2009-12-07 16:03:53 +01:00
|
|
|
return *this ;
|
|
|
|
|
2010-02-03 13:40:27 +01:00
|
|
|
x = source.x ;
|
|
|
|
y = source.y ;
|
|
|
|
z = source.z ;
|
2009-12-07 16:03:53 +01:00
|
|
|
|
|
|
|
return *this ;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
2010-02-03 13:40:27 +01:00
|
|
|
bool Point3D::operator==(const Point3D &source) const
|
2009-12-07 16:03:53 +01:00
|
|
|
{
|
2010-02-03 13:40:27 +01:00
|
|
|
if (this == &source)
|
2009-12-11 17:56:31 +01:00
|
|
|
return true ;
|
|
|
|
|
2009-12-07 16:03:53 +01:00
|
|
|
return
|
2010-02-03 13:40:27 +01:00
|
|
|
x == source.x &&
|
|
|
|
y == source.y &&
|
|
|
|
z == source.z ;
|
2009-12-07 16:03:53 +01:00
|
|
|
}
|
|
|
|
|
|
|
|
|
2010-02-03 13:40:27 +01:00
|
|
|
bool Point3D::operator<(const Point3D &source) const
|
2009-12-07 16:03:53 +01:00
|
|
|
{
|
2010-02-03 13:40:27 +01:00
|
|
|
if (x < source.x)
|
2009-12-07 16:03:53 +01:00
|
|
|
return true ;
|
2010-02-03 13:40:27 +01:00
|
|
|
if (x > source.x)
|
2009-12-07 16:03:53 +01:00
|
|
|
return false ;
|
|
|
|
|
2010-02-03 13:40:27 +01:00
|
|
|
if (y < source.y)
|
2009-12-07 16:03:53 +01:00
|
|
|
return true ;
|
2010-02-03 13:40:27 +01:00
|
|
|
if (y > source.y)
|
2009-12-07 16:03:53 +01:00
|
|
|
return false ;
|
|
|
|
|
2010-02-03 13:40:27 +01:00
|
|
|
if (z < source.z)
|
2009-12-07 16:03:53 +01:00
|
|
|
return true ;
|
|
|
|
|
|
|
|
return false ;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
2011-07-18 13:34:02 +02:00
|
|
|
Point3D::operator std::string(void) const
|
2010-02-04 18:46:23 +01:00
|
|
|
{
|
2011-04-27 18:47:56 +02:00
|
|
|
ostringstream oss ;
|
2010-02-04 18:46:23 +01:00
|
|
|
oss << *this ;
|
|
|
|
return oss.str() ;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
2011-05-04 18:16:10 +02:00
|
|
|
/**
|
|
|
|
* @return \em true if either #x, #y or #z is non-zero.
|
|
|
|
* @return \em false if #x, #y and #z are defined to 0.
|
|
|
|
*/
|
|
|
|
Point3D::operator bool(void) const
|
|
|
|
{
|
|
|
|
return
|
2012-06-20 17:26:00 +02:00
|
|
|
x != 0 ||
|
|
|
|
y != 0 ||
|
2011-05-04 18:16:10 +02:00
|
|
|
z != 0 ;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
2009-12-07 16:03:53 +01:00
|
|
|
|
2011-04-27 18:47:56 +02:00
|
|
|
ostream& operator<<(ostream &os, const Point3D &p)
|
2009-12-07 16:03:53 +01:00
|
|
|
{
|
|
|
|
os << "(" << p.x << ";" << p.y << ";" << p.z << ")" ;
|
|
|
|
return os ;
|
|
|
|
}
|
2010-06-07 15:09:22 +02:00
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
size_t hash_value(const Point3D &source)
|
|
|
|
{
|
|
|
|
size_t seed = 0 ;
|
|
|
|
|
|
|
|
boost::hash_combine(seed, source.x) ;
|
|
|
|
boost::hash_combine(seed, source.y) ;
|
|
|
|
boost::hash_combine(seed, source.z) ;
|
|
|
|
|
|
|
|
return seed ;
|
|
|
|
}
|