From 037045e86688229fae8adb7539258f271a756312 Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Wed, 1 Jul 2020 17:08:36 +0200
Subject: [PATCH] Implement angle between three points

---
 CHANGELOG.md |  1 +
 src/bcar.cc  | 12 +++++++++++-
 2 files changed, 12 insertions(+), 1 deletion(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 667803b..2230340 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -11,6 +11,7 @@ The format is based on [Keep a Changelog][] and this project adheres to
 ### Added
 - Rotation of BicycleCar around the point.
 - Circle-line intersection.
+- Angle between three points computation.
 
 ### Changed
 - When set up BicycleCar heading, set the interval to `[-pi, +pi]`.
diff --git a/src/bcar.cc b/src/bcar.cc
index 99a3e18..0db57a3 100644
--- a/src/bcar.cc
+++ b/src/bcar.cc
@@ -365,5 +365,15 @@ angle_between_three_points(
         double x2, double y2,
         double x3, double y3
 ) {
-        return 0;
+        double d1x = x2 - x1;
+        double d1y = y2 - y1;
+        double d2x = x3 - x2;
+        double d2y = y3 - y2;
+
+        double dot = d1x*d2x + d1y*d2y;
+        double d1 = sqrt(d1x*d1x + d1y*d1y);
+        double d2 = sqrt(d2x*d2x + d2y*d2y);
+
+        double delta = acos(dot / (d1 * d2));
+        return std::min(delta, M_PI - delta);
 }
-- 
2.39.2