Code for using beacons for determing location

Thought this might be of interest to anyone wanting to use beacons of known location by the angle between them to determine the position of a viewer (robot),

Apologies if I have already posted it before. A good example however of using ChatGPT.

I used ChatGPT to generate the Python and Processing language versions. Amazing how they just worked without tweaking. It failed with a C++ version. Python can be slow for low level programming.

Original FreeBASIC demo.

'=========================================================================
' modified version of this Rosetta code example
''https://rosettacode.org/wiki/Find_the_intersection_of_two_lines#QBasic
'=========================================================================

#Define NaN 0 / 0   ' FreeBASIC returns -1.#IND

'some useful defines
Const Pi = 4 * Atn(1)
Dim Shared As single TwoPi = 8 * Atn(1)
Dim Shared As single RtoD = 180 / Pi   ' radians * RtoD = degrees
Dim Shared As single DtoR = Pi / 180   ' degrees * DtoR = radians

screenres 800,600,32
dim as integer mx,my,mb
dim as single x1,y1,x2,y2,x3,y3,x4,y4,a1,a2

'positions of two beacons
x1 = 150
y1 = 150
x2 = 500
y2 = 300

do
    getmouse mx,my,,mb
    screenlock
    cls
    locate 2,2
    a1 = atan2(my-y1,mx-x1)*RtoD
    a2 = atan2(my-y2,mx-x2)*RtoD

    if a1<0 then a1 = a1 + 360
    if a2<0 then a2 = a2 + 360

    circle (x1,y1),2,rgb(255,0,0),,,,f
    circle (x1,y1),5,rgb(255,0,0)
    draw string (x1+5,y1),"A1 = (" & x1 & ","& y1 & ")  angle =" & a1 'int(a1)
    circle (x2,y2),2,rgb(255,255,0),,,,f
    circle (x2,y2),5,rgb(255,255,0)
    draw string (x2+5,y2),"A2 = (" & x2 & ","& y2 & ")  angle =" & a2 'int(a2)

    
    circle (mx,my),2,rgb(0,0,255),,,,f
    circle (mx,my),5,rgb(100,100,255)


    x3 = x1+Cos(a1*DtoR)
    y3 = y1+Sin(a1*DtoR)

    x2 = x2
    x4 = x2+Cos(a2*DtoR)
    y2 = y2
    y4 = y2+Sin(a2*DtoR)
    
    Dim As single a1 = y3 - y1
    Dim As single b1 = x1 - x3
    Dim As single c1 = a1 * x1 + b1 * y1
    Dim As single a2 = y4 - y2
    Dim As single b2 = x2 - x4
    Dim As single c2 = a2 * x2 + b2 * y2
    
    Dim As single det = a1 * b2 - a2 * b1

    x3 = (b2 * c1 - b1 * c2) / det
    y3 = (a1 * c2 - a2 * c1) / det

    circle (x3, y3),8,rgb(200,0,200)
    
    line (x3,y3)-(x1,y1),rgb(255,0,0)
    line (x3,y3)-(x2,y2),rgb(0,255,0)
    
    locate 4,2
    print "mx =";mx;" my =";my
    print
    'print " x3 =";int(x3+0.5);"  y3 =";int(y3+0.5)
    print " x3 =";(x3+0.5);"  y3 =";(y3+0.5)
    
    screenunlock
    
    sleep 2
loop until multikey(&H01)

ChatGPT translation to Python language:

import pygame
import math
import sys
import time

# Initialize Pygame
pygame.init()

# Screen setup
WIDTH, HEIGHT = 800, 600
screen = pygame.display.set_mode((WIDTH, HEIGHT))
pygame.display.set_caption("Beacon Positioning")
clock = pygame.time.Clock()

# Useful constants
Pi = math.atan(1) * 4
TwoPi = 8 * math.atan(1)
RtoD = 180 / Pi  # radians to degrees
DtoR = Pi / 180  # degrees to radians

# Beacon positions
x1, y1 = 150, 150
x2, y2 = 500, 300

def draw_text(surface, text, position, color=(255,255,255)):
    font = pygame.font.SysFont(None, 24)
    text_surface = font.render(text, True, color)
    surface.blit(text_surface, position)

