Square stick output option for DS4Windows

squaresticktestds4.diff

diff --git a/DS4Windows/DS4Control/Mapping.cs b/DS4Windows/DS4Control/Mapping.cs
index 29a0bcd..e8158b3 100644
--- a/DS4Windows/DS4Control/Mapping.cs
+++ b/DS4Windows/DS4Control/Mapping.cs
@@ -75,6 +75,74 @@ namespace DS4Windows
             new Queue<ControlToXInput>(), new Queue<ControlToXInput>()
         };
 
+        struct DS4Vector2
+        {
+            public double x;
+            public double y;
+
+            public DS4Vector2(double x, double y)
+            {
+                this.x = x;
+                this.y = y;
+            }
+
+            public void CircleToSquare(in int device, double innerRoundness)
+            {
+                const double PiOverFour = Math.PI / 4;
+
+                // Determine the theta angle
+                double angle = Math.Atan2(-y, x) + Math.PI;
+                ref DS4Vector2 squared = ref tempSqrVects[device];
+                // Scale according to which wall we're clamping to
+                // X+ wall
+                if (angle <= PiOverFour || angle > 7.0 * PiOverFour)
+                {
+                    double tempVal = 1.0 / Math.Cos(angle);
+                    squared.x = x * tempVal;
+                    squared.y = y * tempVal;
+                }
+                // Y+ wall
+                else if (angle > PiOverFour && angle <= 3.0 * PiOverFour)
+                {
+                    double tempVal = 1.0 / Math.Sin(angle);
+                    squared.x = x * tempVal;
+                    squared.y = y * tempVal;
+                }
+                // X- wall
+                else if (angle > 3.0 * PiOverFour && angle <= 5.0 * PiOverFour)
+                {
+                    double tempVal = -1.0 / Math.Cos(angle);
+                    squared.x = x * tempVal;
+                    squared.y = y * tempVal;
+                }
+                // Y- wall
+                else if (angle > 5.0 * PiOverFour && angle <= 7.0 * PiOverFour)
+                {
+                    double tempVal = -1.0 / Math.Sin(angle);
+                    squared.x = x * tempVal;
+                    squared.y = y * tempVal;
+                }
+                else throw new InvalidOperationException("Invalid angle...?");
+
+                if (innerRoundness == 0d)
+                {
+                    x = squared.x;
+                    y = squared.y;
+                    return;
+                }
+
+                double length = Math.Sqrt((x * x) + (y * y));
+                double factor = Math.Pow(length, innerRoundness);
+                x = x + (squared.x - x) * factor;
+                y = y + (squared.y - y) * factor;
+            }
+        }
+
+        static DS4Vector2[] outSqrVects = new DS4Vector2[4] { new DS4Vector2(0.0, 0.0),
+        new DS4Vector2(0.0, 0.0), new DS4Vector2(0.0, 0.0), new DS4Vector2(0.0, 0.0)};
+        static DS4Vector2[] tempSqrVects = new DS4Vector2[4] { new DS4Vector2(0.0, 0.0),
+        new DS4Vector2(0.0, 0.0), new DS4Vector2(0.0, 0.0), new DS4Vector2(0.0, 0.0)};
+
         public static SyntheticState globalState = new SyntheticState();
         public static SyntheticState[] deviceState = new SyntheticState[4]
             { new SyntheticState(), new SyntheticState(), new SyntheticState(),
@@ -779,6 +847,22 @@ namespace DS4Windows
             if (r2Sens != 1.0)
                 dState.R2 = (byte)Global.Clamp(0, r2Sens * dState.R2, 255);
 
+            if (true)
+            {
+                double capX = dState.LX >= 128 ? 127.0 : 128.0;
+                double capY = dState.LY >= 128 ? 127.0 : 128.0;
+                double tempX = (dState.LX - 128.0) / capX;
+                double tempY = (dState.LY - 128.0) / capY;
+                ref DS4Vector2 tempVect = ref outSqrVects[device];
+                tempVect.x = tempX; tempVect.y = tempY;
+                tempVect.CircleToSquare(in device, 5.0);
+                tempVect.x = tempVect.x < -1.0 ? -1.0 : tempVect.x > 1.0 ? 1.0 : tempVect.x;
+                tempVect.y = tempVect.y < -1.0 ? -1.0 : tempVect.y > 1.0 ? 1.0 : tempVect.y;
+                //Console.WriteLine("Input ({0}) | Output ({1})", tempY, tempHooch.y);
+                dState.LX = (byte)(tempVect.x * capX + 128.0);
+                dState.LY = (byte)(tempVect.y * capY + 128.0);
+            }
+
             int lsOutCurveMode = lsOutCurveModeArray[device] = getLsOutCurveMode(device);
             if (lsOutCurveMode > 0)
             {