Sliding collisions circle/rectangle

test.py

import pygame, math, sys
from pygame.locals import *

#from pygame.math import Vector2
class Vector2(object):
    def __init__(self, x, y):
        self.x = x
        self.y = y

pygame.init()
fpsClock = pygame.time.Clock()

window = pygame.display.set_mode((640, 480))
pygame.display.set_caption("Collisions")

whiteColor = pygame.Color(255, 255, 255)
greenColor = pygame.Color(0, 255, 0)
redColor = pygame.Color(255, 0, 0)
blueColor = pygame.Color(0, 0, 255)

circle = Vector2(100, 100)

def round_vector(v):
    return (int(v.x), int(v.y))

class Rectangle(object):
    def __init__(self, x, y, w, h):
        self.x = x
        self.y = y
        self.w = w
        self.h = h
        self.x2 = x+w
        self.y2 = y+h

    def corners(self):
        yield Vector2(self.x, self.y)
        yield Vector2(self.x, self.y2)
        yield Vector2(self.x2, self.y2)
        yield Vector2(self.x2, self.y)

def round_rectangle(r):
    return (int(r.x), int(r.y), int(r.w), int(r.h))

rectangle = Rectangle(230, 130, 140, 140)

CIRCLE_RADIUS = 50

SPEED = 300.0/1000 # 300px/s

speed = Vector2(0, 0)

