67 lines
2.5 KiB
Python
67 lines
2.5 KiB
Python
import math
|
||
import numpy as np
|
||
|
||
class Geolocation:
|
||
""" Класс геопозиции """
|
||
x: float
|
||
y: float
|
||
z: float # Масштаб (Высота)
|
||
angle: float # Угол направления движения
|
||
|
||
def __init__(self, x: float = 0, y: float = 0, z: float = 1, angle: float = 0):
|
||
self.x = x
|
||
self.y = y
|
||
self.z = z
|
||
self.angle = angle
|
||
|
||
def __str__(self) -> str:
|
||
return (f"Geolocation(x={self.x:.2f}, y={self.y:.2f}, "
|
||
f"z={self.z:.2f}, angle={self.angle:.1f}°)")
|
||
|
||
def __matmul__(self, mat: np.array) -> 'Geolocation':
|
||
return self.copy().apply(mat)
|
||
|
||
def __imatmul__(self, mat: np.array) -> 'Geolocation':
|
||
return self.apply(mat)
|
||
|
||
def apply(self, mat: np.ndarray) -> 'Geolocation':
|
||
""" На основе матрицы трансформации вычисляет новую позицию и направление """
|
||
|
||
tx, ty = -mat[0, 2], -mat[1, 2]
|
||
|
||
# Вычисляем угол поворота
|
||
rotation = -np.arctan2(mat[1, 0], mat[0, 0])
|
||
scale = np.hypot(mat[0, 0], mat[0, 1])
|
||
print("Scale: ", scale)
|
||
|
||
# Координаты уже отцентрированы, поэтому используем их напрямую
|
||
dx_meters = tx
|
||
dy_meters = ty
|
||
|
||
angle_global = self.angle + rotation
|
||
|
||
# Применяем поворот к смещению (учитываем текущий угол БПЛА)
|
||
cos_angle = math.cos(angle_global)
|
||
sin_angle = math.sin(angle_global)
|
||
|
||
# Поворачиваем смещение в глобальные координаты
|
||
# Обратите внимание: dy_meters инвертирован, так как в изображениях Y направлен вниз
|
||
dx_global = dx_meters * cos_angle - dy_meters * sin_angle
|
||
dy_global = dx_meters * sin_angle + dy_meters * cos_angle
|
||
|
||
# Обновляем координаты БПЛА
|
||
self.z /= scale
|
||
self.x = self.x + dx_global * self.z
|
||
self.y = self.y + dy_global * self.z
|
||
|
||
# Нормализуем угол в диапазоне [-π, π]
|
||
self.angle = math.atan2(math.sin(angle_global), math.cos(angle_global))
|
||
|
||
return self
|
||
|
||
def copy(self) -> 'Geolocation':
|
||
return Geolocation(self.x, self.y, self.z, self.angle)
|
||
|
||
@staticmethod
|
||
def transform(g: 'Geolocation', mat: np.ndarray) -> 'Geolocation | None':
|
||
return g.copy().apply(mat) |