[Positioning] Point3D: fix angle()
Use doubles instead of floats. Handle the cases where points are equal.
This commit is contained in:
parent
33b027d472
commit
f5a4f4fedc
|
@ -64,17 +64,24 @@ float Point3D::square_distance(const float _x,
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* A, B and C are three points, A being the current Point3D (*this).
|
* 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
|
double Point3D::angle(const Point3D &b, const Point3D &c) const
|
||||||
{
|
{
|
||||||
float
|
double
|
||||||
sq_ab = square_distance(b),
|
sq_ab = square_distance(b),
|
||||||
ab = sqrt(sq_ab),
|
|
||||||
sq_ac = square_distance(c),
|
sq_ac = square_distance(c),
|
||||||
ac = sqrt(sq_ac),
|
|
||||||
sq_bc = b.square_distance(c) ;
|
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 cos_bac = (sq_ab + sq_ac - sq_bc) / (2 * ab * ac) ;
|
||||||
double bac = acos(cos_bac) ;
|
double bac = acos(cos_bac) ;
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue