Implemented square stick functionality

Related to issue #341.

DS4Windows/DS4Control/Mapping.cs

@@ -79,6 +79,88 @@ 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;
+            }
+        }
+
+        class DS4SquareStick
+        {
+            public DS4Vector2 current;
+            public DS4Vector2 squared;
+
+            public DS4SquareStick()
+            {
+                current = new DS4Vector2(0.0, 0.0);
+                squared = new DS4Vector2(0.0, 0.0);
+            }
+
+            public void CircleToSquare()
+            {
+                const double PiOverFour = Math.PI / 4.0;
+                const double roundness = 5.0;
+
+                // Determine the theta angle
+                double angle = Math.Atan2(current.y, -current.x);
+                angle += Math.PI;
+                double cosAng = Math.Cos(angle);
+                // Scale according to which wall we're clamping to
+                // X+ wall
+                if (angle <= PiOverFour || angle > 7.0 * PiOverFour)
+                {
+                    double tempVal = 1.0 / cosAng;
+                    //Console.WriteLine("1 ANG: {0} | TEMP: {1}", angle, tempVal);
+                    squared.x = current.x * tempVal;
+                    squared.y = current.y * tempVal;
+                }
+                // Y+ wall
+                else if (angle > PiOverFour && angle <= 3.0 * PiOverFour)
+                {
+                    double tempVal = 1.0 / Math.Sin(angle);
+                    //Console.WriteLine("2 ANG: {0} | TEMP: {1}", angle, tempVal);
+                    squared.x = current.x * tempVal;
+                    squared.y = current.y * tempVal;
+                }
+                // X- wall
+                else if (angle > 3.0 * PiOverFour && angle <= 5.0 * PiOverFour)
+                {
+                    double tempVal = -1.0 / cosAng;
+                    //Console.WriteLine("3 ANG: {0} | TEMP: {1}", angle, tempVal);
+                    squared.x = current.x * tempVal;
+                    squared.y = current.y * tempVal;
+                }
+                // Y- wall
+                else if (angle > 5.0 * PiOverFour && angle <= 7.0 * PiOverFour)
+                {
+                    double tempVal = -1.0 / Math.Sin(angle);
+                    //Console.WriteLine("4 ANG: {0} | TEMP: {1}", angle, tempVal);
+                    squared.x = current.x * tempVal;
+                    squared.y = current.y * tempVal;
+                }
+                else return;
+
+                //double lengthOld = Math.Sqrt((x * x) + (y * y));
+                double length = current.x / cosAng;
+                //Console.WriteLine("LENGTH TEST ({0}) ({1}) {2}", lengthOld, length, (lengthOld == length).ToString());
+                double factor = Math.Pow(length, roundness);
+                //double ogX = current.x, ogY = current.y;
+                current.x += (squared.x - current.x) * factor;
+                current.y += (squared.y - current.y) * factor;
+                //Console.WriteLine("INPUT: {0} {1} | {2} {3} | {4} {5} | {6} {7}",
+                //    ogX, ogY, current.x, current.y, squared.x, squared.y, length, factor);
+            }
+        }
+
+        static DS4SquareStick[] outSqrStk = new DS4SquareStick[4] { new DS4SquareStick(),
+        new DS4SquareStick(), new DS4SquareStick(), new DS4SquareStick()};
+
         static ReaderWriterLockSlim syncStateLock = new ReaderWriterLockSlim();
 
         public static SyntheticState globalState = new SyntheticState();
@@ -788,6 +870,24 @@ namespace DS4Windows
             if (r2Sens != 1.0)
                 dState.R2 = (byte)Global.Clamp(0, r2Sens * dState.R2, 255);
 
+            if (getSquareStickLS(device))
+            {
+                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;
+                DS4SquareStick sqstick = outSqrStk[device];
+                sqstick.current.x = tempX; sqstick.current.y = tempY;
+                sqstick.CircleToSquare();
+                //Console.WriteLine("Input ({0}) | Output ({1})", tempY, sqstick.current.y);
+                tempX = sqstick.current.x < -1.0 ? -1.0 : sqstick.current.x > 1.0
+                    ? 1.0 : sqstick.current.x;
+                tempY = sqstick.current.y < -1.0 ? -1.0 : sqstick.current.y > 1.0
+                    ? 1.0 : sqstick.current.y;
+                dState.LX = (byte)(tempX * capX + 128.0);
+                dState.LY = (byte)(tempY * capY + 128.0);
+            }
+
             int lsOutCurveMode = lsOutCurveModeArray[device] = getLsOutCurveMode(device);
             if (lsOutCurveMode > 0)
             {
@@ -859,6 +959,24 @@ namespace DS4Windows
                 }
             }
 
