1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227

import pygame
from pygame.locals import *
import sys

class Rect:
    """Basic AABOX class"""

    def __init__(self, pos=(0,0), ext=(1,1)):
        """Constructs Rect from (center_x,center_y) and (half_width,half_height)-tuples"""
        self.pos = pos
        self.ext = ext

    def __repr__(self):
        """String representation for debugging"""
        return "(%f .. %f, %f .. %f)" % ( self.get_min(0), self.get_max(0), self.get_min(1), self.get_max(1) )

    def get_max( self, axis ):
        """Returns the maximal bound of the Rect on the given axis"""
        return self.pos[axis] + self.ext[axis]

    def get_min( self, axis ):
        """Returns the minimal bound of the Rect on the given axis"""
        return self.pos[axis] - self.ext[axis]

    def intersects(self, other):
        """Check if the Rect intersects with Rect other"""
        d = Vector(other.pos).sub( Vector(self.pos) )
        return \
            ( abs(d.get_x()) < (self.ext[0] + other.ext[0]) ) and \
            ( abs(d.get_y()) < (self.ext[1] + other.ext[1]) )

    def move(self,dp):
        """Moves the position of the Rect by Vector dp"""
        self.pos = Vector(self.pos).add(dp).pos

    def move_tuple(self,dp):
        """Moves the position of the Rect by tuple dp"""
        self.pos = (self.pos[0] + dp[0], self.pos[1] + dp[1])

    def get_moved(self, dp):
        """Returns copy of the Rect that was moved by Vector dp"""
        return Rect( Vector(self.pos).add(dp).pos, self.ext )

    def get_sdl_rect(self):
        """Returns a pygame-compatible representation of the Rect as a (x,y,w,h)-tuple"""
        return (self.get_min(0), self.get_min(1), self.ext[0] * 2.0, self.ext[1] * 2.0)


class Vector:
    """Basic 2D vector class"""

    def __init__(self, pos=(0,0)):
        """Constucts Vector from (x,y)-tuple"""
        self.pos = pos

    def __repr__(self):
        """String representation for debugging"""
        return "(%f, %f)" % ( self.pos[0], self.pos[1] )

    def add(self, other):
        """Returns a copy of the Vector to which Vector other was added"""
        return Vector( (self.pos[0] + other.pos[0], self.pos[1] + other.pos[1]) )

    def sub(self, other):
        """Returns a copy of the Vector from which Vector other was subtracted"""
        return Vector( (self.pos[0] - other.pos[0], self.pos[1] - other.pos[1]) )

    def mul(self, s):
        """Returns a copy of the Vector that was multiplied with scalar s"""
        return Vector( (self.pos[0] * s, self.pos[1] * s) )

    def get_x( self ):
        """Gets x-value of the Vector"""
        return self.pos[0]

    def get_y( self ):
        """Gets y-value of the Vector"""
        return self.pos[1]

    def get_axis(self,axis):
        """Gets the specified axis value of the Vector"""
        return self.pos[axis]


def sweep( ra, va, rb, vb ):
    """Does a sweep test for Rect ra with velocity vector va, and Rect rb with velocity Vector vb

Returns: A (collision,time,code)-tuple, where "collision" is True/False, "time" is the time of 
impact (0.0 <= time < 1.0) (in case "collision" is True), and "code" represents the exit condition 
of the function, used for debugging."""

    #print( "Rects: %s %s" % (ra,rb) )

    intersects = ra.intersects(rb)
    #print( "Intersects: %s" % intersects )
    if intersects:
        return (True,0.0,1)

    v = vb.sub(va)
    #print( "Relative velocity: %s" % v )

    axis_enter = [1.0, 1.0]
    axis_leave = [0.0, 0.0]

    for axis in range(2):

        if v.get_axis(axis) < 0.0:
            # Box B is moving to the left/bottom
            if rb.get_max(axis) <= ra.get_min(axis):
                # Box B is already to the left/bottom of Box B
                # No collision possible
                return (False,0.0,2)
            axis_enter[axis] = (rb.get_min(axis) - ra.get_max(axis)) / abs(v.get_axis(axis))
            axis_leave[axis] = (rb.get_max(axis) - ra.get_min(axis)) / abs(v.get_axis(axis))

        elif v.get_axis(axis) > 0.0:
            # Box B is moving to the right/top
            if rb.get_min(axis) >= ra.get_max(axis):
                # Box B is already to the right/top of Box B
                # No collision possible
                return (False,0.0,3)
            axis_enter[axis] = (ra.get_min(axis) - rb.get_max(axis)) / abs(v.get_axis(axis))
            axis_leave[axis] = (ra.get_max(axis) - rb.get_min(axis)) / abs(v.get_axis(axis))

        elif abs(rb.pos[axis] - ra.pos[axis]) < (ra.ext[axis] + rb.ext[axis]):
            # Box B is not moving along the current axis
            # but overlaps with Box A on current axis
            axis_enter[axis] = 0.0
            axis_leave[axis] = 1.0

        else:
            # Box B is not moving along the current axis
            # and does not overlap with Box A on current axis
            # No collision possible
            return (False,0.0,4)

        #print( "Axis %d: time %s .. %s" % (axis,axis_enter[axis],axis_leave[axis]) )

    if max(axis_enter) >= 1.0:
        # Collision takes place "in the future"
        return (False,0.0,5)
    if max(axis_enter) >= min(axis_leave):
        # No simultanious collision on both axes
        #print axis_enter, axis_leave
        return (False,0.0,6)

    return (True,max(0.0,max(axis_enter)),7)


