From b9ca5aa700e8a879824e18173b1e809e9e4dd1a9 Mon Sep 17 00:00:00 2001
From: Stefan Besler <stefan@besler.me>
Date: Sat, 20 Apr 2024 17:27:00 +0200
Subject: [PATCH] fix(#22): fixed div0 exceptions

---
 .../Struckig/Struckig/Math/Roots.TcPOU        | 34 +++++++++++++++----
 .../Position/PositionThirdOrderStep2.TcPOU    |  6 ++--
 2 files changed, 30 insertions(+), 10 deletions(-)

diff --git a/Struckig/Struckig/Struckig/Struckig/Math/Roots.TcPOU b/Struckig/Struckig/Struckig/Struckig/Math/Roots.TcPOU
index 09e699a..e81cedd 100644
--- a/Struckig/Struckig/Struckig/Struckig/Math/Roots.TcPOU
+++ b/Struckig/Struckig/Struckig/Struckig/Math/Roots.TcPOU
@@ -187,17 +187,20 @@ IF (ABS(a) < Constants.DoubleEpsilon)
 THEN
   IF (ABS(b) < Constants.DoubleEpsilon)
   THEN
-    // Linear equation
-    h := (-d / c);
-    IF ABS(c) > Constants.DoubleEpsilon AND_THEN h >= 0
+    IF ABS(c) > Constants.DoubleEpsilon
     THEN
-      roots[rootCount+1] := h;
-      rootCount := rootCount + 1;
-    END_IF
+      // Linear equation
+      h := (-d / c);
+      IF ABS(c) > Constants.DoubleEpsilon AND_THEN h >= 0
+      THEN
+        roots[rootCount+1] := h;
+        rootCount := rootCount + 1;
+      END_IF
+    END_IF    
   ELSE
     // Quadratic equation
     discriminant := c * c - 4 * b * d;
-    IF (discriminant >= 0)
+    IF discriminant >= 0 AND_THEN ABS(b) > Constants.DoubleEpsilon
     THEN
       inv2b := 1.0 / (2 * b);
       y := SQRT(discriminant);
@@ -393,15 +396,32 @@ IF (ABS(dd) < Constants.DoubleEpsilon) THEN
   IF (ABS(dd) < Constants.DoubleEpsilon) THEN
     p1 := p2 := a / 2;
   ELSE
+    
+    IF dd < 0
+    THEN
+      RETURN;
+    END_IF
+    
     sqrtD := SQRT(dd);
     p1 := (a + sqrtD) / 2;
     p2 := (a - sqrtD) / 2;
   END_IF
 ELSE
+  IF dd < 0
+  THEN
+    RETURN;
+  END_IF
+	
   sqrtD := SQRT(dd);
   q1 := (y + sqrtD) / 2;
   q2 := (y - sqrtD) / 2;
   // g1 + g2 = a AND_THEN g1*h2 + g2*h1 = c   ( AND_THEN g === p )  Krammer
+  
+  IF ABS(q1 - q2) < Constants.DoubleEpsilon
+  THEN
+    RETURN;
+  END_IF
+  
   p1 := (a * q1 - c) / (q1 - q2);
   p2 := (c - a * q2) / (q1 - q2);
 END_IF
diff --git a/Struckig/Struckig/Struckig/Struckig/Position/PositionThirdOrderStep2.TcPOU b/Struckig/Struckig/Struckig/Struckig/Position/PositionThirdOrderStep2.TcPOU
index 5c6e769..8e64135 100644
--- a/Struckig/Struckig/Struckig/Struckig/Position/PositionThirdOrderStep2.TcPOU
+++ b/Struckig/Struckig/Struckig/Struckig/Position/PositionThirdOrderStep2.TcPOU
@@ -883,7 +883,7 @@ DO
   IF t < t_min OR_ELSE t > t_max
   THEN
     CONTINUE;
-	END_IF
+  END_IF
 
   // Single Newton step (regarding pd)
   h0 := jMax*(2*t - tf) - ad;
@@ -1022,7 +1022,7 @@ DO
   IF t > tf OR_ELSE t > (aMax - a0)/jMax
   THEN
     CONTINUE;
-	END_IF
+  END_IF
 
   h1 := ad_ad/(2*jMax_jMax) + (a0*(t + tf) - af*t + jMax*t*tf - vd)/jMax;
   IF h1 >= 0
@@ -1103,7 +1103,7 @@ Roots.SolveCub(ADR(polynom), rootCount:=d_extremasCount, roots:=d_extremas);
 FOR i:=0 TO d_extremasCount
 DO
   t := d_extremas[i];
-  IF t <= 0.0 OR_ELSE ABS(t-tf) > Constants.DoubleEpsilon OR_ELSE ad = 0
+  IF ABS(t) < Constants.DoubleEpsilon OR_ELSE t > tf OR_ELSE ABS(t-tf) < Constants.DoubleEpsilon
   THEN
     CONTINUE;
   END_IF