add nash

WaferAdjust/FitCircleToolBlock.cs

@@ -10,7 +10,7 @@ using System.Threading.Tasks;
 
 namespace WaferAdjust
 {
-    public delegate void OnFitCircleResult(double x, double y, double rms);
+    public delegate void OnFitCircleResult(double x, double y, double r, double rms);
     internal class FitCircleToolBlock
     {
         private CogToolBlock cogToolBlock;
@@ -47,6 +47,7 @@ namespace WaferAdjust
                 FitCircleImage = cogToolBlock.Outputs["OutputImage"].Value as CogImage8Grey;
                 OnFitCircleResult?.Invoke(Math.Round((double)cogToolBlock.Outputs["CenterX"].Value, 3),
                     Math.Round((double)cogToolBlock.Outputs["CenterY"].Value, 3),
+                    Math.Round((double)cogToolBlock.Outputs["Radius"].Value, 3),
                     Math.Round((double)cogToolBlock.Outputs["RMSError"].Value, 3));
             }
             catch (Exception ex)

WaferAdjust/Form1.Designer.cs

@@ -289,7 +289,7 @@
             this.textBox2.Name = "textBox2";
             this.textBox2.Size = new System.Drawing.Size(55, 21);
             this.textBox2.TabIndex = 7;
-            this.textBox2.Text = "64";
+            this.textBox2.Text = "82";
             // 
             // label1
             // 
@@ -323,7 +323,7 @@
             this.textBox5.Name = "textBox5";
             this.textBox5.Size = new System.Drawing.Size(55, 21);
             this.textBox5.TabIndex = 13;
-            this.textBox5.Text = "1";
+            this.textBox5.Text = "-6";
             // 
             // label2
             // 
@@ -349,7 +349,7 @@
             this.textBox4.Name = "textBox4";
             this.textBox4.Size = new System.Drawing.Size(55, 21);
             this.textBox4.TabIndex = 11;
-            this.textBox4.Text = "1";
+            this.textBox4.Text = "-8";
             // 
             // groupBox1
             // 

WaferAdjust/Form1.cs

@@ -6,6 +6,7 @@ using Cognex.VisionPro.ImageProcessing;
 using Cognex.VisionPro.PMAlign;
 using Cognex.VisionPro.ToolBlock;
 using QWhale.Common;
+using QWhale.Syntax;
 using System;
 using System.Collections.Generic;
 using System.ComponentModel;
@@ -19,6 +20,7 @@ using System.Threading;
 using System.Threading.Tasks;
 using System.Windows.Forms;
 using System.Windows.Interop;
+using System.Windows.Media;
 using System.Xml.Serialization;
 
 
@@ -52,6 +54,7 @@ namespace WaferAdjust
 
                 getCircleMachineToolBlock = new GetCircleToolBlock();
                 getCircleMachineToolBlock.OnGetCircleResult += GetCircleMachineToolBlock_OnGetCircleResult;
+                getCircleMachineToolBlock.OnGetNashResult += GetCircleMachineToolBlock_OnGetNashResult;
                 getCircleMachineToolBlock.OnToolReady += GetCircleMachineToolBlock_OnToolReady;
                 getCircleMachineToolBlock.Initialize("vpp\\GetCircleM.vpp");
 
@@ -70,31 +73,88 @@ namespace WaferAdjust
             }            
         }
 
+        private void GetCircleMachineToolBlock_OnGetNashResult(double x, double y)
+        {
+            translateCirclePoint.AddNashPoint(x, y);
+            ShowMessage($"Update Nash X:{x}, Y:{y}{Environment.NewLine}");
+        }
+
         private void FitCircleMachineToolBlock_OnToolReady(bool ready)
         {
             
         }
