Инженерный тур. 2 этап

Z,X,r
0.0,0.0,9
1323.310446804855,741.2170326700434,35
3751.0326937190257,-425.39693527203053,0

4193.68

Краткий алгоритм решения задачи:

1. прочитать входные данные из файла;
2. определить минимальный радиус разворота;
3. определить угол между линиями пути треугольника точек;
4. найти биссектрису угла между линиями пути;
5. найти точку, при прохождении которой выполняются условия перемещения радиуса интереса ППМ с полным диаметром действия полезной нагрузки;
6. найти радиус дуги и центр окружности, формирующей эту дугу, для построения траектории при пересечении этой точки;
7. найти точки перехода с прямолинейной траектории на дугу окружности;
8. найти длины всех участков траектории и сложить их, чтобы получить длину всей траектории.

Представленный алгоритм можно реализовать с помощью следующего программного кода на языке Python:

import numpy as np
import pandas as pd

from math import cos,sin,hypot,tan,hypot,pi,pow
import math

# Матрица пересчета на ось Z
def rotz(theta=None):
    # Rotation around z
    R = np.array([  [cos(theta),    -sin(theta),    0],
                    [sin(theta),    cos(theta),     0],
                    [0,             0,              1]  ])
    return R

# Угол между векторами через библиотеку numpy
def angle_between_vectors_np(u, v):
    u = np.array(u)
    v = np.array(v)
    cos_theta = np.dot(u, v) / (np.linalg.norm(u) * np.linalg.norm(v))
    angle_rad = np.arccos(np.clip(cos_theta, -1.0, 1.0))
    angle_deg = np.degrees(angle_rad)
    return angle_rad, angle_deg

def solve(filename):
    # минимальный радиус разворота
    Vg = 120/3.6
    gamma_max = math.radians(60)
    g = 9.8
    R_min = math.pow(Vg,2)/(g*math.tan(gamma_max))
    # Загрузка ПЗ
    df_input = pd.read_csv(filename)
    # Объявим точки как массив
    w_0 = np.array([df_input["Z"][0],df_input["X"][0],0]).T
    w_1 = np.array([df_input["Z"][1],df_input["X"][1],0]).T
    w_2 = np.array([df_input["Z"][2],df_input["X"][2],0]).T 
    # Загрузим радиус действия полезной нагрузки и радиус интереса ППМ

    # Радиус обзора полезной нагрузки
    R_obs = df_input["r"][0]
    # Радиус интереса ППМ
    R_wp = df_input["r"][1]

    # Найдем единичные вектора между точками
    q_s = (w_1 - w_0) / np.linalg.norm(w_1 - w_0,ord=2)
    q_n = (w_2 - w_1) / np.linalg.norm(w_2 - w_1,ord=2)

    # Найдем угол между линиями пути треугольника точек

    theta,theta_degrees = angle_between_vectors_np(-q_s.T,q_n)

    # Найдем вектор отражающий биссектрису найденного угла
    q_R = q_n @ rotz(theta/2)

    # Найдем точку при прохождении которой, выполняются условия прохождения радиуса интереса ППМ с полным диаметром действия полезной нагрузки

    # point_org_global = w_1 - np.dot(-R_wp+R_obs,q_R)

    # Найдем необходимый радиус дуги окружности для построения траектории при пересечении этой точки

    R_calc = (abs(-R_wp+R_obs))/(1/sin(theta/2) -1)

    R  = R_calc if (R_calc > R_min) else R_min

    # Найдем центр окружности формирующей дугу

    center_circle = w_1 - (R/sin(theta/2))*(q_s - q_n) / np.linalg.norm(q_s - q_n,ord=2)

    # Найдем точки перехода с прямолинейной траектории на дугу окружности

    r1 = w_1 - np.dot(R/tan(theta/2),q_s)
    r2 = w_1 + np.dot(R/tan(theta/2),q_n)

    # Найдем угол сектора, чтобы рассчитать длину дуги

    q_r1 = (r1 -center_circle) / np.linalg.norm(r1 -center_circle,ord=2)
    q_r2 = (r2 -center_circle) / np.linalg.norm(r2 - center_circle,ord=2)

    theta_R,theta_R_degrees = angle_between_vectors_np(q_r1,q_r2)

    # Найдем длину первого прямолинейного участка до точки r1
    L_0 = hypot(w_0[0]-r1[0],w_0[1]-r1[1])
    # Найдем длину дуги окружности
    L_R = theta_R * R
    # Найдем длину второго участка траектории после точки r2
    L_2 = hypot(r2[0]-w_2[0],r2[1]-w_2[1])

    # Найдем общую длину траектории
    L = L_0 + L_R + L_2
    # Выведем длину участка, с точностью до 2 знака после запятой

    print("%.2f" % L)