def draw( screen, ra0, rac, ra1, rb0, rbc, rb1 ):
    """Draws the rects on the screen"""

    pygame.draw.rect( screen, (255,  0,  0), ra0.get_sdl_rect() )
    pygame.draw.rect( screen, (255,  0,  0), ra1.get_sdl_rect(), 1 )
    if rac:
        pygame.draw.rect( screen, (255,150,150), rac.get_sdl_rect() )

    pygame.draw.rect( screen, (  0,255,  0), rb0.get_sdl_rect() )
    pygame.draw.rect( screen, (  0,255,  0), rb1.get_sdl_rect(), 1 )
    if rbc:
        pygame.draw.rect( screen, (150,255,150), rbc.get_sdl_rect() )


def get_rect_at( rects, pos ):
    """Returns the first Rect from list "rects" that contains the (x,y)-tuple "pos" """
    for r in rects:
        dx = abs( pos[0] - r.pos[0] )
        dy = abs( pos[1] - r.pos[1] )

        if dx < r.ext[0] and dy < r.ext[1]:
            return r
    return None

if __name__ == "__main__":

    pygame.init()
    screen = pygame.display.set_mode((640, 480))

    ra0 = Rect( (100,100), (64.0,64.0) )
    ra1 = Rect( (100,300), (64.0,64.0) )

    rb0 = Rect( (400,100), (32.0,32.0) )
    rb1 = Rect( (400,200), (32.0,32.0) )

    va = Vector( ra1.pos ).sub( Vector( ra0.pos ) )
    vb = Vector( rb1.pos ).sub( Vector( rb0.pos ) )
    coll, time, code = sweep( ra0, va, rb0, vb )

    grabbed = None

    font = pygame.font.Font(None,24)

    while 1:
        for event in pygame.event.get():
            if event.type == QUIT:
                sys.exit(0)
            elif event.type == MOUSEBUTTONDOWN:
                grabbed = get_rect_at( [rb1,rb0,ra1,ra0], event.pos)
            elif event.type == MOUSEBUTTONUP:
                grabbed = None
            elif event.type == MOUSEMOTION:
                if grabbed:
                    #grabbed.move(Vector(event.rel))
                    grabbed.move_tuple(event.rel)
                    va = Vector( ra1.pos ).sub( Vector( ra0.pos ) )
                    vb = Vector( rb1.pos ).sub( Vector( rb0.pos ) )
                    coll, time, code = sweep( ra0, va, rb0, vb )

        rac = None
        rbc = None

        if coll:
            rac = ra0.get_moved( va.mul(time) )
            rbc = rb0.get_moved( vb.mul(time) )

        screen.fill((0,0,0))
        draw( screen, ra0, rac, ra1, rb0, rbc, rb1 )

        if coll:
            tmp = font.render("Collision at t=%.2f frames, code %d" % (time,code), 1, (255,255,255) )
            screen.blit( tmp, (8, 480 - 30) )
        else:
            tmp = font.render("No collision, code %d" % (code), 1, (255,255,255) )
            screen.blit( tmp, (8, 480 - 30) )

        pygame.display.update()
        pygame.time.delay(100)