+            if (getSquareStickRS(device))
+            {
+                double capX = dState.RX >= 128 ? 127.0 : 128.0;
+                double capY = dState.RY >= 128 ? 127.0 : 128.0;
+                double tempX = (dState.RX - 128.0) / capX;
+                double tempY = (dState.RY - 128.0) / capY;
+                DS4SquareStick sqstick = outSqrStk[device];
+                sqstick.current.x = tempX; sqstick.current.y = tempY;
+                sqstick.CircleToSquare();
+                tempX = sqstick.current.x < -1.0 ? -1.0 : sqstick.current.x > 1.0
+                    ? 1.0 : sqstick.current.x;
+                tempY = sqstick.current.y < -1.0 ? -1.0 : sqstick.current.y > 1.0
+                    ? 1.0 : sqstick.current.y;
+                //Console.WriteLine("Input ({0}) | Output ({1})", tempY, sqstick.current.y);
+                dState.RX = (byte)(tempX * capX + 128.0);
+                dState.RY = (byte)(tempY * capY + 128.0);
+            }
+
             int rsOutCurveMode = rsOutCurveModeArray[device] = getRsOutCurveMode(device);
             if (rsOutCurveMode > 0)
             {

DS4Windows/DS4Control/ScpUtil.cs

@@ -1059,6 +1059,18 @@ namespace DS4Windows
             return m_Config.btPollRate[index];
         }
 
+        public static bool[] squareStickLS => m_Config.sqLSStickMode;
+        public static bool getSquareStickLS(int device)
+        {
+            return m_Config.sqLSStickMode[device];
+        }
+
+        public static bool[] squareStickRS => m_Config.sqRSStickMode;
+        public static bool getSquareStickRS(int device)
+        {
+            return m_Config.sqRSStickMode[device];
+        }
+
         public static int[] lsOutCurveMode => m_Config.lsOutCurveMode;
         public static int getLsOutCurveMode(int index)
         {
@@ -1500,6 +1512,8 @@ namespace DS4Windows
             MouseCursor.GYRO_MOUSE_DEADZONE };
         public bool[] gyroMouseToggle = new bool[5] { false, false, false,
             false, false };
+        public bool[] sqLSStickMode = new bool[5] { false, false, false, false, false };
+        public bool[] sqRSStickMode = new bool[5] { false, false, false, false, false };
         public int[] lsOutCurveMode = new int[5] { 0, 0, 0, 0, 0 };
         public int[] rsOutCurveMode = new int[5] { 0, 0, 0, 0, 0 };
         public int[] l2OutCurveMode = new int[5] { 0, 0, 0, 0, 0 };
@@ -1847,6 +1861,9 @@ namespace DS4Windows
                 XmlNode xmlLsOutputCurveMode = m_Xdoc.CreateNode(XmlNodeType.Element, "LSOutputCurveMode", null); xmlLsOutputCurveMode.InnerText = stickOutputCurveString(lsOutCurveMode[device]); Node.AppendChild(xmlLsOutputCurveMode);
                 XmlNode xmlRsOutputCurveMode = m_Xdoc.CreateNode(XmlNodeType.Element, "RSOutputCurveMode", null); xmlRsOutputCurveMode.InnerText = stickOutputCurveString(rsOutCurveMode[device]); Node.AppendChild(xmlRsOutputCurveMode);
 
+                XmlNode xmlLsSquareStickMode = m_Xdoc.CreateNode(XmlNodeType.Element, "LSSquareStick", null); xmlLsSquareStickMode.InnerText = sqLSStickMode[device].ToString(); Node.AppendChild(xmlLsSquareStickMode);
+                XmlNode xmlRsSquareStickMode = m_Xdoc.CreateNode(XmlNodeType.Element, "RSSquareStick", null); xmlRsSquareStickMode.InnerText = sqRSStickMode[device].ToString(); Node.AppendChild(xmlRsSquareStickMode);
+
                 XmlNode xmlL2OutputCurveMode = m_Xdoc.CreateNode(XmlNodeType.Element, "L2OutputCurveMode", null); xmlL2OutputCurveMode.InnerText = axisOutputCurveString(l2OutCurveMode[device]); Node.AppendChild(xmlL2OutputCurveMode);
                 XmlNode xmlR2OutputCurveMode = m_Xdoc.CreateNode(XmlNodeType.Element, "R2OutputCurveMode", null); xmlR2OutputCurveMode.InnerText = axisOutputCurveString(r2OutCurveMode[device]); Node.AppendChild(xmlR2OutputCurveMode);
 
