From f5a4f4fedc9f073abc06eb74f0a1a0132983d989 Mon Sep 17 00:00:00 2001
From: Matteo Cypriani <matteo.cypriani@univ-fcomte.fr>
Date: Mon, 18 Jul 2011 13:35:02 +0200
Subject: [PATCH] [Positioning] Point3D: fix angle()

Use doubles instead of floats.
Handle the cases where points are equal.
---
 owlps-positioning/src/point3d.cc | 15 +++++++++++----
 1 file changed, 11 insertions(+), 4 deletions(-)

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) ;