Data Structures

def __add__(self, other):
    """"""
    return Location(x=self.x + other.x, y=self.y + other.y, z=self.z + other.z)

def __str__(self):
    return f"{self.x:.3},{self.y:.3},{self.z:.3}"

def distance(self, other_location):
    """Euclidean distance between current location and other location"""
    return distance.euclidean(
        (self.x, self.y, self.z),
        (other_location.x, other_location.y, other_location.z),
    )

def to_array(self) -> np.array:
    return np.array([self.x, self.y, self.z])

def __str__(self):
    return f"{self.pitch},{self.yaw},{self.roll}"

def to_array(self) -> np.array:
    return np.array([self.pitch, self.yaw, self.roll])

def get_matrix(self) -> np.ndarray:
    """
    Calculate extrinsics matrix with respect to parent object
    http://planning.cs.uiuc.edu/node104.html

    Returns:
        Extrinsics matrix
    """
    location = self.location
    rotation = self.rotation
    yaw, pitch, roll = rotation.yaw, rotation.pitch, rotation.roll
    c_y = np.cos(np.radians(yaw))
    s_y = np.sin(np.radians(yaw))
    c_r = np.cos(np.radians(roll))
    s_r = np.sin(np.radians(roll))
    c_p = np.cos(np.radians(pitch))
    s_p = np.sin(np.radians(pitch))

    matrix = np.identity(4)
    matrix[0, 3] = location.x
    matrix[1, 3] = location.y
    matrix[2, 3] = location.z
    matrix[0, 0] = c_p * c_y
    matrix[0, 1] = c_y * s_p * s_r - s_y * c_r
    matrix[0, 2] = -c_y * s_p * c_r - s_y * s_r
    matrix[1, 0] = s_y * c_p
    matrix[1, 1] = s_y * s_p * s_r + c_y * c_r
    matrix[1, 2] = -s_y * s_p * c_r + c_y * s_r
    matrix[2, 0] = s_p
    matrix[2, 1] = -c_p * s_r
    matrix[2, 2] = c_p * c_r
    return matrix