From 58aa2d5cec068e914c4815d733cfde63cb047a52 Mon Sep 17 00:00:00 2001
From: Paddle <PaddleStroke@users.noreply.github.com>
Date: Fri, 3 Nov 2023 18:46:42 +0100
Subject: [PATCH] Adds areColinear function to Utils.

---
 src/Mod/Sketcher/Gui/Utils.cpp | 35 ++++++++++++++++++++++++++++++++++
 src/Mod/Sketcher/Gui/Utils.h   |  2 ++
 2 files changed, 37 insertions(+)

diff --git a/src/Mod/Sketcher/Gui/Utils.cpp b/src/Mod/Sketcher/Gui/Utils.cpp
index c6f24d716c..0db4a47d12 100644
--- a/src/Mod/Sketcher/Gui/Utils.cpp
+++ b/src/Mod/Sketcher/Gui/Utils.cpp
@@ -801,3 +801,38 @@ std::string SketcherGui::angleToDisplayFormat(double value, int digits)
     QString numericPart = matched.left(requiredLength);
     return Base::Tools::toStdString(numericPart + qUnitString);
 }
+
+
+bool SketcherGui::areColinear(const Base::Vector2d& p1,
+                              const Base::Vector2d& p2,
+                              const Base::Vector2d& p3)
+{
+    Base::Vector2d u = p2 - p1;
+    Base::Vector2d v = p3 - p2;
+    Base::Vector2d w = p1 - p3;
+
+    double uu = u * u;
+    double vv = v * v;
+    double ww = w * w;
+
+    double eps2 = Precision::SquareConfusion();
+    if (uu < eps2 || vv < eps2 || ww < eps2) {
+        return true;
+    }
+
+    double uv = -(u * v);
+    double vw = -(v * w);
+    double uw = -(u * w);
+
+    double w0 = (2 * sqrt(abs(uu * ww - uw * uw)) * uw / (uu * ww));
+    double w1 = (2 * sqrt(abs(uu * vv - uv * uv)) * uv / (uu * vv));
+    double w2 = (2 * sqrt(abs(vv * ww - vw * vw)) * vw / (vv * ww));
+
+    double wx = w0 + w1 + w2;
+
+    if (abs(wx) < Precision::Confusion()) {
+        return true;
+    }
+
+    return false;
+}
diff --git a/src/Mod/Sketcher/Gui/Utils.h b/src/Mod/Sketcher/Gui/Utils.h
index 6f2cd126f0..0d5dd5c073 100644
--- a/src/Mod/Sketcher/Gui/Utils.h
+++ b/src/Mod/Sketcher/Gui/Utils.h
@@ -201,6 +201,8 @@ bool useSystemDecimals();
 std::string lengthToDisplayFormat(double value, int digits);
 std::string angleToDisplayFormat(double value, int digits);
 
+bool areColinear(const Base::Vector2d& p1, const Base::Vector2d& p2, const Base::Vector2d& p3);
+
 }  // namespace SketcherGui
 
 /// converts a 2D vector into a 3D vector in the XY plane