Problem with finding the closest intersection

Question

The rays keep casting on the wrong "wall", but only if the lamp is more in the bottom right. If the lamp is in the left up corner everything is working fine.

I have tried a lot of things, but last time I had a problem I wrote I checked the formulars many times and in the end it was a problem with the formular so I am not even gonne try

(https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection)

function for finding the closest wall:

    def draw(self):
        bestdist = 1000000000000000000
        for obs in run.Obs:
            x1, y1 = obs.startp
            x2, y2 = obs.endp
            x3, y3 = run.lamp
            x4, y4 = self.maxendpoint

            d = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4)
            if d != 0:
                t = ((x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4)) / d
                u = ((x1 - x2) * (y1 - y3) - (y1 - y2) * (x1 - x3)) / d
                if 0 < t < 1 and u > 0:
                    px = round(x1 + t * (x2 - x1))
                    py = round(y1 + t * (y2 - y1))
                    dist = px**2+py**2
                    if dist < bestdist:
                        bestdist = dist
                        self.endpoint= [px, py]
                    # pygame.draw.circle(run.screen, pygame.Color('green'), (px, py), 3)
        if len(self.endpoint) == 2:
            pygame.draw.line(run.screen, pygame.Color('white'), run.lamp, self.endpoint)

the whole code:

import pygame
import sys
import math
import random as rd
import numpy as np

class Obs(object):
    def __init__(self, startp, endp):
        self.startp = startp
        self.endp = endp

    def drawww(self):
        pygame.draw.line(run.screen, pygame.Color('red'), (self.startp), (self.endp))


class rays(object):
    def __init__(self, maxendpoint):
        self.maxendpoint = maxendpoint
        self.endpoint = []

    def draw(self):
        bestdist = 1000000000000000000
        for obs in run.Obs:
            x1, y1 = obs.startp
            x2, y2 = obs.endp
            x3, y3 = run.lamp
            x4, y4 = self.maxendpoint

            d = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4)
            if d != 0:
                t = ((x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4)) / d
                u = ((x1 - x2) * (y1 - y3) - (y1 - y2) * (x1 - x3)) / d
                if 0 < t < 1 and u > 0:
                    px = round(x1 + t * (x2 - x1))
                    py = round(y1 + t * (y2 - y1))
                    dist = px**2+py**2
                    if dist < bestdist:
                        bestdist = dist
                        self.endpoint= [px, py]
                    # pygame.draw.circle(run.screen, pygame.Color('green'), (px, py), 3)
        if len(self.endpoint) == 2:
            pygame.draw.line(run.screen, pygame.Color('white'), run.lamp, self.endpoint)


class Control(object):
    def __init__(self):
        self.winw = 800
        self.winh = 800
        self.screen = pygame.display.set_mode((self.winw, self.winh))
        self.fps = 60
        self.clock = pygame.time.Clock()
        self.lamp = [round(self.winw/2), round(self.winh/2)]
        self.lampr = 13
        self.lines = []
        self.r = 5
        self.Obs = []
        self.angel = 0
        self.fov = 360
        self.scene = np.ones(self.fov)
        self.done = False
        self.makeobs()

    def event_loop(self):
        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                self.done = True
            elif event.type == pygame.KEYDOWN:
                if event.key == pygame.K_F5:
                    self.__init__()
                elif event.key == pygame.K_LEFT:
                    if self.angel <= 0:
                        self.angel = 360
                    else:
                        self.angel -= 5
                elif event.key == pygame.K_RIGHT:
                    if self.angel >= 360:
                        self.angel = 0
                    else:
                        self.angel += 5
                elif event.key == pygame.K_w:
                    self.lamp[1] -= 10
                elif event.key == pygame.K_a:
                    self.lamp[0] -= 10
                elif event.key == pygame.K_s:
                    self.lamp[1] += 10
                elif event.key == pygame.K_d:
                    self.lamp[0] += 10
                elif event.key == pygame.K_y:
                    pass

        if pygame.mouse.get_pressed() == (1, 0, 0):
            if pygame.mouse.get_pos()[0] > 800:
                self.lamp = [799, pygame.mouse.get_pos()[1]]
            else:
                self.lamp[0] = pygame.mouse.get_pos()[0]
                self.lamp[1] = pygame.mouse.get_pos()[1]

    def draw(self):
        self.screen.fill((pygame.Color('black')))
        pygame.draw.circle(self.screen, pygame.Color('white'), self.lamp, self.lampr)
        for obs in self.Obs:
            obs.drawww()
        for line in self.lines:
            line.draw()

    def makeobs(self):
        for i in range(2):
            self.Obs.append(Obs((rd.randint(0, self.winw), rd.randint(0, self.winh)),
                                (rd.randint(0, self.winw), rd.randint(0, self.winh))))
        # self.Obs.append(Obs((0, 0), (self.winw, 0)))
        # self.Obs.append(Obs((0, 0), (0, self.winh)))
        # self.Obs.append(Obs((self.winw, 0), (self.winw, self.winh)))
        # self.Obs.append(Obs((0, self.winh), (self.winw, self.winh)))

    def createlines(self):
        self.lines.clear()
        for angle in range(self.angel, self.angel+self.fov):
            angle = math.radians(angle)
            self.lines.append(rays([self.lamp[0] + math.cos(angle), self.lamp[1] + math.sin(angle)]))

    def main_loop(self):
        while not self.done:
            self.event_loop()
            self.createlines()
            self.draw()
            pygame.display.update()
            self.clock.tick(self.fps)
            pygame.display.set_caption(f"Draw  FPS: {self.clock.get_fps()}")


