-
Notifications
You must be signed in to change notification settings - Fork 1
/
main.py
65 lines (50 loc) · 1.78 KB
/
main.py
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
import sys, pygame
import time
from pygame.math import Vector2
pygame.init()
size = width, height = 640, 480
screen = pygame.display.set_mode(size)
points = list(map(Vector2, [(100, 100), (200, 100), (300, 100), (400, 100), (500, 100)]))
target = Vector2(450, 300)
target_speed = Vector2(3, 3)
rel_points = []
angles = []
max_angle = 360 # Adjust for limited angles
for i in range(1, len(points)):
rel_points.append(points[i] - points[i-1])
angles.append(0)
def solve_ik(i, endpoint, target):
if i < len(points) - 2:
endpoint = solve_ik(i+1, endpoint, target)
current_point = points[i]
angle = (endpoint-current_point).angle_to(target-current_point)
angles[i] += min(max(-3, angle), 3)
angles[i] = min(max(180-max_angle, (angles[i]+180)%360), 180+max_angle)-180
return current_point + (endpoint-current_point).rotate(angle)
def render():
black = 0, 0, 0
white = 255, 255, 255
screen.fill(white)
for i in range(1, len(points)):
prev = points[i-1]
cur = points[i]
pygame.draw.aaline(screen, black, prev, cur)
for point in points:
pygame.draw.circle(screen, black, (int(point[0]), int(point[1])), 5)
pygame.draw.circle(screen, black, (int(target[0]), int(target[1])), 10)
pygame.display.flip()
while 1:
for event in pygame.event.get():
if event.type == pygame.QUIT: sys.exit()
solve_ik(0, points[-1], target)
angle = 0
for i in range(1, len(points)):
angle += angles[i-1]
points[i] = points[i-1] + rel_points[i-1].rotate(angle)
target += target_speed
if target.x <= 0 or target.x >= width:
target_speed.x = -target_speed.x
if target.y <= 0 or target.y >= height:
target_speed.y = -target_speed.y
render()
pygame.time.wait(int(1000/60))