[Positioning] Add Point3D::angle()

owlps-positioning/Makefile

@@ -139,6 +139,7 @@ $(OBJ_DIR)/%.o: $(SRC_DIR)/%.cc $(SRC_DIR)/%.hh
 
 # Dependencies
 $(OBJ_DIR)/point3d.o: \
+	$(OBJ_DIR)/posutil.o \
 	$(OBJ_DIR)/posexcept.o
 $(OBJ_DIR)/posutil.o: \
 	$(OBJ_DIR)/stock.o \

owlps-positioning/src/point3d.cc

@@ -1,4 +1,5 @@
 #include "point3d.hh"
+#include "posutil.hh"
 #include "posexcept.hh"
 
 #include <sstream>
@@ -61,6 +62,26 @@ 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.
+ */
+double Point3D::angle(const Point3D &b, const Point3D &c) const
+{
+  float
+  sq_ab = square_distance(b),
+  ab = sqrt(sq_ab),
+  sq_ac = square_distance(c),
+  ac = sqrt(sq_ac),
+  sq_bc = b.square_distance(c) ;
+
+  double cos_bac = (sq_ab + sq_ac - sq_bc) / (2 * ab * ac) ;
+  double bac = acos(cos_bac) ;
+
+  return PosUtil::rad2deg(bac) ;
+}
+
+
 
 /* *** Operators *** */
 

owlps-positioning/src/point3d.hh

@@ -44,7 +44,7 @@ public:
   void set_coordinates(const Point3D &source) ;
   //@}
 
-  /** @name Distance operations */
+  /** @name Distance & angles operations */
   //@{
   /// Square euclidean distance to a Point3D
   float square_distance(const Point3D &p) const ;
@@ -61,6 +61,8 @@ public:
   /// Euclidean distance to the radius of a sphere
   float distance_to_sphere(const Point3D &centre,
                            const float radius) const ;
+  /// Angle BÂC (A being *this)
+  double angle(const Point3D &b, const Point3D &c) const ;
   //@}
 
   /** @name Operators */