-
+        private void AddPointMarker(CogGraphicCollection graphicCollection, double x, double y, CogColorConstants color = CogColorConstants.Green)
-        private void FitCircleMachineToolBlock_OnFitCircleResult(double x, double y, double rms)
         {
-            CogPointMarker marker_1 = new CogPointMarker();
+            CogPointMarker marker = new CogPointMarker();
+            marker.X = x;
+            marker.Y = y;
+            marker.Color = color;
+            marker.Interactive = false;
+            graphicCollection.Add(marker);
+        }
+        private void AddLineMarker(CogGraphicCollection graphicCollection, double x1, double y1, double x2, double y2, CogColorConstants color = CogColorConstants.Green)
+        {
+            CogLineSegment line = new CogLineSegment();
+            line.SetStartEnd((int)x1, (int)y1, (int)x2, (int)y2);
+            line.Color = color;
+            line.Interactive = false;
+            graphicCollection.Add(line);
+        }
+        private void AddLabelMarker(CogGraphicCollection graphicCollection, string text, double x, double y, CogColorConstants color = CogColorConstants.Green)
+        {
+            CogGraphicLabel label = new CogGraphicLabel();
+            label.Text = text;
+            label.X = x;
+            label.Y = y;
+            label.Color = color;
+            graphicCollection.Add(label);
+        }
+        /// <summary>
+        /// 计算从 lineA 到 lineB 的逆时针有向角度（0-360度）
+        /// 坐标系：向右为正，向下为正（标准屏幕坐标系）
+        /// </summary>
+        public double GetCounterClockwiseAngle360(double x, double y, double x1, double y1, double x2, double y2)
+        {
+            // 2. 获取方向向量（dy 向下为正）
+            double dxA = x1 - x;
+            double dyA = y1 - y;
 
-            marker_1.X = x;
+            double dxB = x2 - x;
-            marker_1.Y = y;
+            double dyB = y2 - y;
-            marker_1.Color = CogColorConstants.Green;
-            marker_1.Interactive = false;
-            m_graphics2.Add(marker_1);
 
-            CogPointMarker marker_2 = new CogPointMarker();
+            // 3. 将Y轴翻转，转换为标准数学坐标系（dy向上为正）
+            //    这样 Math.Atan2 返回的就是标准数学角度（逆时针为正）
+            double angleA = Math.Atan2(-dyA, dxA);  // 注意：y取负号
+            double angleB = Math.Atan2(-dyB, dxB);  // 注意：y取负号
 
-            marker_2.X = Convert.ToDouble(textBox4.Text);
+            // 4. 计算逆时针角度差（弧度）
-            marker_2.Y = Convert.ToDouble(textBox5.Text);
+            double angleDiff = angleB - angleA;
-            marker_2.Color = CogColorConstants.Red;
+
-            marker_2.Interactive = false;
+            // 5. 转换为角度并规范化为 [0, 360)
-            m_graphics2.Add(marker_2);
+            double angleDegrees = angleDiff * 180.0 / Math.PI;
+            angleDegrees = angleDegrees % 360.0;
+            if (angleDegrees < 0) angleDegrees += 360.0;
+
+            return Math.Round(angleDegrees, 3);
+        }
+        private void FitCircleMachineToolBlock_OnFitCircleResult(double x, double y, double r, double rms)
+        {
+            AddPointMarker(m_graphics2, x, y);
+            AddPointMarker(m_graphics2, translateCirclePoint.GetRotateX(), translateCirclePoint.GetRotateY(), CogColorConstants.Red);
+
+            PointInfo nPoint = translateCirclePoint.GetNashPoint();
+            if (nPoint != null)
+            {
+                AddPointMarker(m_graphics2, nPoint.X, nPoint.Y, CogColorConstants.Yellow);
+                AddLineMarker(m_graphics2, x, y, x, y - r);
+                AddLineMarker(m_graphics2, x, y, nPoint.X, nPoint.Y, CogColorConstants.Yellow);
+                var aa = GetCounterClockwiseAngle360(x, y, x, y - r, nPoint.X, nPoint.Y);
+                AddLabelMarker(m_graphics2, aa.ToString(), x, y, CogColorConstants.Yellow);
+                ShowMessage($"{Environment.NewLine}nash degree:{aa}{Environment.NewLine}");
+            }
 
             ShowMessage($"{Environment.NewLine}X:{x}, Y:{y}, RMS:{rms}{Environment.NewLine}");
-            ShowMessage($"{Environment.NewLine}DiffX:{x - Convert.ToDouble(textBox4.Text)}, DiffY:{y - Convert.ToDouble(textBox5.Text)}, RMS:{rms}{Environment.NewLine}");
+            ShowMessage($"{Environment.NewLine}DiffX:{Math.Round((x - Convert.ToDouble(textBox4.Text)), 3)}, DiffY:{Math.Round((y - Convert.ToDouble(textBox5.Text)), 3)}, RMS:{rms}{Environment.NewLine}");
             ImageDisplay(FitCircleMachineToolBlock.FitCircleImage);
             ImageDisplay(m_graphics);
             ImageDisplay(m_graphics2);
@@ -118,15 +178,9 @@ namespace WaferAdjust
             
         }
 
-        private void FitCircleCameraToolBlock_OnFitCircleResult(double x, double y, double rms)
+        private void FitCircleCameraToolBlock_OnFitCircleResult(double x, double y, double r, double rms)
         {
-            CogPointMarker marker_1 = new CogPointMarker();
+            AddPointMarker(m_graphics2, x, y);
-
-            marker_1.X = x;
-            marker_1.Y = y;
-            marker_1.Color = CogColorConstants.Green;
-            marker_1.Interactive = false;
-            m_graphics2.Add(marker_1);
             ShowMessage($"{Environment.NewLine}X:{x}, Y:{y}, RMS:{rms}{Environment.NewLine}");
             ImageDisplay(fitCircleCameraToolBlock.FitCircleImage);
             ImageDisplay(m_graphics);
@@ -196,13 +250,7 @@ namespace WaferAdjust
             m_graphics = new CogGraphicCollection();
             foreach (PointInfo pointInfo in aa)
             {
-                CogPointMarker marker_1 = new CogPointMarker();//圆
+                AddPointMarker(m_graphics, pointInfo.X, pointInfo.Y);              
-
-                marker_1.X = pointInfo.X;
-                marker_1.Y = pointInfo.Y;
-                marker_1.Color = CogColorConstants.Green;
-                marker_1.Interactive = false;
-                m_graphics.Add(marker_1);                
             }
             fitCircleCameraToolBlock.Run(m_lastBMP, aa);
         }
