73 lines
2.1 KiB
Python
73 lines
2.1 KiB
Python
# Constructs line equastions from a set of polar cords that represent a room
|
|
|
|
# Invoke python mklines.py <filename>
|
|
|
|
import csv
|
|
import sys
|
|
import math
|
|
from os.path import exists as file_exists
|
|
from basictypes import PointWithID, StraightLine, point_from_polar
|
|
|
|
|
|
def main():
|
|
filename = get_filename()
|
|
if filename == -1: return -1
|
|
if not file_exists(filename):
|
|
perr('File does not exist')
|
|
return -1
|
|
points = read_csv(filename, remove_header=True)
|
|
if not valid_data(points): # Assumes header is removed
|
|
perr('Invalid or corrupt samples')
|
|
return -1
|
|
#for val in points: print(val.get_cartesian())
|
|
lines = list()
|
|
for i in range(0, len(points), 2):
|
|
p1 = points[i]
|
|
p2 = points[i+1]
|
|
print('p1 ', p1.get_x(), p1.get_y())
|
|
print('p2 ', p2.get_x(), p2.get_y())
|
|
line = StraightLine(p1, p2)
|
|
lines.append(line)
|
|
print('line ', line.get_m(), line.get_b())
|
|
intercepts = list()
|
|
for i, line in enumerate(lines):
|
|
p = None # This is risky
|
|
if i == len(lines) - 1:
|
|
print("last")
|
|
p = lines[i].intercept(lines[0])
|
|
# Last line is a special case
|
|
else:
|
|
p = lines[i].intercept(lines[i+1])
|
|
print(p.get_x(), p.get_y())
|
|
return 0
|
|
|
|
def read_csv(filename: str, remove_header=False) -> list[PointWithID]:
|
|
tmp_data = list() # New list of points (PointWithID)
|
|
with open(filename, 'r') as csv_file:
|
|
reader = csv.reader(csv_file)
|
|
for i, row in enumerate(reader):
|
|
if remove_header and i == 0: continue # Skip header
|
|
# Assumes wall_id,r,theta
|
|
point_tmp = point_from_polar(float(row[1]), float(row[2]), row[0])
|
|
tmp_data.append(point_tmp)
|
|
|
|
return tmp_data
|
|
|
|
def valid_data(samples:list) -> bool:
|
|
# Check if we have an even number of samples
|
|
return len(samples) % 2 == 0
|
|
|
|
|
|
def get_filename() -> str:
|
|
try:
|
|
filename = sys.argv[1]
|
|
return filename
|
|
except IndexError:
|
|
perr('Provide target file name')
|
|
return -1
|
|
|
|
def perr(*args):
|
|
print(*args, file=sys.stderr)
|
|
|
|
if __name__ == '__main__':
|
|
exit(main()) |