running = True
while running:
    for event in pygame.event.get():
        if event.type == pygame.QUIT:
            running = False

    # Get mouse position
    mx, my = pygame.mouse.get_pos()

    # Calculate angles from beacons to mouse
    a1 = math.atan2(my - y1, mx - x1) * RtoD
    a2 = math.atan2(my - y2, mx - x2) * RtoD

    if a1 < 0:
        a1 += 360
    if a2 < 0:
        a2 += 360

    # Clear screen
    screen.fill((0, 0, 0))

    # Draw beacons
    pygame.draw.circle(screen, (255, 0, 0), (x1, y1), 5)
    pygame.draw.circle(screen, (255, 0, 0), (x1, y1), 2)
    draw_text(screen, f"A1 = ({x1},{y1}) angle={a1:.2f}", (x1 + 5, y1))
    
    pygame.draw.circle(screen, (255, 255, 0), (x2, y2), 5)
    pygame.draw.circle(screen, (255, 255, 0), (x2, y2), 2)
    draw_text(screen, f"A2 = ({x2},{y2}) angle={a2:.2f}", (x2 + 5, y2))
    
    # Draw mouse position
    pygame.draw.circle(screen, (0, 0, 255), (mx, my), 5)
    pygame.draw.circle(screen, (100, 100, 255), (mx, my), 2)

    # Compute points for the lines based on angles
    x3 = x1 + math.cos(a1 * DtoR)
    y3 = y1 + math.sin(a1 * DtoR)

    x4 = x2 + math.cos(a2 * DtoR)
    y4 = y2 + math.sin(a2 * DtoR)

    # Line equations coefficients
    a1_coef = y3 - y1
    b1_coef = x1 - x3
    c1 = a1_coef * x1 + b1_coef * y1

    a2_coef = y4 - y2
    b2_coef = x2 - x4
    c2 = a2_coef * x2 + b2_coef * y2

    # Determinant
    det = a1_coef * b2_coef - a2_coef * b1_coef

    if det != 0:
        # Intersection point
        xi = (b2_coef * c1 - b1_coef * c2) / det
        yi = (a1_coef * c2 - a2_coef * c1) / det

        # Draw the intersection point
        pygame.draw.circle(screen, (200, 0, 200), (int(xi), int(yi)), 8)

        # Draw lines from intersection to beacons
        pygame.draw.line(screen, (255, 0, 0), (xi, yi), (x1, y1))
        pygame.draw.line(screen, (0, 255, 0), (xi, yi), (x2, y2))
        
        # Display mouse coordinates
        draw_text(screen, f"mx = {mx} my = {my}", (10, 10))
        # Display intersection point
        draw_text(screen, f"x3 = {xi:.2f} y3 = {yi:.2f}", (10, 40))
    else:
        # Lines are parallel; no intersection
        draw_text(screen, "Lines are parallel, no intersection.", (10, 40))
        draw_text(screen, f"mx = {mx} my = {my}", (10, 10))
    
    pygame.display.flip()

    # Delay for 2 seconds
    time.sleep(2)

pygame.quit()
sys.exit()

ChatGPT translation to Processing language:

// Define constants
final float Pi = 4 * atan(1);
final float TwoPi = 8 * atan(1);
final float RtoD = 180.0 / Pi;  // Radians to Degrees
final float DtoR = Pi / 180.0;  // Degrees to Radians

// Beacon positions
float x1 = 150;
float y1 = 150;
float x2 = 500;
float y2 = 300;

void setup() {
  size(800, 600);
  frameRate(30);
  noStroke();
  textSize(12);
}

void draw() {
  background(255);
  
  // Get mouse position
  float mx = mouseX;
  float my = mouseY;
  int mb = mouseButton; // Not used directly here, but for completeness
  
  // Calculate angles from beacons to mouse
  float a1 = atan2(my - y1, mx - x1) * RtoD;
  float a2 = atan2(my - y2, mx - x2) * RtoD;
  
  if (a1 < 0) a1 += 360;
  if (a2 < 0) a2 += 360;
  
  // Draw beacon 1
  fill(255, 0, 0);
  ellipse(x1, y1, 10, 10);
  fill(0);
  text("A1 = (" + int(x1) + "," + int(y1) + ")  angle = " + nf(a1, 0, 2), x1 + 5, y1);
  
  // Draw beacon 2
  fill(255, 255, 0);
  ellipse(x2, y2, 10, 10);
  fill(0);
  text("A2 = (" + int(x2) + "," + int(y2) + ")  angle = " + nf(a2, 0, 2), x2 + 5, y2);
  
  // Draw mouse position
  fill(0, 0, 255);
  ellipse(mx, my, 10, 10);
  fill(100, 100, 255);
  ellipse(mx, my, 20, 20);
  
  // Calculate points based on angles
  float x3 = x1 + cos(a1 * DtoR);
  float y3 = y1 + sin(a1 * DtoR);
  
  float x4 = x2 + cos(a2 * DtoR);
  float y4 = y2 + sin(a2 * DtoR);
  
  // Line-line intersection calculation
  float a1_coef = y3 - y1;
  float b1_coef = x1 - x3;
  float c1 = a1_coef * x1 + b1_coef * y1;
  
  float a2_coef = y4 - y2;
  float b2_coef = x2 - x4;
  float c2 = a2_coef * x2 + b2_coef * y2;
  
  float det = a1_coef * b2_coef - a2_coef * b1_coef;
  
  float xi = 0;
  float yi = 0;
  if (abs(det) > 0.0001) { // Prevent division by zero
    xi = (b2_coef * c1 - b1_coef * c2) / det;
    yi = (a1_coef * c2 - a2_coef * c1) / det;
  }
  
  // Draw intersection point
  fill(200, 0, 200);
  ellipse(xi, yi, 16, 16);
  
  // Draw lines from intersection to beacons
  stroke(255, 0, 0);
  line(xi, yi, x1, y1);
  stroke(0, 255, 0);
  line(xi, yi, x2, y2);
  noStroke();
  
  // Display coordinates
  fill(0);
  text("mx = " + int(mx) + " my = " + int(my), 10, 20);
  text("x3 = " + nf(xi, 0, 2) + "  y3 = " + nf(yi, 0, 2), 10, 40);
  
  // Optional: Delay similar to sleep 2 in FreeBASIC
  // In Processing, the frame rate controls update speed.
  // To mimic a delay, you could use delay(2000); but it's usually unnecessary here.
}