import math import numpy as np from typing import Optional class Geolocation: """Класс геопозиции с полной ориентацией БПЛА в 3D пространстве""" x: float # Координата X (метры) y: float # Координата Y (метры) z: float # Масштаб (высота) yaw: float # Рыскание (rotation around Z-axis) pitch: float # Тангаж (rotation around Y-axis) roll: float # Крен (rotation around X-axis) def __init__( self, x: float = 0, y: float = 0, z: float = 1, yaw: float = 0, pitch: float = 0, roll: float = 0 ): self.x = x self.y = y self.z = z self.yaw = yaw self.pitch = pitch self.roll = roll def __str__(self) -> str: return ( f"Geolocation(x={self.x:.2f}, y={self.y:.2f}, z={self.z:.2f}, " f"yaw={math.degrees(self.yaw):.1f}°, " f"pitch={math.degrees(self.pitch):.1f}°, " f"roll={math.degrees(self.roll):.1f}°)" ) def __matmul__(self, mat: np.ndarray) -> 'Geolocation': return self.copy().apply(mat) def __imatmul__(self, mat: np.ndarray) -> 'Geolocation': return self.apply(mat) def apply(self, mat: np.ndarray) -> 'Geolocation': """ Применяет матрицу трансформации для вычисления новой позиции и ориентации. Для 2D трансформаций (3x3 матрица) обновляет только x, y, z и yaw. Для 3D трансформаций (4x4 матрица) обновляет все параметры включая pitch и roll. """ if mat.shape == (3, 3): return self._apply_2d(mat) elif mat.shape == (4, 4): return self._apply_3d(mat) else: raise ValueError(f"Unsupported matrix shape: {mat.shape}. Expected (3,3) or (4,4)") def _apply_2d(self, mat: np.ndarray) -> 'Geolocation': """Применяет 2D трансформацию""" 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]) dx_meters = tx dy_meters = ty angle_global = self.yaw + rotation cos_angle = math.cos(angle_global) sin_angle = math.sin(angle_global) dx_global = dx_meters * cos_angle - dy_meters * sin_angle dy_global = dx_meters * sin_angle + dy_meters * cos_angle self.z /= scale if scale != 0 else 1 self.x += dx_global * self.z self.y += dy_global * self.z self.yaw = self._normalize_angle(angle_global) return self def _apply_3d(self, mat: np.ndarray) -> 'Geolocation': """Применяет 3D трансформацию с извлечением углов Эйлера""" position = np.array([self.x, self.y, 0, 1]) new_position = mat @ position self.x = new_position[0] self.y = new_position[1] rotation_mat = mat[:3, :3] scale = np.linalg.norm(rotation_mat[:, 0]) self.z *= scale normalized_mat = rotation_mat / scale yaw, pitch, roll = self._extract_euler_angles(normalized_mat) self.yaw = self._normalize_angle(self.yaw + yaw) self.pitch = self._normalize_angle(self.pitch + pitch) self.roll = self._normalize_angle(self.roll + roll) return self @staticmethod def _extract_euler_angles(R: np.ndarray) -> tuple[float, float, float]: """ Извлекает углы Эйлера (yaw, pitch, roll) из матрицы вращения. Использует ZYX convention (yaw-pitch-roll). """ sy = math.sqrt(R[0, 0]**2 + R[1, 0]**2) singular = sy < 1e-6 if not singular: roll = math.atan2(R[2, 1], R[2, 2]) pitch = math.atan2(-R[2, 0], sy) yaw = math.atan2(R[1, 0], R[0, 0]) else: roll = math.atan2(-R[1, 2], R[1, 1]) pitch = math.atan2(-R[2, 0], sy) yaw = 0 return yaw, pitch, roll @staticmethod def _normalize_angle(angle: float) -> float: """Нормализует угол в диапазон [-π, π]""" return math.atan2(math.sin(angle), math.cos(angle)) def copy(self) -> 'Geolocation': """Создает полную копию объекта""" return Geolocation(self.x, self.y, self.z, self.yaw, self.pitch, self.roll) def get_rotation_matrix(self) -> np.ndarray: """ Матрица вращения с порядком применения: yaw → pitch → roll Умножение: R = Rx(roll) * Ry(pitch) * Rz(yaw) """ cy, sy = math.cos(self.yaw), math.sin(self.yaw) cp, sp = math.cos(self.pitch), math.sin(self.pitch) cr, sr = math.cos(self.roll), math.sin(self.roll) # Порядок умножения: Rx * Ry * Rz (справа налево: yaw→pitch→roll) return np.array([ [cy*cp, -sy*cr + cy*sp*sr, sy*sr + cy*sp*cr], [sy*cp, cy*cr + sy*sp*sr, -cy*sr + sy*sp*cr], [-sp, cp*sr, cp*cr] ]) def to_dict(self) -> dict: """Экспорт в словарь""" return { 'x': self.x, 'y': self.y, 'z': self.z, 'yaw': self.yaw, 'pitch': self.pitch, 'roll': self.roll } @classmethod def from_dict(cls, data: dict) -> 'Geolocation': """Импорт из словаря""" return cls( x=data.get('x', 0), y=data.get('y', 0), z=data.get('z', 1), yaw=data.get('yaw', 0), pitch=data.get('pitch', 0), roll=data.get('roll', 0) ) @staticmethod def transform(g: 'Geolocation', mat: np.ndarray) -> Optional['Geolocation']: """Статический метод трансформации (создает копию и применяет матрицу)""" if g is None: return None return g.copy().apply(mat) # Пример использования if __name__ == "__main__": # Создание геопозиции geo = Geolocation(x=100, y=200, z=50, yaw=math.radians(45), pitch=math.radians(10), roll=math.radians(5)) print("Исходная позиция:", geo) # 2D трансформация (обратная совместимость) mat_2d = np.array([ [0.9, -0.1, 5], [0.1, 0.9, 10], [0, 0, 1] ]) geo_2d = geo @ mat_2d print("\nПосле 2D трансформации:", geo_2d) # 3D трансформация angle = math.radians(30) mat_3d = np.array([ [math.cos(angle), -math.sin(angle), 0, 15], [math.sin(angle), math.cos(angle), 0, 20], [0, 0, 1, 0], [0, 0, 0, 1] ]) geo_3d = geo.copy() geo_3d @= mat_3d print("\nПосле 3D трансформации:", geo_3d) # Получение матрицы вращения rot_mat = geo.get_rotation_matrix() print("\nМатрица вращения:\n", rot_mat) print("\n✓ Код успешно выполнен")