solve("input.txt")

P2
# 130.5 0.966 0.0 0.0 0.259
320 240 220 63 63 63 63 63 63 63 63 63 63 63 63 63 63 63 63 63 63 63 63 63 63 63 129 63 63 115 63 63 63 63 63 63 63

90.57 8

Краткий алгоритм решения задачи:

1. прочитать входные данные из файла;
2. найти углы Эйлера, используя заданный кватернион;
3. найти высоту полета БЛА, используя найденные углы Эйлера и заданное значение показаний дальномера;
4. выполнить размытие изображения;
5. подобрать значение пороговой яркости;
6. выполнить поиск и подсчет контрастных ориентиров.

Представленный алгоритм можно реализовать с помощью следующего программного кода:

from math import sin, cos, sqrt, atan2, asin, pi
import sys
import copy

def normalize_quaternion(q):
    w, x, y, z = q
    norm = sqrt(w**2 + x**2 + y**2 + z**2)
    if norm == 0:
        raise ValueError("Cannot normalize a zero quaternion")
    return (w / norm, x / norm, y / norm, z / norm)

def quaternion_to_euler(q):
    # Normalize the quaternion
    q = normalize_quaternion(q)
    w, x, y, z = q
    # Calculate roll (phi)
    roll = atan2(2 * (y * z + w * x), 1 - 2 * (y**2 + x**2))
    # Calculate pitch (theta)
    pitch = asin(2 * (w * y - x * z))
    # Calculate yaw (psi)
    yaw = atan2(2 * (x * y + w * z), 1 - 2 * (z**2 + y**2))
    return (roll, pitch, yaw)

def apply_simple_blur(image):
    """Apply a simple blur to a 2D list representing an image."""
    height = len(image)
    width = len(image[0])
    blurred_image = [[0] * width for _ in range(height)]

    # Define the blur radius
    radius = 1  # Since we want to blur within 3 pixels area (1 on each side)

    for i in range(height):
        for j in range(width):
            total = 0
            count = 0

            # Iterate over the neighborhood
            for dx in range(-radius, radius + 1):
                for dy in range(-radius, radius + 1):
                    nx, ny = i + dx, j + dy
                    # Check bounds
                    if 0 <= nx < height and 0 <= ny < width:
                        total += image[nx][ny]
                        count += 1
            
            # Calculate the average and assign it to the blurred image
            blurred_image[i][j] = total // count  # Use integer division

    return blurred_image

def numIslands(grid, weight) -> int:
        # weight = 0.8

        def dfs(row, col):
            if grid[row][col] < weight:
                return 0
            grid[row][col] = 0.0
            if (col + 1) < len(grid[0]):
                dfs(row, col+1)
            if col > 0:
                dfs(row, col-1)
            if (row + 1) < len(grid):
                dfs(row+1, col)
            if row > 0:
                dfs(row - 1, col)
            return 1
        count = 0
        for i in range(len(grid)):
            for j in range(len(grid[0])):
                if grid[i][j] >= weight:
                    count += 1
                    dfs(i, j)
        return count

