116 lines
3.9 KiB
Python
116 lines
3.9 KiB
Python
import pygame
|
||
|
||
from math import dist, cos, sin, atan2, pi
|
||
|
||
class Laby:
|
||
L1=1.50
|
||
L2=2.00
|
||
L3=0.20
|
||
L4=0.40
|
||
L5=0.20
|
||
L6=0.40
|
||
L7=L4+L2-L5-L6
|
||
L8=0.40
|
||
L9=0.40
|
||
L10=0.40
|
||
L11=1.10
|
||
L12=2.95
|
||
L13=0.15
|
||
L14=0.40
|
||
L15=0.90
|
||
L16=0.20
|
||
L17=0.20
|
||
walls = [
|
||
((0,0),(0,L17)),
|
||
#((0,L17+L4),(0,L17+L4+L2)),
|
||
((0,0),(L8,0)),
|
||
((L8,0),(L8,L7)),
|
||
((L8,L7),(L8+L3,L7)),
|
||
((L8+L3,L7),(L8+L3,0)),
|
||
((L8+L3,0),(L8+L3+L9,0)),
|
||
((L8+L3+L9,0),(L8+L3+L9,L17)),
|
||
|
||
((0,L17+L4),(0,L17+L4+L2)),
|
||
((0,L17+L4+L2),(L8,L17+L4+L2)),
|
||
((L8,L17+L4+L2),(L8,L17+L4+L2-L5)),
|
||
((L8,L17+L4+L2-L5),(L8+L3,L17+L4+L2-L5)),
|
||
((L8+L3,L17+L4+L2-L5),(L8+L3,L17+L4+L2)),
|
||
((L8+L3,L17+L4+L2),(L8+L3+L9,L17+L4+L2)),
|
||
((L8+L3+L9,L17+L4+L2),(L8+L3+L9,L17+L10)),
|
||
|
||
((L8+L3+L9,L17),(L8+L3+L9+L14,L17+L13)),
|
||
((L8+L3+L9+L14,L17+L13),(L8+L3+L9+L11-L16,L17)),
|
||
((L8+L3+L9,L17+L10),(L8+L3+L9+L14,L17+L10+L13)),
|
||
((L8+L3+L9+L14,L17+L10+L13),(L8+L3+L9+L11-L16,L17+L10)),
|
||
]
|
||
|
||
def __init__(self, pos):
|
||
self.pos = pos
|
||
self.walls = [([w+p for w,p in zip(l[0], self.pos)],[w+p for w,p in zip(l[1], self.pos)]) for l in self.walls]
|
||
|
||
def draw(self, surf):
|
||
for l in self.walls:
|
||
pygame.draw.line(surf, (255,255,255), [100*wp for wp in l[0]], [100*wp for wp in l[1]])
|
||
|
||
def get_closest_wall_dist(self, ray):
|
||
inters = [self.get_inter(ray,w) for w in self.walls]
|
||
inters = list(filter(None, inters))
|
||
if len(inters) == 0:
|
||
return None
|
||
dists = [dist(ray[0], inter) for inter in inters]
|
||
index_min = min(range(len(dists)), key=dists.__getitem__)
|
||
closest = inters[index_min]
|
||
return closest
|
||
|
||
def get_closest_wall_dist(self, center, dir, lgt, scatter):
|
||
inters = [self.get_inter_area(center, dir, lgt, scatter, w) for w in self.walls]
|
||
inters = list(filter(None, inters))
|
||
if len(inters) == 0:
|
||
return None
|
||
return self.closest_point(center, inters)
|
||
|
||
def get_inter(self,l1,l2):
|
||
(x1,y1),(x2,y2) = l1
|
||
(x3,y3),(x4,y4) = l2
|
||
# https://en.wikipedia.org/wiki/Line–line_intersection#Given_two_points_on_each_line_segment
|
||
tn = (x1-x3)*(y3-y4) - (y1-y3)*(x3-x4)
|
||
td = (x1-x2)*(y3-y4) - (y1-y2)*(x3-x4)
|
||
un = (x1-x3)*(y1-y2) - (y1-y3)*(x1-x2)
|
||
ud = (x1-x2)*(y3-y4) - (y1-y2)*(x3-x4)
|
||
if td == 0 or ud == 0:
|
||
return None
|
||
t = tn/td
|
||
u = un/ud
|
||
if not (0 <= t <= 1 and 0 <= u <= 1):
|
||
return None
|
||
pint = (x1+t*(x2-x1),y1+t*(y2-y1))
|
||
return pint
|
||
|
||
def get_inter_area(self, center, dir, lgt, scatter, l):
|
||
endL = (center[0]+lgt*sin(dir-scatter), center[1]+lgt*cos(dir-scatter))
|
||
endR = (center[0]+lgt*sin(dir+scatter), center[1]+lgt*cos(dir+scatter))
|
||
interL = self.get_inter((center,endL), l)
|
||
interR = self.get_inter((center,endR), l)
|
||
interE = self.get_inter((endL, endR), l)
|
||
inters = [interL, interR, interE]
|
||
inters = list(filter(None, inters))
|
||
line_points_in_area = list(filter(lambda p: self.point_in_area(center, dir, lgt, scatter, p), l))
|
||
inters += line_points_in_area
|
||
if len(inters) == 0:
|
||
return None
|
||
return self.closest_point(center, inters)
|
||
|
||
def point_in_area(self, center, dir, lgt, scatter, p):
|
||
pdir = atan2(p[0]-center[0], p[1]-center[1])%(2*pi)
|
||
dirn = (dir)%(2*pi)
|
||
if abs(dirn-pdir)>scatter and abs(dirn-pdir+2*pi)>scatter:
|
||
return False
|
||
if dist(center, p) > lgt:
|
||
return False
|
||
return True
|
||
|
||
def closest_point(self, pc, ps):
|
||
dists = [dist(pc, p) for p in ps]
|
||
index_min = min(range(len(dists)), key=dists.__getitem__)
|
||
return ps[index_min]
|