Vector Collisions using PyGame

collisions.py

# This code is ugly and slow, just a reference.
# But is works (mostly)

from __future__ import annotations

from dataclasses import dataclass
from typing import Tuple

import pygame.draw
from pygame import Surface

WHITE = (255, 255, 255)
GREEN = (0, 255, 0)
BLUE = (0, 0, 255)
RED = (255, 0, 0)
PINK = (255, 0, 255)
YELLOW = (255, 255, 0)


@dataclass
class Vector2:
    x: float
    y: float

    def __add__(self, other: Vector2 | int | float) -> Vector2:
        if isinstance(other, Vector2):
            return Vector2(self.x + other.x, self.y + other.y)
        return Vector2(self.x + other, self.y + other)

    def __sub__(self, other: Vector2 | int | float) -> Vector2:
        if isinstance(other, Vector2):
            return Vector2(self.x - other.x, self.y - other.y)
        return Vector2(self.x - other, self.y - other)

    def __mul__(self, other: Vector2 | int | float) -> Vector2:
        if isinstance(other, Vector2):
            return Vector2(self.x * other.x, self.y * other.y)
        return Vector2(self.x * other, self.y * other)

    def __truediv__(self, other: Vector2 | int | float) -> Vector2:
        if isinstance(other, Vector2):
            return Vector2(self.x / other.x, self.y / other.y)
        return Vector2(self.x / other, self.y / other)

    def __pow__(self, other: Vector2 | int | float):
        if isinstance(other, Vector2):
            return Vector2(self.x ** other.x, self.y ** other.y)
        return Vector2(self.x ** other, self.y ** other)

    def __bool__(self):
        return not (self.x == 0 and self.y == 0)


@dataclass
class MovementVector:
    origin: Vector2
    destination: Vector2

    def render(self, screen: Surface):
        pygame.draw.line(screen, GREEN, (self.origin.x, self.origin.y), (self.destination.x, self.destination.y))


class Entity:

    def __init__(self, position: Vector2, size: Vector2):
        self.position = position
        self.last_position = position
        self.size = size
        self.collided = False

    def movement_vector(self) -> MovementVector:
        origin = self.last_position + (self.size / 2)
        destination = self.position + (self.size / 2)
        return MovementVector(origin, destination)

    def render(self, screen: Surface, color=None):
        if not color:
            color = PINK if self.collided else WHITE
        pygame.draw.rect(screen, color, (self.position.x, self.position.y, self.size.x, self.size.y), 1)

    def translate(self, value: Vector2):
        self.last_position = self.position
        self.position = self.position + value


# ---

def make_entity_sides(entity: Entity) -> Tuple[MovementVector, MovementVector, MovementVector, MovementVector]:
    upside = MovementVector(Vector2(entity.position.x,
                                    entity.position.y),
                            Vector2(entity.position.x + entity.size.x,
                                    entity.position.y))
    downside = MovementVector(Vector2(entity.position.x,
                                      entity.position.y + entity.size.y),
                              Vector2(entity.position.x + entity.size.x,
                                      entity.position.y + entity.size.y))
    leftside = MovementVector(Vector2(entity.position.x,
                                      entity.position.y),
                              Vector2(entity.position.x,
                                      entity.position.y + entity.size.y))
    rightside = MovementVector(Vector2(entity.position.x + entity.size.x,
                                       entity.position.y),
                               Vector2(entity.position.x + entity.size.x,
                                       entity.position.y + entity.size.y))
    return upside, downside, leftside, rightside


def expand_entity(to_expand: Entity, to_collide: Entity):
    return Entity(to_expand.position - (to_collide.size / 2) + 1,
                  to_expand.size + to_collide.size - 1)


def calculate_normal(movement_vector: MovementVector) -> Vector2:
    normal = Vector2(0, 0)

    if movement_vector.origin.x > movement_vector.destination.x:
        normal.x = 1
    elif movement_vector.origin.x < movement_vector.destination.x:
        normal.x = -1

    if movement_vector.origin.y > movement_vector.destination.y:
        normal.y = -1
    elif movement_vector.origin.y < movement_vector.destination.y:
        normal.y = 1

    return normal