@@ -284,20 +332,16 @@ namespace WaferAdjust
 
             ShowMessage("转换机械坐标系下圆弧各点的原始坐标：\n");
             var points = translateCirclePoint.DoTranslatePoint();
+           translateCirclePoint.DoTranslateNashPoint();
 
             ShowMessage("拟合机械坐标系下的圆中心：\n");
             m_graphics = new CogGraphicCollection();
             m_graphics2 = new CogGraphicCollection();
             foreach (PointInfo pointInfo in points)
             {
-                CogPointMarker marker_1 = new CogPointMarker();//圆
+                AddPointMarker(m_graphics, pointInfo.X, pointInfo.Y);
-
-                marker_1.X = pointInfo.X;
-                marker_1.Y = pointInfo.Y;
-                marker_1.Color = CogColorConstants.Green;
-                marker_1.Interactive = false;
-                m_graphics.Add(marker_1);
             }
+
             FitCircleMachineToolBlock.Run(m_lastBMP, points);
         }
 

WaferAdjust/GetCircleToolBlock.cs

@@ -13,12 +13,14 @@ namespace WaferAdjust
 {
     public delegate void OnGetCircleResult(double x, double y, double r);
     public delegate void OnToolReady(bool ready);
+    public delegate void OnGetNashResult(double x, double y);
     internal class GetCircleToolBlock
     {
         private CogToolBlock cogToolBlock;
         private bool initialized = false;
         public event OnToolReady OnToolReady;
         public event OnGetCircleResult OnGetCircleResult;
+        public event OnGetNashResult OnGetNashResult;
         public List<PointInfo> PointInfos;
         public void Initialize(string vpp)
         {
@@ -62,6 +64,12 @@ namespace WaferAdjust
                 OnGetCircleResult?.Invoke(Math.Round((double)cogToolBlock.Outputs["CenterX"].Value, 3),
                     Math.Round((double)cogToolBlock.Outputs["CenterY"].Value, 3),
                     Math.Round((double)cogToolBlock.Outputs["Radius"].Value, 3));
+
+                if ((int)cogToolBlock.Outputs["Results_Count"].Value == 1)
+                {
+                    OnGetNashResult?.Invoke(Math.Round((double)cogToolBlock.Outputs["TranslationX"].Value, 3),
+                    Math.Round((double)cogToolBlock.Outputs["TranslationY"].Value, 3));
+                }
             }
             catch (Exception ex)
             {

WaferAdjust/TranslateCirclePoint.cs

@@ -14,6 +14,7 @@ namespace WaferAdjust
         List<PointInfo> totalCenters;
         private int circleIndex;
         private double perAngle;
+        PointInfo nashPoint;
         public void SetRotateXY(double x, double y, double angle)
         {
             rotateX = x; rotateY = y;
@@ -21,6 +22,15 @@ namespace WaferAdjust
             totalCenters = new List<PointInfo>();
             circleIndex = 0;
             perAngle = angle;
+            nashPoint = null;
+        }
+        public double GetRotateX()
+        {
+            return rotateX;
+        }
+        public double GetRotateY()
+        {
+            return rotateY;
         }
         public void AddCirclePoint(List<PointInfo> pointInfos)
         {
@@ -33,6 +43,10 @@ namespace WaferAdjust
         {
             circleIndex++;
         }
+        public void AddNashPoint(double x, double y)
+        {
+            nashPoint = new PointInfo(x, y, circleIndex * perAngle);
+        }
         public void AddCircleCenter(double x, double y, double r)
         {
             totalCenters.Add(new PointInfo(x, y, r));
@@ -44,10 +58,23 @@ namespace WaferAdjust
             {
                 var res = Rotation2D.GetOriginalPointDegrees(item.X, item.Y,
                     rotateX, rotateY, item.Radius, true, false);
-                Console.WriteLine($"{item.X} {item.Y} {item.Radius} ==> {res.x} {res.y}");
                 trans.Add(new PointInfo(res.x, res.y, item.Radius));
             }
             return trans;
         }
+        public void DoTranslateNashPoint()
+        {
+            if (nashPoint == null)
+                return;
+
+            var res = Rotation2D.GetOriginalPointDegrees(nashPoint.X, nashPoint.Y,
+                    rotateX, rotateY, nashPoint.Radius, true, false);
+
+            nashPoint = new PointInfo(res.x, res.y, nashPoint.Radius);
+        }
+        public PointInfo GetNashPoint()
+        {
+            return nashPoint;
+        }
     }
 }