diff --git a/owlps-positioning/Makefile b/owlps-positioning/Makefile index aa875cf..b508a0b 100644 --- a/owlps-positioning/Makefile +++ b/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 \ diff --git a/owlps-positioning/src/point3d.cc b/owlps-positioning/src/point3d.cc index 00d3606..bfc55f6 100644 --- a/owlps-positioning/src/point3d.cc +++ b/owlps-positioning/src/point3d.cc @@ -1,4 +1,5 @@ #include "point3d.hh" +#include "posutil.hh" #include "posexcept.hh" #include @@ -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 *** */ diff --git a/owlps-positioning/src/point3d.hh b/owlps-positioning/src/point3d.hh index 96300ab..54c536d 100644 --- a/owlps-positioning/src/point3d.hh +++ b/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 ¢re, const float radius) const ; + /// Angle BÂC (A being *this) + double angle(const Point3D &b, const Point3D &c) const ; //@} /** @name Operators */