Fix domain of acos in angle_between()

1 files changed, 7 insertions, 1 deletions

diff --git a/src/utils.h b/src/utils.h
index d9025fa0..7d8274bd 100644
--- a/src/utils.h
+++ b/src/utils.h
@@ -107,7 +107,13 @@ static inline double angle_between(double x1, double y1, double z1,
 {
 	double mod1 = modulus(x1, y1, z1);
 	double mod2 = modulus(x2, y2, z2);
-	return acos( (x1*x2 + y1*y2 + z1*z2) / (mod1*mod2) );
+	double cosine = (x1*x2 + y1*y2 + z1*z2) / (mod1*mod2);
+
+	/* Fix domain if outside due to rounding */
+	if ( cosine > 1.0 ) cosine = 1.0;
+	if ( cosine < -1.0 ) cosine = -1.0;
+
+	return acos(cosine);
 }