def solve(filename):
    f = open(filename, "r")
    inp = f.read()
    arr = inp.split(" ")
    laser = float(arr[2])
    quat = [float(x) for x in arr[3:7]]
    quat_sum = sqrt(quat[0]**2 + quat[1]**2 + quat[2]**2 + quat[3]**2)
    if not quat_sum == 1:
        quat = [quat[0] / quat_sum, quat[1] /quat_sum, quat[2] / quat_sum, quat[3] / quat_sum]

    width = int(arr[7])
    height = int(arr[8])
    max_brightness = int(arr[9])
    pgm_image = [int(x) for x in arr[10:-1]]
    rot = quaternion_to_euler([quat[0], quat[1], quat[3], quat[2]])
    angles = rot
    H = cos(angles[0] ) * cos(angles[1]) * laser
 
    norm_image = [float(x) for x in pgm_image]
    norm_image_arr = []
    for i in range(int(len(norm_image) / width)):
        norm_image_arr.append(norm_image[i*(width):(i+1)*width])
    norm_image_arr = apply_simple_blur(norm_image_arr)
    sys.setrecursionlimit(85000)
    sacrefise = copy.deepcopy(norm_image_arr)
    sol = numIslands(sacrefise, 0.8 * max_brightness)
    bright_pixels = [x for x in norm_image if x > 0.8*max_brightness]

    print(f'{H:.2f} {sol}')

solve('input.txt')

55.811539 37.499727 8

1 0 0 0

55.811539 37.499727

22 17 16 20 30
23 17 15 18 27
22 17 14 19 25
20 17 10 19 23
18 17 10 12 20

55.801453 37.471452

Краткий алгоритм решения задачи:

1. прочитать входные данные из файла;
2. преобразовать градусы в метры;
3. преобразовать координаты БЛА в его координаты в матрице рельефа;
4. вычислить направление камеры;
5. нормализовать вектор направления камеры;
6. найти координаты точки пересечения вектора с матрицей рельефа;
7. преобразовать координаты точки пересечения в географические координаты.

Представленный алгоритм можно реализовать с помощью следующего программного кода на языке Python:

import numpy as np


def quaternion_rotate_vector(q, v):
    # Поворот вектора v с помощью кватерниона q
    qvec = np.array([q[0], q[1], q[2]])
    uv = np.cross(qvec, v)
    uuv = np.cross(qvec, uv)
    uv *= (2.0 * q[3])
    uuv *= 2.0
    return v + uv + uuv

def get_terrain_height(terrain_matrix, x, y):
    # Получение высоты рельефа в точке (x, y) без интерполяции
    x0 = int(np.floor(x))
    y0 = int(np.floor(y))
    if x0 < 0 or x0 >= terrain_matrix.shape[1] or y0 < 0 or y0 >= terrain_matrix.shape[0]:
        return None
    return terrain_matrix[y0, x0]

def find_intersection(uav_pos, direction, terrain_matrix):
    t = 0.0
    delta_t = 0.5  # Шаг в метрах
    max_distance = 10000  # Максимальное расстояние поиска (в метрах)

    uav_x, uav_y, uav_z = uav_pos
    dx, dy, dz = direction
    terrain_size = terrain_matrix.shape[0]

    while t < max_distance:
        x = uav_x + t * dx
        y = uav_y + t * dy
        z = uav_z + t * dz

        if x < 0 or x >= terrain_size - 1 or y < 0 or y >= terrain_size - 1:
            # Луч вышел за пределы рельефа без пересечения
            return None

        terrain_height = get_terrain_height(terrain_matrix, x, y)
        if terrain_height is None:
            return None
        if z <= terrain_height:
            # Найдена точка пересечения
            # Уточнение точки пересечения
            # Возврат назад и использование меньших шагов
            t_high = t
            t_low = t - delta_t
            for _ in range(100):
                t_mid = (t_low + t_high) / 2.0
                x_mid = uav_x + t_mid * dx
                y_mid = uav_y + t_mid * dy
                z_mid = uav_z + t_mid * dz
                terrain_height_mid = get_terrain_height(terrain_matrix, x_mid, y_mid)
                if terrain_height_mid is None:
                    return None
                if z_mid <= terrain_height_mid:
                    t_high = t_mid
                else:
                    t_low = t_mid
            t = t_high
            x = uav_x + t * dx
            y = uav_y + t * dy
            z = uav_z + t * dz
            return x, y, z  # Точка пересечения в координатах рельефа

        t += delta_t

    # Пересечение не найдено в пределах максимального расстояния
    return None

