feat: add pitch, roll in Geolocation

autopilot.py

@@ -207,8 +207,8 @@ class AutoPilot(Pilot):
         return {
             'x': self.geo.x,
             'y': self.geo.y,
-            'angle': self.geo.angle,
-            'angle_degrees': math.degrees(self.geo.angle),
+            'angle': self.geo.yaw,
+            'angle_degrees': math.degrees(self.geo.yaw),
             'frame_count': self.frame_count
         }
 
@@ -331,7 +331,7 @@ class AutoPilot(Pilot):
         # Вычисляем угол к цели
         target_angle = math.atan2(self.points[self.target_idx][1] - self.geo.y, self.points[self.target_idx][0] - self.geo.x)
         
-        angle_trajectory = self.geo.angle + math.pi / 2
+        angle_trajectory = self.geo.yaw + math.pi / 2
 
         # print("[ANGLE]", angle_trajectory, "->", target_angle)
 

geolocation.py

@@ -1,66 +1,213 @@
+
 import math
 import numpy as np
+from typing import Optional
+
 
 class Geolocation:
-    """ Класс геопозиции """
-    x: float
-    y: float
-    z: float # Масштаб (Высота)
-    angle: float # Угол направления движения
+    """Класс геопозиции с полной ориентацией БПЛА в 3D пространстве"""
 
-    def __init__(self, x: float = 0, y: float = 0, z: float = 1, angle: float = 0):
+    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.angle = angle
+        self.yaw = yaw
+        self.pitch = pitch
+        self.roll = roll
 
     def __str__(self) -> str:
-        return (f"Geolocation(x={self.x:.2f}, y={self.y:.2f}, "
-                f"z={self.z:.2f}, angle={self.angle:.1f}°)")
+        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.array) -> 'Geolocation':
+    def __matmul__(self, mat: np.ndarray) -> 'Geolocation':
         return self.copy().apply(mat)
 
-    def __imatmul__(self, mat: np.array) -> 'Geolocation':
+    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.angle + rotation
+        angle_global = self.yaw + 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))
-
+        
+        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 copy(self) -> 'Geolocation':
-        return Geolocation(self.x, self.y, self.z, self.angle)
+    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 transform(g: 'Geolocation', mat: np.ndarray) -> 'Geolocation | None':
-        return g.copy().apply(mat)
+    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✓ Код успешно выполнен")

simulator.py

@@ -103,7 +103,7 @@ class Simulator:
         h, w = img_array.shape[:2]
         
         # Получаем исходные точки с учётом перспективы
-        yaw_deg = -math.degrees(self.geo.angle)
+        yaw_deg = -math.degrees(self.geo.yaw)
         src_points = self._get_perspective_points(w, h, yaw_deg)
         
         # Целевые точки - квадрат 700x700
@@ -144,11 +144,11 @@ class Simulator:
         action.move_to_element_with_offset(html, 200, 200)
         action.click_and_hold()
         
-        self.geo.angle += dangle
+        self.geo.yaw += dangle
         velocity = max(velocity, 10)
         
-        dx = math.cos(math.pi / 2 + self.geo.angle) * velocity / self.geo.z
-        dy = math.sin(math.pi / 2 + self.geo.angle) * velocity / self.geo.z
+        dx = math.cos(math.pi / 2 + self.geo.yaw) * velocity / self.geo.z
+        dy = math.sin(math.pi / 2 + self.geo.yaw) * velocity / self.geo.z
         
         self.update_trajectory(dx, dy)