diff --git a/owlps-positioning/src/point3d.cc b/owlps-positioning/src/point3d.cc index bfc55f6..10c7652 100644 --- a/owlps-positioning/src/point3d.cc +++ b/owlps-positioning/src/point3d.cc @@ -64,17 +64,24 @@ float Point3D::square_distance(const float _x, /** * A, B and C are three points, A being the current Point3D (*this). - * @return The angle BÂC. + * 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(const Point3D &b, const Point3D &c) const { - float + double sq_ab = square_distance(b), - ab = sqrt(sq_ab), sq_ac = square_distance(c), - ac = sqrt(sq_ac), sq_bc = b.square_distance(c) ; + if (sq_ab == 0 || sq_ac == 0 || sq_bc == 0) + return 0 ; + + double + ab = sqrt(sq_ab), + ac = sqrt(sq_ac) ; + double cos_bac = (sq_ab + sq_ac - sq_bc) / (2 * ab * ac) ; double bac = acos(cos_bac) ;