def read_and_compute(filename):
    with open(filename, 'r') as f:
        lines = f.readlines()

  # Чтение координат БЛА и высоты
    uav_line = lines[0].strip()
    uav_latitude, uav_longitude, uav_altitude = map(float, uav_line.split())
    if uav_altitude < 0:
        print(f"{uav_latitude:.6f} {uav_longitude:.6f}")
        return

    # Чтение кватерниона
    quat_line = lines[1].strip()
    qw, qx, qy, qz = map(float, quat_line.split())

    # Чтение начальных координат рельефа
    terrain_line = lines[2].strip()
    terrain_latitude, terrain_longitude = map(float, terrain_line.split())

    # Чтение матрицы рельефа
    terrain_data = lines[3:]
    terrain_matrix = np.array([list(map(int, line.strip().split())) for line in terrain_data])
    terrain_matrix = np.flipud(terrain_matrix)

    # Конвертация градусов в метры
    latitude_rad = np.radians(terrain_latitude)
    meters_per_degree_latitude = 111120  # Среднее количество метров на градус широты
    meters_per_degree_longitude = 111120 * np.cos(latitude_rad)  # Метров на градус долготы зависит от широты

    # Преобразование координат БЛА в координаты матрицы рельефа
    delta_lat_uav = uav_latitude - (terrain_latitude - terrain_matrix.shape[0]/meters_per_degree_latitude)
    delta_lon_uav = uav_longitude - terrain_longitude

    delta_x_meters = delta_lon_uav * meters_per_degree_longitude
    delta_y_meters = delta_lat_uav * meters_per_degree_latitude

    # Координаты БЛА в матрице рельефа
    uav_x = delta_x_meters
    uav_y = delta_y_meters

    terrain_height_under_uav = get_terrain_height(terrain_matrix, uav_x, uav_y)

    if terrain_height_under_uav is None:
        print(f"{uav_latitude:.6f} {uav_longitude:.6f}")
        return

    uav_z = uav_altitude + terrain_height_under_uav


    # Вычисление направления камеры
    q = np.array([qx, qy, qz, qw])
    camera_forward = np.array([1, 0, 0])  # Ось X в системе координат камеры (направление оптической оси)
    direction_vector = quaternion_rotate_vector(q, camera_forward)

    # Нормализация вектора направления
    norm_dir = np.linalg.norm(direction_vector)
    if norm_dir == 0:
        print(f"{uav_latitude:.6f} {uav_longitude:.6f}")
        return
    direction = direction_vector / norm_dir

    # Поиск точки пересечения
    intersection = find_intersection((uav_x, uav_y, uav_z), direction, terrain_matrix)

    if intersection is None:
        print(f"{uav_latitude:.6f} {uav_longitude:.6f}")
        return

    intersect_x, intersect_y, intersect_z = intersection

    # Преобразование координат точки пересечения обратно в географические координаты
    delta_lat_intersect = intersect_y / meters_per_degree_latitude
    delta_lon_intersect = intersect_x / meters_per_degree_longitude

    object_latitude = (terrain_latitude - terrain_matrix.shape[0]/meters_per_degree_latitude) + delta_lat_intersect
    object_longitude = terrain_longitude + delta_lon_intersect

    print(f"{object_latitude:.6f} {object_longitude:.6f}")

read_and_compute('input.txt')