while True:
    window.fill(whiteColor)

    pygame.draw.circle(
            window, greenColor,
            round_vector(circle),
            CIRCLE_RADIUS)
    pygame.draw.rect(window, redColor, round_rectangle(rectangle))

    for event in pygame.event.get():
        if event.type == QUIT:
            pygame.quit()
            sys.exit()
        elif event.type == KEYDOWN:
            if event.key == K_LEFT:
                speed.x = -1
            elif event.key == K_RIGHT:
                speed.x = 1
            elif event.key == K_UP:
                speed.y = -1
            elif event.key == K_DOWN:
                speed.y = 1
        elif event.type == KEYUP:
            if event.key == K_ESCAPE:
                pygame.quit()
                sys.exit()
            elif event.key == K_LEFT:
                speed.x = max(speed.x, 0)
            elif event.key == K_RIGHT:
                speed.x = min(speed.x, 0)
            elif event.key == K_UP:
                speed.y = max(speed.y, 0)
            elif event.key == K_DOWN:
                speed.y = min(speed.y, 0)

    # Movement
    dt = fpsClock.get_time()
    new_circle = Vector2(
            circle.x + speed.x * dt * SPEED,
            circle.y + speed.y * dt * SPEED)

    # Collision detection
    aabb = Rectangle(
            new_circle.x - CIRCLE_RADIUS, new_circle.y - CIRCLE_RADIUS,
            2 * CIRCLE_RADIUS, 2 * CIRCLE_RADIUS)
    if (aabb.x2 > rectangle.x and aabb.x < rectangle.x2 and
            aabb.y2 > rectangle.y and aabb.y < rectangle.y2):
        pygame.draw.circle(window, redColor, (20, 20), 10)

        movement = Vector2(
                new_circle.x - circle.x,
                new_circle.y - circle.y)
        fixed = False

        # Collision between the circle and an edge of the rectangle
        if movement.x != 0 and rectangle.y < circle.y < rectangle.y2:
            pygame.draw.circle(window, greenColor, (40, 20), 10)
            if circle.x < rectangle.x  and aabb.x2 > rectangle.x: # Left
                new_circle.x = rectangle.x - CIRCLE_RADIUS
                fixed = True
            elif circle.x > rectangle.x2 and aabb.x < rectangle.x2: # Right
                new_circle.x = rectangle.x2 + CIRCLE_RADIUS
                fixed = True
        if movement.y != 0 and rectangle.x < circle.x < rectangle.x2:
            pygame.draw.circle(window, greenColor, (40, 40), 10)
            if circle.y < rectangle.y and aabb.y2 > rectangle.y: # Up
                new_circle.y = rectangle.y - CIRCLE_RADIUS
                fixed = True
            elif circle.y > rectangle.y2 and aabb.y < rectangle.y2: # Down
                new_circle.y = rectangle.y2 + CIRCLE_RADIUS
                fixed = True

        if (movement.x or movement.y) and not fixed:
            # Collision between the circle and a corner of the rectangle

            # x(t) = ix + dx*t
            # y(t) = iy + dy*t
            # (x(t) - cx)^2 + (y(t) - cy)^2 = R^2
            #
            # (ix + dx*t - cx)^2 + (iy + dy*t - cy)^2 = R^2
            # ((ix - cx) + dx*t)^2 + ((iy - cy) + dy*t)^2 = R^2
            # (ix - cx)^2 + dx^2*t^2 + 2*(ix - cx)*dx*t + (iy - cy)^2 + dy^2*t^2 + 2*(iy - cy)*dy*t = R^2
            # dx^2*t^2 + dy^2*t^2 + 2*(ix - cx)*dx*t + 2*(iy - cy)*dy*t + (ix - cx)^2 + (iy - cy)^2 - R^2 = 0
            # (dx^2 + dy^2)*t^2 + 2*((ix - cx)*dx + (iy - cy)*dy)*t + ((ix - cx)^2 + (iy - cy)^2 - R^2) = 0
            #
            # delta = 4*((ix - cx)*dx + (iy - cy)*dy)^2 - 4*(dx^2 + dy^2)*((ix - cx)^2 + (iy - cy)^2 - R^2)
            # (2*((cx - ix)*dx + (cy - iy)*dy) - sqrt(delta))/(2*(dx^2 + dy^2))
            # (2*((cx - ix)*dx + (cy - iy)*dy) + sqrt(delta))/(2*(dx^2 + dy^2))

            # Try each corner
            for corner in rectangle.corners():
                sqdist = (
                        (corner.x - new_circle.x)**2 +
                        (corner.y - new_circle.y)**2)
                if sqdist < CIRCLE_RADIUS**2:
                    pygame.draw.circle(window, blueColor, (20, 60), 10)
                    # dx^2 + dy^2
                    delta_A = movement.x**2 + movement.y**2
                    # 2*((ix - cx)*dx + (iy - cy)*dy)
                    delta_B = 2 * ((circle.x - corner.x) * movement.x + (circle.y - corner.y) * movement.y)
                    # (ix - cx)^2 + (iy - cy)^2 - R^2
                    delta_C = (circle.x - corner.x)**2 + (circle.y - corner.y)**2 - CIRCLE_RADIUS**2
                    delta = delta_B**2 - 4 * delta_A * delta_C
                    assert delta >= 0
                    # First solution is best solution!
                    # (2*((cx - ix)*dx + (cy - iy)*dy) - sqrt(delta))/(2*(dx^2 + dy^2))
                    t1 = (2*((corner.x - circle.x)*movement.x + (corner.y - circle.y)*movement.y) - math.sqrt(delta))/(2*(movement.x**2 + movement.y**2))

                    # Position of the circle when the collision occurred
                    collision = Vector2(
                            circle.x + movement.x * t1,
                            circle.y + movement.y * t1)

                    # Direction of movement since collision (perpendicular to relative contact position)
                    dir = Vector2(
                            corner.x - collision.x,
                            corner.y - collision.y)
                    l = math.sqrt(dir.x*dir.x + dir.y*dir.y)
                    dir = Vector2(
                            dir.y/l,
                            -dir.x/l)

                    movement_after = Vector2(
                            new_circle.x - collision.x,
                            new_circle.y - collision.y)
                    proj = dir.x * movement_after.x + dir.y * movement_after.y
                    new_circle = Vector2(
                            collision.x + dir.x * proj,
                            collision.y + dir.y * proj)
                    break

    circle = new_circle

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