From fd978fd8de959ef97c9223fb536ccc069f195121 Mon Sep 17 00:00:00 2001
From: drgallaci <35565192+drgallaci@users.noreply.github.com>
Date: Mon, 6 Sep 2021 23:54:46 +0200
Subject: [PATCH 1/2] Probe/Orient evolutionary-like method for orient/scaling

---
 bCNC/CNC.py | 126 +++++++++++++++++++++++++++++++++++++++++++++++++---
 1 file changed, 119 insertions(+), 7 deletions(-)

diff --git a/bCNC/CNC.py b/bCNC/CNC.py
index 72dd0d14e..884f3e05c 100644
--- a/bCNC/CNC.py
+++ b/bCNC/CNC.py
@@ -17,6 +17,8 @@
 import json
 #import binascii
 
+import random
+
 from dxf	import DXF
 from bstl	import Binary_STL_Writer
 from bpath	import eq,Path, Segment
@@ -482,6 +484,8 @@ def clear(self, item=None):
 		self.phi = 0.0
 		self.xo  = 0.0
 		self.yo  = 0.0
+		self.scalex = 1.0
+		self.scaley = 1.0
 		self.valid = False
 		self.saved = False
 
@@ -550,7 +554,7 @@ def __len__(self):
 	#   2. no aspect ratio
 	#
 	#-----------------------------------------------------------------------
-	def solve(self):
+	def solve_old(self):
 		self.valid = False
 		if len(self.markers)< 2: raise Exception("Too few markers")
 		A = []
@@ -582,6 +586,98 @@ def solve(self):
 		self.valid = True
 		return self.phi,self.xo,self.yo
 
+	def CalculateSSE(self, _xo, _yo, _phi, _scalex, _scaley):
+		_c = cos(_phi)
+		_s = sin(_phi)
+		SSE = 0.0
+		for i,(xm,ym,x,y) in enumerate(self.markers):
+			dx = _scalex * (_c*x - _s*y + _xo) - xm
+			dy = _scaley * (_s*x + _c*y + _yo) - ym
+			err = dx**2 + dy**2
+			SSE += err
+		return SSE
+
+	#-----------------------------------------------------------------------
+	# Return the rotation angle phi in radians, the offset (xo,yo) 
+	#   and the scales (scalex,scaley)
+	# Applies a primitive/naive evolutionary-like algorithm,
+	#   not too sophisticated but does the job in (250-400) 400 iterations
+	# Provides better MSE values than the previous methods even without sx,sy
+	# It can be easily extended to handle skew (if necessary)
+	#-----------------------------------------------------------------------
+	def solve(self):
+		self.valid = False
+		if len(self.markers)< 3: raise Exception("Too few markers")
+		A = []
+		B = []
+		for xm,ym,x,y in self.markers:
+			A.append([x,y]);	B.append([xm, ym])
+
+		print ("1")
+		rate = 0.1
+		SSE_0 = self.CalculateSSE(self.xo, self.yo, self.phi, self.scalex, self.scaley)
+		for rep in range(400):
+			dxo = random.random() - 0.5
+			dyo = random.random() - 0.5
+			dphi = rate * (random.random() - 0.5)
+			dsx = rate * (random.random() - 0.5)
+			dsy = rate * (random.random() - 0.5)
+			
+			SSE = self.CalculateSSE(self.xo + dxo, self.yo, self.phi, self.scalex, self.scaley)
+			if SSE<=SSE_0:
+				self.xo += dxo
+				SSE_0 = SSE
+			else:
+				SSE = self.CalculateSSE(self.xo - dxo, self.yo, self.phi, self.scalex, self.scaley)
+				if SSE<=SSE_0:
+					self.xo -= dxo
+					SSE_0 = SSE
+
+			SSE = self.CalculateSSE(self.xo, self.yo + dyo, self.phi, self.scalex, self.scaley)
+			if SSE<=SSE_0:
+				self.yo += dyo
+				SSE_0 = SSE
+			else:
+				SSE = self.CalculateSSE(self.xo, self.yo - dyo, self.phi, self.scalex, self.scaley)
+				if SSE<=SSE_0:
+					self.yo -= dyo
+					SSE_0 = SSE
+
+			SSE = self.CalculateSSE(self.xo, self.yo, self.phi + dphi, self.scalex, self.scaley)
+			if SSE<=SSE_0:
+				self.phi += dphi
+				SSE_0 = SSE
+			else:
+				SSE = self.CalculateSSE(self.xo, self.yo, self.phi - dphi, self.scalex, self.scaley)
+				if SSE<=SSE_0:
+					self.phi -= dphi
+					SSE_0 = SSE
+					
+			SSE = self.CalculateSSE(self.xo, self.yo, self.phi, self.scalex + dsx, self.scaley)
+			if SSE<=SSE_0:
+				self.scalex += dsx
+				SSE_0 = SSE
+			else:
+				SSE = self.CalculateSSE(self.xo, self.yo, self.phi, self.scalex - dsx, self.scaley)
+				if SSE<=SSE_0:
+					self.scalex -= dsx
+					SSE_0 = SSE
+
+			SSE = self.CalculateSSE(self.xo, self.yo, self.phi, self.scalex, self.scaley + dsy)
+			if SSE<=SSE_0:
+				self.scaley += dsy
+				SSE_0 = SSE
+			else:
+				SSE = self.CalculateSSE(self.xo, self.yo, self.phi, self.scalex, self.scaley - dsy)
+				if SSE<=SSE_0:
+					self.scaley -= dsy
+					SSE_0 = SSE
+
+		# if abs(self.phi)<TOLERANCE: self.phi = 0.0	# rotation
+
+		self.valid = True
+		return self.phi,self.xo,self.yo,self.scalex,self.scaley
+		
 	#-----------------------------------------------------------------------
 	# @return minimum, average and maximum error
 	#-----------------------------------------------------------------------
