214 lines
7.4 KiB
Python
214 lines
7.4 KiB
Python
|
||
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✓ Код успешно выполнен")
|