四边定位的旋转量模拟（Python）

import math

# 工件
class Device(object):
    def __init__(self, x, y):
        self.x = float(x)
        self.y = float(y)
        self.area = self.x * self.y
        self.diagonal = math.sqrt(self.x**2+self.y**2)

# 定位爪
class Position(object):
    def __init__(self, x, y, dx, dy):
        self.x = float(x)
        self.y = float(y)
        self.dx = float(dx)
        self.dy = float(dy)
        self.area = self.x * self.y
        self.diagonal = min(math.sqrt(self.dx**2+self.y**2),math.sqrt(self.x**2+self.dy**2))

# 点到直线距离
def p2l(p=(0, 0), l=(0, 0, 0)):
    x, y = p[0], p[1]
    a, b, c = l[0], l[1], l[2]
    return abs(a * x + b * y + c) / math.sqrt(a**2 + b**2)

# 两直线交点
def l2l(l1=(0, 0, 0), l2=(0, 0, 0)):
    a1, b1, c1 = l1[0], l1[1], l1[2]
    a2, b2, c2 = l2[0], l2[1], l2[2]
    return (
        (c2 * b1 - c1 * b2) / (a1 * b2 - a2 * b1),
        (c1 * a2 - c2 * a1) / (a1 * b2 - a2 * b1),
    )

# 形状检查
def sharp_check(d,p):
    if p.x <= d.x or p.y <= d.y:
        return True
    elif p.dx >= p.x or p.dy >= p.y:
        return True
    elif d.diagonal < p.diagonal:
        return True
    else:
        return False

# 计算多边形的面积
def calculate_polygon_area(*coords):
    n = len(coords)
    area = 0
    j = n - 1
    for i in range(0, n):
        area += (coords[j][0] + coords[i][0]) * (coords[j][1] - coords[i][1])
        j = i
    return abs(area / 2)

print("工件面积：{:.2f}".format(device.area))
print("定位爪区域面积：{:.2f}".format(position.area))
# 工件
cv.create_rectangle((-device.x / 2, device.y / 2), (device.x / 2, -device.y / 2))
# 定位爪
cv.create_rectangle((position.x / 2, position.dy / 2),(position.x / 2 + math.inf, -position.dy / 2))
cv.create_rectangle((-position.dx / 2, position.y / 2 + math.inf),(position.dx / 2, position.y / 2))
cv.create_rectangle((-position.x / 2 - math.inf, position.dy / 2),(-position.x / 2, -position.dy / 2))
cv.create_rectangle((-position.dx / 2, -position.y / 2),(position.dx / 2, -position.y / 2 - math.inf))
# 形状非法
if sharp_check(device,position):
    print("形状非法，请重新输入。\n")
    return
print("生成中，请稍候...")
# 工件的四个顶点坐标
p01=(position.x/2,-position.dy/2)
p02=(position.dx/2,position.y/2)
p03=(-position.x/2,position.dy/2)
p04=(-position.dx/2,-position.y/2)
# 计算 旋转工件四个顶点
for alpha in range(1, 90000):
    alpha = math.radians(alpha/1000)
    k = math.tan(alpha)
    p1 = (device.x/2*math.cos(alpha)+device.y/2*math.sin(alpha),device.x/2*math.sin(alpha)-device.y/2*math.cos(alpha))
    p2 = (device.x/2*math.cos(alpha)-device.y/2*math.sin(alpha),device.x/2*math.sin(alpha)+device.y/2*math.cos(alpha))
    p3 = (-device.x/2*math.cos(alpha)-device.y/2*math.sin(alpha),-device.x/2*math.sin(alpha)+device.y/2*math.cos(alpha))
    p4 = (-device.x/2*math.cos(alpha)+device.y/2*math.sin(alpha),-device.x/2*math.sin(alpha)-device.y/2*math.cos(alpha))
    l1 = (-1/k,-1,1/k*p1[0]+p1[1])
    l2 = (k,-1,-k*p2[0]+p2[1])
    l3 = (-1/k,-1,1/k*p3[0]+p3[1])
    l4 = (k,-1,-k*p4[0]+p4[1])
    # 碰到定位爪停止
    if any((p2l(p01,l1)<=0.001,p2l(p02,l2)<=0.001,p2l(p03,l3)<=0.001,p2l(p04,l4)<=0.001)):
        print("工件极限旋转角：{:.2f}°".format(math.degrees(alpha)))
        # X向定位
        if any((p2l(p01,l1)<=0.001,p2l(p03,l3)<=0.001)):
            p1 = (p1[0],p1[1]-p2l(p02,l2))
            p2 = (p2[0],p2[1]-p2l(p02,l2))
            p3 = (p3[0],p3[1]-p2l(p02,l2))
            p4 = (p4[0],p4[1]-p2l(p02,l2))
        # Y向定位
        else:
            p1 = (p1[0]-p2l(p01,l1),p1[1])
            p2 = (p2[0]-p2l(p01,l1),p2[1])
            p3 = (p3[0]-p2l(p01,l1),p3[1])
            p4 = (p4[0]-p2l(p01,l1),p4[1])
        # 工件的极限旋转情形
        cv.create_polygon(p1,p2,p3,p4)
        break
# 重叠区域计算
# 极限情形的各边
l1 = (-1/k, -1, 1/k*p1[0]+p1[1])
l2 = (k, -1, -k*p2[0]+p2[1])
l3 = (-1/k, -1, 1/k*p3[0]+p3[1])
l4 = (k, -1, -k*p4[0]+p4[1])
# 极限情形与理论情形的各边交点
p11 = l2l(l1,(1,0,-device.x/2))
p12 = l2l(l1,(0,1,-device.y/2))
p13 = l2l(l2,(0,1,-device.y/2))
p14 = l2l(l2,(1,0,device.x/2))
p15 = l2l(l3,(1,0,device.x/2))
p16 = l2l(l3,(0,1,device.y/2))
p17 = l2l(l4,(0,1,device.y/2))
p18 = l2l(l4,(1,0,-device.x/2))
print("重叠部分面积为：{:.2f}\n".format(calculate_polygon_area(p11, p12, p13, p14, p15, p16, p17, p18)))
cv.create_polygon(p11, p12, p13, p14, p15, p16, p17, p18)