698 lines
27 KiB
Python
698 lines
27 KiB
Python
import pygame
|
||
import math
|
||
import numpy as np
|
||
import random
|
||
from utils.cv_marker import cap_and_mark
|
||
# from utils.stack_exe import Stackbot
|
||
import cv2
|
||
import sys
|
||
|
||
|
||
pygame.init()
|
||
|
||
WIDTH, HEIGHT = 800, 600
|
||
screen = pygame.display.set_mode((WIDTH, HEIGHT))
|
||
pygame.display.set_caption("Stack Simulation")
|
||
|
||
WHITE = (255, 255, 255)
|
||
BLACK = (0, 0, 0)
|
||
RED = (255, 0, 0)
|
||
|
||
LAYER_COLORS = [
|
||
(255, 0, 0), (0, 255, 0), (0, 0, 255), (255, 255, 0), (255, 0, 255),
|
||
(0, 255, 255), (128, 0, 0), (0, 128, 0), (0, 0, 128), (128, 128, 0),
|
||
]
|
||
|
||
cap = cv2.VideoCapture(1)
|
||
cap.set(6,cv2.VideoWriter.fourcc('M','J','P','G'))
|
||
|
||
cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1920)
|
||
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 1080)
|
||
|
||
# 定义屏幕尺寸
|
||
SCREEN_WIDTH = 800
|
||
SCREEN_HEIGHT = 600
|
||
|
||
# 定义按钮尺寸
|
||
BUTTON_WIDTH = 200
|
||
BUTTON_HEIGHT = 200
|
||
|
||
class Block:
|
||
def __init__(self, x, y, width, height, angle, layer):
|
||
self.x = x
|
||
self.y = y
|
||
self.width = width
|
||
self.height = height
|
||
self.angle = angle
|
||
self.layer = layer
|
||
self.color = LAYER_COLORS[layer % len(LAYER_COLORS)]
|
||
|
||
def draw(self, surface):
|
||
rotated_surface = pygame.Surface((self.width, self.height), pygame.SRCALPHA)
|
||
rotated_surface.set_alpha(125)
|
||
pygame.draw.rect(rotated_surface, self.color, (0, 0, self.width, self.height))
|
||
rotated_surface = pygame.transform.rotate(rotated_surface, self.angle)
|
||
surface.blit(rotated_surface, (self.x - rotated_surface.get_width() // 2, self.y - rotated_surface.get_height() // 2))
|
||
|
||
def get_corners(self):
|
||
w, h = self.width / 2, self.height / 2
|
||
corners = [(-w, -h), (w, -h), (w, h), (-w, h)]
|
||
angle_rad = np.deg2rad(self.angle)
|
||
rotation_matrix = np.array([
|
||
[np.cos(angle_rad), -np.sin(angle_rad)],
|
||
[np.sin(angle_rad), np.cos(angle_rad)]
|
||
])
|
||
rotated_corners = np.dot(corners, rotation_matrix)
|
||
translated_corners = rotated_corners + np.array([self.x, self.y])
|
||
# print(translated_corners)
|
||
rotated_corners = [(float(point[0]), float(point[1])) for point in translated_corners]
|
||
|
||
return rotated_corners
|
||
|
||
|
||
class Tower:
|
||
def __init__(self, blueprint, tower_image=None):
|
||
self.blocks = []
|
||
self.current_layer = 0
|
||
self.layer_centroids = []
|
||
self.stability_threshold = 10
|
||
self.rotation_angle = 15
|
||
self.angle_tolerance = 5
|
||
self.position_tolerance = 10
|
||
|
||
self.sim_to_robot_matrix = np.array([
|
||
[1, 0, 0, -300],
|
||
[0, -1, 0, -200],
|
||
[0, 0, 1, 0],
|
||
[0, 0, 0, 1]
|
||
])
|
||
self.robot_to_sim_matrix = np.linalg.inv(self.sim_to_robot_matrix)
|
||
|
||
self.is_upper = False
|
||
|
||
self.blueprint = blueprint
|
||
self.center_x, self.center_y = 300, 300 # 定义塔的中心点
|
||
self.ideal_positions = self.get_ideal_positions()
|
||
self.position_tolerance = 20
|
||
self.angle_tolerance = 15
|
||
|
||
self.tower_image = None
|
||
if tower_image:
|
||
self.tower_image = pygame.transform.scale(tower_image, (150, 150)) # 调整图片大小
|
||
|
||
def sim_to_robot_coords(self, x, y, angle):
|
||
sim_coords = np.array([x, y, 0, 1])
|
||
robot_coords = self.sim_to_robot_matrix.dot(sim_coords)
|
||
robot_angle = (angle + 360) % 360 # 确保角度在0-359范围内
|
||
return robot_coords[0], robot_coords[1], robot_angle
|
||
|
||
def robot_to_sim_coords(self, x, y, angle):
|
||
robot_coords = np.array([x, y, 0, 1])
|
||
sim_coords = self.robot_to_sim_matrix.dot(robot_coords)
|
||
sim_angle = (angle + 360) % 360
|
||
return sim_coords[0], sim_coords[1], sim_angle
|
||
|
||
def add_block_from_robot(self, x, y, angle):
|
||
"""
|
||
这个函数根据机器人的坐标和角度，在模拟世界中添加一个方块
|
||
|
||
参数:
|
||
x (float): 机器人的 x 坐标
|
||
y (float): 机器人的 y 坐标
|
||
angle (float): 机器人的角度
|
||
|
||
返回值：
|
||
bool：如果方块添加成，返回 True，否则返回 False
|
||
|
||
注意：这个函数需要先调用 robot_to_sim_coords 来将机器人坐标转换为模拟坐标，并且需要一个名为 Block 的类来创建方块对象
|
||
"""
|
||
sim_x, sim_y, sim_angle = self.robot_to_sim_coords(x, y, angle)
|
||
new_block = Block(sim_x, sim_y, 100, 20, sim_angle, self.current_layer)
|
||
if self.add_block(new_block):
|
||
print(f"Block added from robot at ({sim_x:.2f}, {sim_y:.2f}) with angle {sim_angle}")
|
||
return True
|
||
else:
|
||
print(f"Failed to add block from robot at ({sim_x:.2f}, {sim_y:.2f}) with angle {sim_angle}")
|
||
return False
|
||
|
||
|
||
def add_block(self, block):
|
||
self.add_block_from_sim(block)
|
||
return True
|
||
# if self.current_layer == 5:
|
||
# self.is_upper = True
|
||
# print("is upper")
|
||
|
||
# if not self.check_interference(block):
|
||
# self.blocks.append(block)
|
||
# self.stack_bot.pick_stack()
|
||
# robo_xyz = self.sim_to_robot_coords(block.x, block.y, block.angle)
|
||
# print(block.x, block.y, block.angle)
|
||
# if self.is_upper:
|
||
# self.stack_bot.place_stack(robo_xyz[0], robo_xyz[1], robo_xyz[2], self.current_layer - 5)
|
||
# else:
|
||
# self.stack_bot.place_stack(robo_xyz[0], robo_xyz[1], robo_xyz[2], self.current_layer)
|
||
# return True
|
||
# return False
|
||
|
||
def add_block_from_sim(self, block):
|
||
self.blocks.append(block)
|
||
return True
|
||
|
||
def draw(self, surface):
|
||
for block in self.blocks:
|
||
block.draw(surface)
|
||
|
||
def check_stability(self):
|
||
current_layer_centroid = self.calculate_current_layer_centroid()
|
||
tower_centroid = self.calculate_tower_centroid()
|
||
|
||
if current_layer_centroid is None or tower_centroid is None:
|
||
return True # 如果没有足够的块来计算重心，我们假设它是稳定的
|
||
|
||
horizontal_distance = abs(current_layer_centroid[0] - tower_centroid[0])
|
||
|
||
return horizontal_distance <= self.stability_threshold
|
||
|
||
def blocks_overlap(self, block1, block2):
|
||
corners1 = block1.get_corners()
|
||
corners2 = block2.get_corners()
|
||
|
||
# 检查 block1 的角是否在 block2 内部
|
||
for corner in corners1:
|
||
if self.point_inside_polygon(corner, corners2):
|
||
return True
|
||
|
||
# 检查 block2 的角是否在 block1 内部
|
||
for corner in corners2:
|
||
if self.point_inside_polygon(corner, corners1):
|
||
return True
|
||
|
||
# 检查边是否相交
|
||
for i in range(4):
|
||
for j in range(4):
|
||
if self.line_intersects(corners1[i], corners1[(i+1)%4], corners2[j], corners2[(j+1)%4]):
|
||
return True
|
||
|
||
return False
|
||
|
||
def point_inside_polygon(self, point, polygon):
|
||
x, y = point
|
||
n = len(polygon)
|
||
inside = False
|
||
p1x, p1y = polygon[0]
|
||
for i in range(n + 1):
|
||
p2x, p2y = polygon[i % n]
|
||
if y > min(p1y, p2y):
|
||
if y <= max(p1y, p2y):
|
||
if x <= max(p1x, p2x):
|
||
if p1y != p2y:
|
||
xinters = (y - p1y) * (p2x - p1x) / (p2y - p1y) + p1x
|
||
if p1x == p2x or x <= xinters:
|
||
inside = not inside
|
||
p1x, p1y = p2x, p2y
|
||
return inside
|
||
|
||
def line_intersects(self, p1, p2, p3, p4):
|
||
def ccw(A, B, C):
|
||
return (C[1]-A[1]) * (B[0]-A[0]) > (B[1]-A[1]) * (C[0]-A[0])
|
||
return ccw(p1,p3,p4) != ccw(p2,p3,p4) and ccw(p1,p2,p3) != ccw(p1,p2,p4)
|
||
|
||
def check_interference(self, new_block):
|
||
# 检查是否与其他木块重叠
|
||
for block in self.blocks:
|
||
if block.layer == new_block.layer and self.blocks_overlap(block, new_block):
|
||
return True
|
||
return False
|
||
|
||
def calculate_current_layer_centroid(self):
|
||
current_layer_blocks = [block for block in self.blocks if block.layer == self.current_layer]
|
||
if not current_layer_blocks:
|
||
return None
|
||
|
||
total_area = 0
|
||
weighted_x = 0
|
||
weighted_y = 0
|
||
|
||
for block in current_layer_blocks:
|
||
area = block.width * block.height
|
||
total_area += area
|
||
weighted_x += block.x * area
|
||
weighted_y += block.y * area
|
||
|
||
if total_area == 0:
|
||
return None
|
||
|
||
centroid_x = weighted_x / total_area
|
||
centroid_y = weighted_y / total_area
|
||
return (centroid_x, centroid_y)
|
||
|
||
def calculate_tower_centroid(self):
|
||
if not self.blocks:
|
||
return None
|
||
|
||
total_area = 0
|
||
weighted_x = 0
|
||
weighted_y = 0
|
||
|
||
for block in self.blocks:
|
||
area = block.width * block.height
|
||
total_area += area
|
||
weighted_x += block.x * area
|
||
weighted_y += block.y * area
|
||
|
||
if total_area == 0:
|
||
return None
|
||
|
||
centroid_x = weighted_x / total_area
|
||
centroid_y = weighted_y / total_area
|
||
return (centroid_x, centroid_y)
|
||
|
||
def increase_layer(self):
|
||
centroid = self.calculate_current_layer_centroid()
|
||
if centroid:
|
||
self.layer_centroids.append(centroid)
|
||
self.current_layer += 1
|
||
|
||
def get_ideal_positions(self):
|
||
# 将相对坐标转换为绝对坐标
|
||
absolute_positions = []
|
||
for layer in self.blueprint:
|
||
layer_positions = []
|
||
for rel_x, rel_y, angle in layer:
|
||
abs_x = self.center_x + rel_x
|
||
abs_y = self.center_y + rel_y
|
||
layer_positions.append((abs_x, abs_y, angle))
|
||
absolute_positions.append(layer_positions)
|
||
return absolute_positions
|
||
|
||
def auto_place_block(self):
|
||
if self.current_layer >= len(self.ideal_positions):
|
||
print("塔已经完成")
|
||
return None, 0
|
||
|
||
possible_positions = self.generate_possible_positions()
|
||
best_position, similarity_score = self.find_best_position(possible_positions)
|
||
|
||
if best_position:
|
||
x, y, angle = best_position
|
||
print(f"选择的最佳位置: ({x}, {y}) 角度为 {angle}")
|
||
return Block(x, y, 100, 20, angle, self.current_layer), similarity_score
|
||
else:
|
||
print("无法找到合适的放置位置")
|
||
return None, 0
|
||
|
||
def generate_possible_positions(self):
|
||
ideal_positions = self.ideal_positions[self.current_layer]
|
||
possible_positions = []
|
||
|
||
for ideal_x, ideal_y, ideal_angle in ideal_positions:
|
||
for dx in range(-self.position_tolerance, self.position_tolerance + 1, 2):
|
||
for dy in range(-self.position_tolerance, self.position_tolerance + 1, 2):
|
||
for dangle in range(-self.angle_tolerance, self.angle_tolerance + 1, 5):
|
||
x = ideal_x + dx
|
||
y = ideal_y + dy
|
||
angle = (ideal_angle + dangle) % 360
|
||
possible_positions.append((x, y, angle))
|
||
|
||
print(f"生成的可能位置数量: {len(possible_positions)}")
|
||
return possible_positions
|
||
|
||
def find_best_position(self, possible_positions):
|
||
best_position = None
|
||
min_distance = float('inf')
|
||
best_similarity_score = 0
|
||
|
||
blocks_in_current_layer = sum(1 for block in self.blocks if block.layer == self.current_layer)
|
||
ideal_positions = self.ideal_positions[self.current_layer]
|
||
|
||
if blocks_in_current_layer < len(ideal_positions):
|
||
ideal_x, ideal_y, ideal_angle = ideal_positions[blocks_in_current_layer]
|
||
else:
|
||
print("警告：当前层的所有位置都已被填满")
|
||
return None, 0
|
||
|
||
for x, y, angle in possible_positions:
|
||
new_block = Block(x, y, 100, 20, angle, self.current_layer)
|
||
|
||
if self.has_support(new_block) and not self.check_interference(new_block):
|
||
temp_tower = Tower(self.blueprint)
|
||
temp_tower.blocks = self.blocks.copy()
|
||
temp_tower.add_block(new_block)
|
||
if temp_tower.check_stability():
|
||
distance = np.sqrt((x - ideal_x)**2 + (y - ideal_y)**2) + abs(angle - ideal_angle)
|
||
# 方案1：使用指数函数
|
||
ratio = distance / (np.sqrt(WIDTH**2 + HEIGHT**2) + 360)
|
||
similarity_score = 100 * (1 - np.power(ratio, 0.5)) # 使用0.5次方增加敏感度
|
||
|
||
if distance < min_distance:
|
||
min_distance = distance
|
||
best_position = (x, y, angle)
|
||
best_similarity_score = similarity_score
|
||
|
||
print(f"选择的最佳位置: {best_position}，相似性评分: {best_similarity_score:.2f}")
|
||
return best_position, best_similarity_score
|
||
|
||
def has_support(self, block):
|
||
if self.current_layer == 0:
|
||
return True # 第一层总是有支撑
|
||
|
||
block_corners = block.get_corners()
|
||
for lower_block in [b for b in self.blocks if b.layer == self.current_layer - 1]:
|
||
lower_corners = lower_block.get_corners()
|
||
for corner in block_corners:
|
||
if self.point_inside_polygon(corner, lower_corners):
|
||
return True
|
||
return False
|
||
|
||
def auto_place_layer(self):
|
||
center_x, center_y = 359, 90
|
||
block_width, block_height = 100, 20
|
||
square_size = 110 # 正方形的边长
|
||
|
||
# 基于当前层数计算旋转角度
|
||
layer_rotation = (self.current_layer * 30) % 360
|
||
|
||
# 计算正方形四个角的坐标
|
||
base_positions = [
|
||
(-square_size/2, -square_size/2), # 左上
|
||
(square_size/2, -square_size/2), # 右上
|
||
(square_size/2, square_size/2), # 右下
|
||
(-square_size/2, square_size/2) # 左下
|
||
]
|
||
|
||
# 旋转基础位置
|
||
rotated_positions = []
|
||
for x, y in base_positions:
|
||
angle_rad = math.radians(layer_rotation)
|
||
rotated_x = x * math.cos(angle_rad) - y * math.sin(angle_rad)
|
||
rotated_y = x * math.sin(angle_rad) + y * math.cos(angle_rad)
|
||
rotated_positions.append((center_x + rotated_x, center_y + rotated_y))
|
||
|
||
base_angles = [45, 135, 225, 315]
|
||
angles = [(angle - layer_rotation) % 360 for angle in base_angles]
|
||
|
||
for (x, y), angle in zip(rotated_positions, angles):
|
||
new_block = Block(x, y, block_width, block_height, angle, self.current_layer)
|
||
if self.add_block(new_block):
|
||
print(f"Block placed at ({x:.2f}, {y:.2f}) with angle {angle}")
|
||
else:
|
||
print(f"Failed to place block at ({x:.2f}, {y:.2f}) with angle {angle}")
|
||
|
||
|
||
def clear_current_layer(self):
|
||
self.blocks = [block for block in self.blocks if block.layer!= self.current_layer]
|
||
|
||
def scan_current_layer(self):
|
||
current_block_pos = cap_and_mark(cap=cap)
|
||
for block in current_block_pos:
|
||
print(block)
|
||
self.add_block_from_robot(block[0][0], block[0][1], block[1])
|
||
|
||
|
||
def draw_tower_image(self, screen):
|
||
if self.tower_image:
|
||
image_rect = self.tower_image.get_rect()
|
||
image_rect.topright = (SCREEN_WIDTH - 10, 10) # 放置在右上角，留出10像素的边距
|
||
screen.blit(self.tower_image, image_rect)
|
||
|
||
def show_tower_selection(screen):
|
||
# 加载塔型图片
|
||
tower_images = [
|
||
pygame.image.load("tower1.png"),
|
||
pygame.image.load("tower2.png"),
|
||
pygame.image.load("tower3.png")
|
||
]
|
||
|
||
# 调整图片大小
|
||
tower_images = [pygame.transform.scale(img, (BUTTON_WIDTH, BUTTON_HEIGHT)) for img in tower_images]
|
||
|
||
# 计算按钮位置
|
||
button_y = (SCREEN_HEIGHT - BUTTON_HEIGHT) // 2
|
||
button_x_start = (SCREEN_WIDTH - (BUTTON_WIDTH * 3 + 40)) // 2
|
||
|
||
# 创建按钮矩形
|
||
buttons = [
|
||
pygame.Rect(button_x_start + i * (BUTTON_WIDTH + 20), button_y, BUTTON_WIDTH, BUTTON_HEIGHT)
|
||
for i in range(3)
|
||
]
|
||
|
||
while True:
|
||
for event in pygame.event.get():
|
||
if event.type == pygame.QUIT:
|
||
pygame.quit()
|
||
sys.exit()
|
||
if event.type == pygame.MOUSEBUTTONDOWN:
|
||
mouse_pos = pygame.mouse.get_pos()
|
||
for i, button in enumerate(buttons):
|
||
if button.collidepoint(mouse_pos):
|
||
return i # 返回选择的塔型索引
|
||
|
||
screen.fill(WHITE)
|
||
|
||
# 绘制按钮和图片
|
||
for i, (button, image) in enumerate(zip(buttons, tower_images)):
|
||
screen.blit(image, button.topleft)
|
||
pygame.draw.rect(screen, BLACK, button, 2)
|
||
|
||
# 添加标题
|
||
font = pygame.font.Font(None, 36)
|
||
title = font.render("Select Tower Type", True, BLACK)
|
||
title_rect = title.get_rect(center=(SCREEN_WIDTH // 2, 50))
|
||
screen.blit(title, title_rect)
|
||
|
||
pygame.display.flip()
|
||
|
||
def polar_to_cartesian(r, theta, angle):
|
||
"""
|
||
将极坐标转换为直角坐标
|
||
|
||
参数:
|
||
r: 半径
|
||
theta: 极角(度)
|
||
angle: 方块的旋转角度(度)
|
||
|
||
返回:
|
||
(x, y, angle): 直角坐标和方块旋转角度
|
||
"""
|
||
# 将角度转换为弧度
|
||
theta_rad = math.radians(theta)
|
||
|
||
# 计算 x 和 y
|
||
x = r * math.cos(theta_rad)
|
||
y = r * math.sin(theta_rad)
|
||
|
||
# 四舍五入到整数
|
||
x = round(x)
|
||
y = round(y)
|
||
|
||
return (x, y, angle)
|
||
|
||
def get_tower_blueprint_1():
|
||
# 第一种塔型的蓝图（相对坐标）
|
||
return [
|
||
[(-40, 0, 90), (40, 0, 90)],
|
||
[(0, 40, 0), (0, -40, 0)],
|
||
[(-35, 0, 90), (40, 0, 90)],
|
||
[(0, 40, 0), (0, -40, 0)],
|
||
[(-30, 0, 90), (40, 0, 90)],
|
||
[(0, 40, 0), (0, -40, 0)],
|
||
[(-25, 0, 90), (40, 0, 90)],
|
||
[(0, 40, 0), (0, -40, 0)],
|
||
[(-20, 0, 90), (40, 0, 90)],
|
||
[(0, 40, 0), (0, -40, 0)],
|
||
[(-15, 0, 90), (40, 0, 90)],
|
||
[(0, 40, 0), (0, -40, 0)],
|
||
[(-10, 0, 90), (40, 0, 90)],
|
||
[(0, 40, 0), (0, -40, 0)],
|
||
[(-5, 0, 90), (40, 0, 90)],
|
||
[(0, 40, 0), (0, -40, 0)],
|
||
[(0, 0, 90), (40, 0, 90)],
|
||
[(0, 40, 0), (0, -40, 0)],
|
||
[(-5, 0, 90), (40, 0, 90)],
|
||
[(0, 40, 0), (0, -40, 0)],
|
||
[(-10, 0, 90), (40, 0, 90)],
|
||
[(0, 40, 0), (0, -40, 0)],
|
||
[(-15, 0, 90), (40, 0, 90)],
|
||
[(0, 40, 0), (0, -40, 0)],
|
||
[(-20, 0, 90), (40, 0, 90)],
|
||
[(0, 40, 0), (0, -40, 0)],
|
||
[(-25, 0, 90), (40, 0, 90)],
|
||
[(0, 40, 0), (0, -40, 0)],
|
||
[(-30, 0, 90), (40, 0, 90)],
|
||
[(0, 40, 0), (0, -40, 0)],
|
||
[(-35, 0, 90), (40, 0, 90)],
|
||
[(0, 40, 0), (0, -40, 0)],
|
||
[(-40, 0, 90), (40, 0, 90)],
|
||
[(0, 40, 0), (0, -40, 0)],
|
||
]
|
||
|
||
def get_tower_blueprint_2():
|
||
# 使用极坐标定义第二种塔型的蓝图
|
||
return [
|
||
[(-30, 0, 90), (30, 0, 90)],
|
||
[(-3, 30, -5), (3, -30, -5)],
|
||
[(-29, -5, 80), (29, 5, 80)],
|
||
[(-8, 29, -15), (8, -29, -15)],
|
||
[(-28, -10, 70), (28, 10, 70)],
|
||
[(-13, 27, -25), (13, -27, -25)],
|
||
[(-27, -13, 60), (27, 13, 60)],
|
||
[(-18, 25, -35), (18, -25, -35)],
|
||
[(-26, -18, 50), (26, 18, 50)],
|
||
[(-21, 23, -45), (21, -23, -45)],
|
||
[(-25, -21, 40), (25, 21, 40)],
|
||
[(-22, 20, -55), (22, -20, -55)],
|
||
[(-24, -22, 30), (24, 22, 30)],
|
||
[(-20, 19, -65), (20, -19, -65)],
|
||
[(-23, -20, 20), (23, 20, 20)],
|
||
[(-19, 18, -75), (19, -18, -75)],
|
||
[(-22, -19, 10), (22, 19, 10)],
|
||
[(-17, 17, -85), (17, -17, -85)],
|
||
[(-21, -17, 0), (21, 17, 0)],
|
||
[(-16, 16, -95), (16, -16, -95)],
|
||
]
|
||
|
||
def get_tower_blueprint_3():
|
||
# 第三种塔型的蓝图（相对坐标）
|
||
return [
|
||
[(-60, -60, 45), (60, -60, 135), (-60, 60, 135), (60, 60, 45)],
|
||
[(0, -80, 0), (80, 0, 90), (0, 80, 0), (-80, 0, 90)],
|
||
[(-55, -55, 45), (55, -55, 135), (-55, 55, 135), (55, 55, 45)],
|
||
[(0, -75, 0), (75, 0, 90), (0, 75, 0), (-75, 0, 90)],
|
||
[(-50, -50, 45), (50, -50, 135), (-50, 50, 135), (50, 50, 45)],
|
||
[(0, -70, 0), (70, 0, 90), (0, 70, 0), (-70, 0, 90)],
|
||
[(-45, -45, 45), (45, -45, 135), (-45, 45, 135), (45, 45, 45)],
|
||
[(0, -65, 0), (65, 0, 90), (0, 65, 0), (-65, 0, 90)],
|
||
[(-40, -40, 45), (40, -40, 135), (-40, 40, 135), (40, 40, 45)],
|
||
[(0, -60, 0), (60, 0, 90), (0, 60, 0), (-60, 0, 90)],
|
||
[(-40, 0, 90), (40, 0, 90)],
|
||
[(0, 40, 0), (0, -40, 0)],
|
||
]
|
||
|
||
def main():
|
||
pygame.init()
|
||
screen = pygame.display.set_mode((SCREEN_WIDTH, SCREEN_HEIGHT))
|
||
pygame.display.set_caption("Stack Simulation")
|
||
|
||
# 加载塔型图片
|
||
tower_images = [
|
||
pygame.image.load("tower1.png"),
|
||
pygame.image.load("tower2.png"),
|
||
pygame.image.load("tower3.png")
|
||
]
|
||
|
||
# 显示塔选择界面
|
||
selected_tower = show_tower_selection(screen)
|
||
|
||
# 根据选择的塔型设置相应的蓝图和图片
|
||
if selected_tower == 0:
|
||
tower_blueprint = get_tower_blueprint_1()
|
||
elif selected_tower == 1:
|
||
tower_blueprint = get_tower_blueprint_2()
|
||
else:
|
||
tower_blueprint = get_tower_blueprint_3()
|
||
|
||
tower = Tower(tower_blueprint, tower_images[selected_tower])
|
||
clock = pygame.time.Clock()
|
||
current_block = None
|
||
similarity_score = 0 # 初始化相似性评分
|
||
font = pygame.font.Font(None, 36) # 创建字体对象
|
||
|
||
running = True
|
||
while running:
|
||
for event in pygame.event.get():
|
||
if event.type == pygame.QUIT:
|
||
running = False
|
||
elif event.type == pygame.MOUSEBUTTONDOWN:
|
||
if event.button == 1:
|
||
x, y = pygame.mouse.get_pos()
|
||
current_block = Block(x, y, 100, 20, 0, tower.current_layer)
|
||
elif event.type == pygame.MOUSEMOTION:
|
||
if current_block:
|
||
current_block.x, current_block.y = pygame.mouse.get_pos()
|
||
# 实时计算当前位置的相似性评分
|
||
ideal_positions = tower.ideal_positions[tower.current_layer]
|
||
blocks_in_current_layer = sum(1 for block in tower.blocks if block.layer == tower.current_layer)
|
||
if blocks_in_current_layer < len(ideal_positions):
|
||
ideal_x, ideal_y, ideal_angle = ideal_positions[blocks_in_current_layer]
|
||
distance = np.sqrt((current_block.x - ideal_x)**2 + (current_block.y - ideal_y)**2) + abs(current_block.angle - ideal_angle)
|
||
# similarity_score = 100 * (1 - distance / (np.sqrt(WIDTH**2 + HEIGHT**2) + 360))
|
||
ratio = distance / (np.sqrt(WIDTH**2 + HEIGHT**2) + 360)
|
||
similarity_score = 100 * (1 - np.power(ratio, 0.5)) # 使用0.5次方增加敏感度
|
||
elif event.type == pygame.MOUSEBUTTONUP:
|
||
if event.button == 1 and current_block:
|
||
# 计算最终放置位置的相似性评分
|
||
ideal_positions = tower.ideal_positions[tower.current_layer]
|
||
blocks_in_current_layer = sum(1 for block in tower.blocks if block.layer == tower.current_layer)
|
||
if blocks_in_current_layer < len(ideal_positions):
|
||
ideal_x, ideal_y, ideal_angle = ideal_positions[blocks_in_current_layer]
|
||
distance = np.sqrt((current_block.x - ideal_x)**2 + (current_block.y - ideal_y)**2) + abs(current_block.angle - ideal_angle)
|
||
similarity_score = 100 * (1 - distance / (np.sqrt(WIDTH**2 + HEIGHT**2) + 360))
|
||
|
||
if tower.add_block_from_sim(current_block):
|
||
print(f"Block placed with similarity score: {similarity_score:.2f}")
|
||
current_block = None
|
||
else:
|
||
print("CANNOT place block here, there is already a block there.")
|
||
elif event.type == pygame.KEYDOWN:
|
||
# 处理所有按键事件
|
||
if event.key == pygame.K_r and current_block: # 按 'R' 键旋转当前块
|
||
current_block.angle += 15
|
||
elif event.key == pygame.K_SPACE: # 按 'Space' 键确认当前层完成放置
|
||
tower.increase_layer()
|
||
elif event.key == pygame.K_a: # 按 'A' 键自动放置
|
||
auto_block, new_similarity_score = tower.auto_place_block()
|
||
if auto_block:
|
||
if tower.add_block_from_sim(auto_block):
|
||
print(f"Successfully placed block at ({auto_block.x}, {auto_block.y}) with angle {auto_block.angle}")
|
||
similarity_score = new_similarity_score
|
||
else:
|
||
print("Cannot place block")
|
||
else:
|
||
print("Cannot find suitable position")
|
||
elif event.key == pygame.K_n: # 按 'N' 键自动放置新的一层
|
||
tower.auto_place_layer()
|
||
elif event.key == pygame.K_s: # 按 'S' 键扫描当前层
|
||
tower.scan_current_layer()
|
||
|
||
|
||
screen.fill(WHITE)
|
||
tower.draw(screen)
|
||
if current_block:
|
||
current_block.draw(screen)
|
||
|
||
# 显示相似性评分
|
||
score_text = font.render(f"Similarity Score: {similarity_score:.2f}", True, (0, 0, 0))
|
||
screen.blit(score_text, (10, 50))
|
||
|
||
# 计算并绘制当前层的重心
|
||
current_centroid = tower.calculate_current_layer_centroid()
|
||
if current_centroid:
|
||
pygame.draw.circle(screen, RED, (int(current_centroid[0]), int(current_centroid[1])), 5)
|
||
font = pygame.font.Font(None, 24)
|
||
|
||
# 绘制之前层的重心
|
||
for i, centroid in enumerate(tower.layer_centroids):
|
||
pygame.draw.circle(screen, BLACK, (int(centroid[0]), int(centroid[1])), 3)
|
||
font = pygame.font.Font(None, 24)
|
||
text = font.render(f"{i+1}", True, BLACK)
|
||
screen.blit(text, (int(centroid[0]) + 10, int(centroid[1]) - 10))
|
||
|
||
if not tower.check_stability():
|
||
font = pygame.font.Font(None, 36)
|
||
text = font.render("WARNING: Tower is NOT stable", True, RED)
|
||
screen.blit(text, (WIDTH // 2 - text.get_width() // 2, 20))
|
||
|
||
font = pygame.font.Font(None, 36)
|
||
layer_text = font.render(f"Current Layer: {tower.current_layer + 1}", True, BLACK)
|
||
screen.blit(layer_text, (10, 10))
|
||
|
||
tower.draw_tower_image(screen)
|
||
|
||
pygame.display.flip()
|
||
clock.tick(60)
|
||
|
||
pygame.quit()
|
||
|
||
if __name__ == "__main__":
|
||
main() |