if __name__ == '__main__':
    run = Control()
    run.main_loop()
    pygame.quit()
    sys.exit()

Expected it to work no matter where the lamp is.

Answer 1

The code

px = round(x1 + t * (x2 - x1))
py = round(y1 + t * (y2 - y1))
dist = px**2+py**2

doesn't make any sense, because (py, py) is a point, and so dist = px**2+py**2 is the squared Euclidean distance from the origin (0, 0) to (py, py).

You've to calculate the distance from (x3, x4) to the intersection point:

vx = u * (x4 - x3)
vy = u * (y4 - y3)
dist = vx**2+vy**2

Further there is an issue when in the calculation of u:

u = ((x1 - x2) * (y1 - y3) - (y1 - y2) * (x1 - x3)) / d
u = ((x1 - x3) * (y1 - y2) - (y1 - y3) * (x1 - x2)) / d

Method class rays:

class rays(object):
    def __init__(self, maxendpoint):
        self.maxendpoint = maxendpoint
        self.endpoint = []

    def draw(self):
        bestdist = 1000000000000000000
        for obs in run.Obs:
            x1, y1 = obs.startp
            x2, y2 = obs.endp
            x3, y3 = run.lamp
            x4, y4 = self.maxendpoint

            d = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4)
            if d != 0:
                t = ((x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4)) / d
                u = ((x1 - x3) * (y1 - y2) - (y1 - y3) * (x1 - x2)) / d
                if 0 < t < 1 and u > 0:
                    vx = u * (x4 - x3)
                    vy = u * (y4 - y3)
                    dist = vx**2+vy**2
                    if dist < bestdist:
                        px = round(x3 + u * (x4 - x3))
                        py = round(y3 + u * (y4 - y3))
                        bestdist = dist
                        self.endpoint= [px, py]
                    # pygame.draw.circle(run.screen, pygame.Color('green'), (px, py), 3)
        if len(self.endpoint) == 2:
            pygame.draw.line(run.screen, pygame.Color('white'), run.lamp, self.endpoint)

---

Minimal example: repl.it/@Rabbid76/PyGame-IntersectAndCutLines

import pygame
import math
import random

def intersect(obstacles, P0, P1):
    bestdist = 1000000000000000000
    endpoint = P1
    for Q0, Q1 in obstacles:
        d = (P1[0]-P0[0]) * (Q1[1]-Q0[1]) + (P1[1]-P0[1]) * (Q0[0]-Q1[0]) 
        if d != 0:
            t = ((Q0[0]-P0[0]) * (Q1[1]-Q0[1]) + (Q0[1]-P0[1]) * (Q0[0]-Q1[0])) / d
            u = ((Q0[0]-P0[0]) * (P1[1]-P0[1]) + (Q0[1]-P0[1]) * (P0[0]-P1[0])) / d
            if 0 <= t <= 1 and 0 <= u <= 1:
                vx, vy = (P1[0]-P0[0]) * t, (P1[1]-P0[1]) * t
                dist = vx*vx + vy*vy
                if dist < bestdist:
                    px, py = round(Q1[0] * u + Q0[0] * (1-u)), round(Q1[1] * u + Q0[1] * (1-u))
                    bestdist = dist
                    endpoint = (px, py)
    return endpoint

def createRays(center):
    return [(center[0] + 1200 * math.cos(angle), center[1] + 1200 * math.sin(angle)) for angle in range(0, 360, 10)]

def createObstacles(surface):
    w, h = surface.get_size()
    return [((random.randrange(w), random.randrange(h)), (random.randrange(w), random.randrange(h))) for _ in range(5)]

window = pygame.display.set_mode((800, 800))
clock = pygame.time.Clock()

origin = window.get_rect().center
rays = createRays(origin)
obstacles = createObstacles(window)

move_center = True
run = True
while run:
    clock.tick(60)
    for event in pygame.event.get():
        if event.type == pygame.QUIT:
            run = False
        if event.type == pygame.MOUSEBUTTONDOWN:
            obstacles = createObstacles(window) 
        if event.type == pygame.KEYDOWN:
            move_center = not move_center

    if move_center:
        origin = pygame.mouse.get_pos()
        rays = createRays(origin) 
        
    window.fill(0)
    for endpoint in rays:
        endpoint = intersect(obstacles, origin, endpoint)
        pygame.draw.line(window, (128, 128, 128), origin, endpoint)
    pygame.draw.circle(window, (255, 255, 255), origin, 10)
    for start, end in obstacles:
        pygame.draw.line(window, (255, 0, 0), start, end)
    pygame.display.flip()

pygame.quit()
exit()