room_scan_sim/basictypes.py

# Container classes for points and lines

import math

class Point:
    
    IDX_X = 0
    IDX_Y = 1

    def __init__(self, x, y):
        self._x = x
        self._y = y

    def get_cartesian(self) -> tuple:
        return self.get_x(), self.get_y()

    def get_polar(self) -> tuple:
        pass

    def get_x(self) -> float:
        return self._x

    def get_y(self) -> float:
        return self._y

    def get_r(self) -> float:
        pass

    def get_theta(self) -> float:
        pass
    
    def apply_polar_shift(self, r, theta) -> None:
        pass

    def apply_cartesian_shift(self, x, y) -> None:
        pass


class PointWithID(Point):

    IDX_WALLID = 3
    def __init__(self, x, y, wall_id):
        super().__init__(x, y)
        self._wall_id = wall_id

    def get_wallid(self):
        return self._wall_id

class Line:

    IDX_M = 0
    IDX_B = 1

    def __init__(self, p1: PointWithID, p2: PointWithID):
        self.wall_id = p1.get_wallid()
        self.m, self.b = self.__find_line(p1, p2)
        v = ValueError()
        if not self.__matching_wall_ids(p1, p2): 
            raise WallIDMismatch('Wall ID for points does not match')

    def __find_line(self, p1:PointWithID, p2:PointWithID) -> float:
        m = (p2.get_y() - p1.get_y()) / (p2.get_x() - p1.get_x())
        # Using p1 to find b
        b = p1.get_y() - (m*p1.get_x())
        # y = mx + b
        # b = y - mx
        return m, b

    def __matching_wall_ids(self, p1, p2) -> bool:
        if p1.get_wallid() == p2.get_wallid(): return True
        return False

class WallIDMismatch (RuntimeError): ...

# Helper methods
def polar_to_cart(r, theta_deg) -> tuple:
    #print('polar to cart ',r, theta_deg)
    x = r * math.cos(deg_to_rad(theta_deg))
    y = r * math.sin(deg_to_rad(theta_deg))
    return x,y

def deg_to_rad(theta_deg) -> float:
    return theta_deg*(math.pi/180)

def point_from_polar(r, theta, wall_id) -> PointWithID:
    x, y = polar_to_cart(r, theta)
    return PointWithID(x, y, wall_id)