@@ -2770,6 +2787,12 @@ namespace DS4Windows
                 try { Item = m_Xdoc.SelectSingleNode("/" + rootname + "/RSOutputCurveMode"); rsOutCurveMode[device] = stickOutputCurveId(Item.InnerText); }
                 catch { rsOutCurveMode[device] = 0; missingSetting = true; }
 
+                try { Item = m_Xdoc.SelectSingleNode("/" + rootname + "/LSSquareStick"); bool.TryParse(Item.InnerText, out sqLSStickMode[device]); }
+                catch { sqLSStickMode[device] = false; missingSetting = true; }
+
+                try { Item = m_Xdoc.SelectSingleNode("/" + rootname + "/RSSquareStick"); bool.TryParse(Item.InnerText, out sqRSStickMode[device]); }
+                catch { sqRSStickMode[device] = false; missingSetting = true; }
+
                 try { Item = m_Xdoc.SelectSingleNode("/" + rootname + "/L2OutputCurveMode"); l2OutCurveMode[device] = axisOutputCurveId(Item.InnerText); }
                 catch { l2OutCurveMode[device] = 0; missingSetting = true; }
 
@@ -4005,6 +4028,8 @@ namespace DS4Windows
             gyroSmoothing[device] = false;
             gyroSmoothWeight[device] = 0.5;
             gyroMouseHorizontalAxis[device] = 0;
+            sqLSStickMode[device] = false;
+            sqRSStickMode[device] = false;
             lsOutCurveMode[device] = 0;
             rsOutCurveMode[device] = 0;
             l2OutCurveMode[device] = 0;

DS4Windows/DS4Forms/Options.Designer.cs

@@ -397,6 +397,9 @@
             this.shareTouchInvStripMenuItem = new System.Windows.Forms.ToolStripMenuItem();
             this.psTouchInvStripMenuItem = new System.Windows.Forms.ToolStripMenuItem();
             this.advColorDialog = new DS4Windows.AdvancedColorDialog();
+            this.squStickTabPage = new System.Windows.Forms.TabPage();
+            this.lsSquStickCk = new System.Windows.Forms.CheckBox();
+            this.rsSquStickCk = new System.Windows.Forms.CheckBox();
             ((System.ComponentModel.ISupportInitialize)(this.nUDRainbow)).BeginInit();
             ((System.ComponentModel.ISupportInitialize)(this.tBBlueBar)).BeginInit();
             ((System.ComponentModel.ISupportInitialize)(this.tBGreenBar)).BeginInit();
@@ -491,6 +494,7 @@
             ((System.ComponentModel.ISupportInitialize)(this.nUDSZS)).BeginInit();
             this.cMGyroTriggers.SuspendLayout();
             this.cMTouchDisableInvert.SuspendLayout();
+            this.squStickTabPage.SuspendLayout();
             this.SuspendLayout();
             // 
             // lowColorChooserButton
@@ -2826,6 +2830,7 @@
             this.tCSens.Controls.Add(this.tPDeadzone);
             this.tCSens.Controls.Add(this.antiDeadzoneTabPage);
             this.tCSens.Controls.Add(this.maxZoneTabPage);
+            this.tCSens.Controls.Add(this.squStickTabPage);
             this.tCSens.Controls.Add(this.tPOutCurve);
             this.tCSens.Controls.Add(this.tPCurve);
             this.tCSens.Controls.Add(this.tpRotation);
@@ -4227,6 +4232,28 @@
             resources.ApplyResources(this.psTouchInvStripMenuItem, "psTouchInvStripMenuItem");
             this.psTouchInvStripMenuItem.CheckedChanged += new System.EventHandler(this.TouchDisableInvert_CheckedChanged);
             // 
