gesture_rec/robot/map_simulator.py

import pygame
import sys
import math

class MapSimulator:
    def __init__(self, config):
        pygame.init()
        self.width = config.MAP_WIDTH
        self.height = config.MAP_HEIGHT
        self.screen = pygame.display.set_mode((self.width, self.height))
        pygame.display.set_caption("Robot Map Simulator")
        self.clock = pygame.time.Clock()
        self.font = pygame.font.SysFont(None, 24)

        self.obstacles = config.MAP_OBSTACLES
        self.start = config.START_POS
        self.finish = config.FINISH_POS
        self.robot_radius = config.ROBOT_RADIUS

        if config.ROBOT_IMAGE_PATH:
            try:
                self.robot_image = pygame.image.load(config.ROBOT_IMAGE_PATH)
                self.robot_image = pygame.transform.scale(self.robot_image,
                                                          (self.robot_radius*2, self.robot_radius*2))
            except:
                self.robot_image = None
        else:
            self.robot_image = None

        self.reset()

    def reset(self):
        self.x, self.y = self.start
        self.angle = 0.0
        self.linear_speed = 0.0
        self.angular_speed = 0.0
        self.game_over = False
        self.won = False

    def set_speeds(self, linear, angular):
        self.linear_speed = linear
        self.angular_speed = angular

    def update(self):
        if self.game_over or self.won:
            return

        dt = 1/30.0
        self.angle += self.angular_speed * dt * 2
        dx = self.linear_speed * math.cos(self.angle) * dt * 100
        dy = self.linear_speed * math.sin(self.angle) * dt * 100
        new_x = self.x + dx
        new_y = self.y + dy

        # Ограничение границами карты
        new_x = max(self.robot_radius, min(self.width - self.robot_radius, new_x))
        new_y = max(self.robot_radius, min(self.height - self.robot_radius, new_y))

        # Проверка столкновений с препятствиями
        if not self.check_collision(new_x, new_y):
            self.x, self.y = new_x, new_y
        else:
            self.game_over = True

        # Проверка финиша
        dist = math.hypot(self.x - self.finish[0], self.y - self.finish[1])
        if dist < self.robot_radius:
            self.won = True

    def check_collision(self, x, y):
        for ox, oy, ow, oh in self.obstacles:
            closest_x = max(ox, min(x, ox + ow))
            closest_y = max(oy, min(y, oy + oh))
            dist = math.hypot(x - closest_x, y - closest_y)
            if dist < self.robot_radius:
                return True
        return False

    def draw(self):
        self.screen.fill((255,255,255))

        for obs in self.obstacles:
            pygame.draw.rect(self.screen, (100,100,100), obs)

        pygame.draw.circle(self.screen, (0,255,0),
                           (int(self.finish[0]), int(self.finish[1])), self.robot_radius, 2)

        if self.robot_image:
            rotated = pygame.transform.rotate(self.robot_image, -math.degrees(self.angle))
            rect = rotated.get_rect(center=(int(self.x), int(self.y)))
            self.screen.blit(rotated, rect)
        else:
            nose = (self.x + self.robot_radius * math.cos(self.angle),
                    self.y + self.robot_radius * math.sin(self.angle))
            left = (self.x + self.robot_radius * math.cos(self.angle + 2.5),
                    self.y + self.robot_radius * math.sin(self.angle + 2.5))
            right = (self.x + self.robot_radius * math.cos(self.angle - 2.5),
                     self.y + self.robot_radius * math.sin(self.angle - 2.5))
            pygame.draw.polygon(self.screen, (0,0,255), [nose, left, right])

        texts = [
            f"Linear: {self.linear_speed:.2f}",
            f"Angular: {self.angular_speed:.2f}",
        ]
        y = 10
        for t in texts:
            surf = self.font.render(t, True, (0,0,0))
            self.screen.blit(surf, (10, y))
            y += 25

        if self.game_over:
            msg = self.font.render("GAME OVER – Press R to restart", True, (255,0,0))
            self.screen.blit(msg, (self.width//2 - 150, self.height//2))
        elif self.won:
            msg = self.font.render("YOU WIN! – Press R to restart", True, (0,100,0))
            self.screen.blit(msg, (self.width//2 - 120, self.height//2))

        pygame.display.flip()

    def handle_events(self):
        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                return False
            if event.type == pygame.KEYDOWN and event.key == pygame.K_r:
                self.reset()
        return True

    def step(self):
        if not self.handle_events():
            return False
        self.update()
        self.draw()
        self.clock.tick(30)
        return True

    def quit(self):
        pygame.quit()