@@ -597,8 +693,8 @@ def error(self):
 		del self.errors[:]
 
 		for i,(xm,ym,x,y) in enumerate(self.markers):
-			dx = c*x - s*y + self.xo - xm
-			dy = s*x + c*y + self.yo - ym
+			dx = self.scalex * (c*x - s*y + self.xo) - xm
+			dy = self.scaley * (s*x + c*y + self.yo) - ym
 			err = sqrt(dx**2 + dy**2)
 			self.errors.append(err)
 
@@ -614,8 +710,8 @@ def error(self):
 	def gcode2machine(self, x, y):
 		c = cos(self.phi)
 		s = sin(self.phi)
-		return	c*x - s*y + self.xo, \
-			s*x + c*y + self.yo
+		return	self.scalex * (c*x - s*y + self.xo), \
+			self.scaley * (s*x + c*y + self.yo)
 
 	#-----------------------------------------------------------------------
 	# Convert machine to gcode coordinates
@@ -623,6 +719,8 @@ def gcode2machine(self, x, y):
 	def machine2gcode(self, x, y):
 		c = cos(self.phi)
 		s = sin(self.phi)
+		x /= self.scalex
+		y /= self.scaley
 		x -= self.xo
 		y -= self.yo
 		return	 c*x + s*y, \
@@ -4422,7 +4520,20 @@ def transformFunc(self, new, old, relative, c, s, xo, yo):
 			new['I'] = c*i - s*j
 			new['J'] = s*i + c*j
 		return True
+	
+	def transformScaleFunc(self, new, old, relative, c, s, xo, yo, scalex, scaley):
+		if 'X' not in new and 'Y' not in new: return False
+		x = getValue('X',new,old)
+		y = getValue('Y',new,old)
+		new['X'] = scalex * (c*x - s*y + xo) 
+		new['Y'] = scaley * (s*x + c*y + yo)
 
+		if 'I' in new or 'J' in new:
+			i = getValue('I',new,old)
+			j = getValue('J',new,old)
+			new['I'] = scalex * (c*i - s*j)
+			new['J'] = scaley * (s*i + c*j)
+		return True
 	#----------------------------------------------------------------------
 	# Rotate items around optional center (on XY plane)
 	# ang in degrees (counter-clockwise)
@@ -4443,8 +4554,9 @@ def orientLines(self, items):
 		if not self.orient.valid: return "ERROR: Orientation information is not valid"
 		c = math.cos(self.orient.phi)
 		s = math.sin(self.orient.phi)
-		return self.modify(items, self.transformFunc, None, c, s,
-					self.orient.xo, self.orient.yo)
+		return self.modify(items, self.transformScaleFunc, None, c, s,
+					self.orient.xo, self.orient.yo, 
+					self.orient.scalex, self.orient.scaley)
 
 	#----------------------------------------------------------------------
 	# Mirror Horizontal

From 0a3d5f0b523392d81af6b526e31b0d0c39ba8642 Mon Sep 17 00:00:00 2001
From: drgallaci <35565192+drgallaci@users.noreply.github.com>
Date: Mon, 6 Sep 2021 23:56:46 +0200
Subject: [PATCH 2/2] Orient diplays scalex/scaley for the new Orient method

---
 bCNC/ProbePage.py | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/bCNC/ProbePage.py b/bCNC/ProbePage.py
index 282e5d795..0329325fb 100644
--- a/bCNC/ProbePage.py
+++ b/bCNC/ProbePage.py
@@ -817,10 +817,10 @@ def probeCenter(self, event=None):
 	#-----------------------------------------------------------------------
 	def orientSolve(self, event=None):
 		try:
-			phi, xo, yo = self.app.gcode.orient.solve()
+			phi, xo, yo, scalex, scaley = self.app.gcode.orient.solve()
 			self.angle_orient["text"]="%*f"%(CNC.digits, math.degrees(phi))
-			self.xo_orient["text"]="%*f"%(CNC.digits, xo)
-			self.yo_orient["text"]="%*f"%(CNC.digits, yo)
+			self.xo_orient["text"]="%*f        (Scale: x%*f)"%(CNC.digits, xo, CNC.digits, scalex)
+			self.yo_orient["text"]="%*f        (Scale: x%*f)"%(CNC.digits, yo, CNC.digits, scaley)
 
 			minerr, meanerr, maxerr = self.app.gcode.orient.error()
 			self.err_orient["text"] = "Avg:%*f  Max:%*f  Min:%*f"%\