+            // squStickTabPage
+            // 
+            this.squStickTabPage.Controls.Add(this.rsSquStickCk);
+            this.squStickTabPage.Controls.Add(this.lsSquStickCk);
+            resources.ApplyResources(this.squStickTabPage, "squStickTabPage");
+            this.squStickTabPage.Name = "squStickTabPage";
+            this.squStickTabPage.UseVisualStyleBackColor = true;
+            // 
+            // lsSquStickCk
+            // 
+            resources.ApplyResources(this.lsSquStickCk, "lsSquStickCk");
+            this.lsSquStickCk.Name = "lsSquStickCk";
+            this.lsSquStickCk.UseVisualStyleBackColor = true;
+            this.lsSquStickCk.Click += new System.EventHandler(this.lsSquStickCk_Click);
+            // 
+            // rsSquStickCk
+            // 
+            resources.ApplyResources(this.rsSquStickCk, "rsSquStickCk");
+            this.rsSquStickCk.Name = "rsSquStickCk";
+            this.rsSquStickCk.UseVisualStyleBackColor = true;
+            this.rsSquStickCk.Click += new System.EventHandler(this.rsSquStickCk_Click);
+            // 
             // Options
             // 
             resources.ApplyResources(this, "$this");
@@ -4294,6 +4321,7 @@
             this.pnlLSTrack.ResumeLayout(false);
             this.pnlRSTrack.ResumeLayout(false);
             this.fLPTiltControls.ResumeLayout(false);
+            this.fLPTiltControls.PerformLayout();
             this.tCControls.ResumeLayout(false);
             this.tPControls.ResumeLayout(false);
             this.pnlController.ResumeLayout(false);
@@ -4350,6 +4378,7 @@
             ((System.ComponentModel.ISupportInitialize)(this.nUDSZS)).EndInit();
             this.cMGyroTriggers.ResumeLayout(false);
             this.cMTouchDisableInvert.ResumeLayout(false);
+            this.squStickTabPage.ResumeLayout(false);
             this.ResumeLayout(false);
 
         }
@@ -4725,5 +4754,8 @@
         private System.Windows.Forms.Label label27;
         private System.Windows.Forms.NumericUpDown gyroMouseDzNUD;
         private System.Windows.Forms.CheckBox toggleGyroMCb;
+        private System.Windows.Forms.TabPage squStickTabPage;
+        private System.Windows.Forms.CheckBox rsSquStickCk;
+        private System.Windows.Forms.CheckBox lsSquStickCk;
     }
 }

DS4Windows/DS4Forms/Options.cs

@@ -631,6 +631,9 @@ namespace DS4Windows
                     btnBrowse.Text = Path.GetFileNameWithoutExtension(LaunchProgram[device]);
                 }
 
+                lsSquStickCk.Checked = squareStickLS[device];
+                rsSquStickCk.Checked = squareStickRS[device];
+
                 cBDinput.Checked = DinputOnly[device];
                 olddinputcheck = cBDinput.Checked;
                 cbStartTouchpadOff.Checked = StartTouchpadOff[device];
@@ -807,6 +810,9 @@ namespace DS4Windows
                 nUDSXS.Value = 1;
                 nUDSZS.Value = 1;
 
+                lsSquStickCk.Checked = false;
+                rsSquStickCk.Checked = false;
+
                 cBLaunchProgram.Checked = false;
                 pBProgram.Image = null;
                 btnBrowse.Text = Properties.Resources.Browse;
@@ -1312,6 +1318,8 @@ namespace DS4Windows
             SZMaxzone[device] = (double)nUDSixAxisZMaxZone.Value;
             SXAntiDeadzone[device] = (double)nUDSixaxisXAntiDead.Value;
             SZAntiDeadzone[device] = (double)nUDSixaxisZAntiDead.Value;
+            squareStickLS[device] = lsSquStickCk.Checked;
+            squareStickRS[device] = rsSquStickCk.Checked;
             MouseAccel[device] = cBMouseAccel.Checked;
             DinputOnly[device] = cBDinput.Checked;
             StartTouchpadOff[device] = cbStartTouchpadOff.Checked;
@@ -3099,6 +3107,22 @@ namespace DS4Windows
             }
         }
 
+        private void lsSquStickCk_Click(object sender, EventArgs e)
+        {
+            if (loading == false)
+            {
+                squareStickLS[device] = lsSquStickCk.Checked;
+            }
+        }
+
+        private void rsSquStickCk_Click(object sender, EventArgs e)
+        {
+            if (loading == false)
+            {
+                squareStickRS[device] = rsSquStickCk.Checked;
+            }
+        }
+
         private void trackFrictionNUD_ValueChanged(object sender, EventArgs e)
         {
             if (loading == false)