def collides(to_collide: Entity, entity: Entity):
    dynamic_vector = to_collide.movement_vector()
    expanded = expand_entity(entity, to_collide)

    expanded.render(screen, BLUE)

    up, down, left, right = make_entity_sides(expanded)

    collides_right = intersect(dynamic_vector.origin,
                               dynamic_vector.destination,
                               right.origin,
                               right.destination)

    collides_left = intersect(dynamic_vector.origin,
                              dynamic_vector.destination,
                              left.origin,
                              left.destination)

    collides_up = intersect(dynamic_vector.origin,
                            dynamic_vector.destination,
                            up.origin,
                            up.destination)

    collides_down = intersect(dynamic_vector.origin,
                              dynamic_vector.destination,
                              down.origin,
                              down.destination)

    collides_diagonal = (collides_left and (collides_up or collides_down)) or \
                        (collides_right and (collides_up or collides_down))

    normals = calculate_normal(dynamic_vector)

    if collides_right:
        pygame.draw.line(screen, YELLOW, (right.origin.x, right.origin.y),
                         (right.destination.x, right.destination.y))
    if collides_left:
        pygame.draw.line(screen, YELLOW, (left.origin.x, left.origin.y),
                         (left.destination.x, left.destination.y))
    if collides_up:
        pygame.draw.line(screen, YELLOW, (up.origin.x, up.origin.y),
                         (up.destination.x, up.destination.y))
    if collides_down:
        pygame.draw.line(screen, YELLOW, (down.origin.x, down.origin.y),
                         (down.destination.x, down.destination.y))

    resolution = Vector2(0, 0)
    if collides_diagonal:
        if collides_left:
            resolution.x = expanded.position.x - dynamic_vector.destination.x
        elif collides_right:
            resolution.x = (expanded.position.x + expanded.size.x) - dynamic_vector.destination.x
        if collides_up:
            resolution.y = expanded.position.y - dynamic_vector.destination.y - 1
        elif collides_down:
            resolution.y = (expanded.position.y + expanded.size.y) - dynamic_vector.destination.y + 1

    elif collides_right or collides_left:
        if normals.x > 0:
            pygame.draw.line(screen, RED,
                             (expanded.position.x + expanded.size.x,
                              expanded.position.y + (expanded.size.y / 2)),
                             (expanded.position.x + expanded.size.x + 10,
                              expanded.position.y + (expanded.size.y / 2)))
            resolution.x = (expanded.position.x + expanded.size.x) - dynamic_vector.destination.x + 1
        elif normals.x < 0:
            pygame.draw.line(screen, RED,
                             (entity.position.x, entity.position.y + (entity.size.y / 2)),
                             (entity.position.x - 10, entity.position.y + (entity.size.y / 2)))
            resolution.x = expanded.position.x - dynamic_vector.destination.x - 1

    elif collides_up or collides_down:
        if normals.y > 0:
            pygame.draw.line(screen, RED,
                             (expanded.position.x + (expanded.size.x / 2), expanded.position.y),
                             (expanded.position.x + (expanded.size.x / 2),
                              expanded.position.y - 10))
            resolution.y = expanded.position.y - dynamic_vector.destination.y - 1
        elif normals.y < 0:
            pygame.draw.line(screen, RED,
                             (expanded.position.x + (expanded.size.x / 2),
                              expanded.position.y + expanded.size.y),
                             (expanded.position.x + (expanded.size.x / 2),
                              expanded.position.y + expanded.size.y + 10))
            resolution.y = (expanded.position.y + expanded.size.y) - dynamic_vector.destination.y + 1

    to_collide.position += resolution


# ---

def cross_product(p1: Vector2, p2: Vector2):
    return p1.x * p2.y - p2.x * p1.y


def on_segment(p1: Vector2, p2: Vector2, p: Vector2):
    return min(p1.x, p2.x) <= p.x <= max(p1.x, p2.x) and \
           min(p1.y, p2.y) <= p.y <= max(p1.y, p2.y)


def direction(p1: Vector2, p2: Vector2, p3: Vector2):
    dir = cross_product(p3 - p1, p2 - p1)
    if dir > 0:
        return 1  # Clockwise (right)
    if dir < 0:
        return -1  # Counterclockwise (left)
    return 0  # Collinear


def intersect(p1: Vector2, p2: Vector2, p3: Vector2, p4: Vector2):
    d1 = direction(p3, p4, p1)
    d2 = direction(p3, p4, p2)
    d3 = direction(p1, p2, p3)
    d4 = direction(p1, p2, p4)

    return (((d1 > 0 and d2 < 0) or (d1 < 0 and d2 > 0)) and
            ((d3 > 0 and d4 < 0) or (d3 < 0 and d4 > 0))) or \
           (d1 == 0 and on_segment(p3, p4, p1)) or \
           (d2 == 0 and on_segment(p3, p4, p2)) or \
           (d3 == 0 and on_segment(p1, p2, p3)) or \
           (d4 == 0 and on_segment(p1, p2, p4))


# ---

def move_entity(entity, speed):
    global pos_y, pos_x
    screen.fill((0, 0, 0))

    translation = Vector2(0, 0)
    keys = pygame.key.get_pressed()
    if keys[pygame.K_UP]:
        translation.y -= speed
    if keys[pygame.K_DOWN]:
        translation.y += speed
    if keys[pygame.K_LEFT]:
        translation.x -= speed
    if keys[pygame.K_RIGHT]:
        translation.x += speed

    for _ in pygame.event.get():
        pass

    entity.translate(translation)


# ---

static_entity = Entity(Vector2(350, 250), Vector2(100, 100))
moving_entity = Entity(Vector2(350, 100), Vector2(50, 50))

# ---

pygame.init()
screen = pygame.display.set_mode((800, 600))
clock = pygame.time.Clock()

speed = 2
pos_y = 300
pos_x = 300

while True:
    for event in pygame.event.get():
        pass

    move_entity(moving_entity, speed)

    dynamic_ray = MovementVector(Vector2(pos_x, pos_y), Vector2(pos_x + 100, pos_y + 100))
    collides(moving_entity, static_entity)

    static_entity.render(screen)
    moving_entity.render(screen)
    moving_entity.movement_vector().render(screen)

    pygame.display.update()
    clock.tick(60)