From 08ddfb39d41eb535c420b5d03433dd51d726c5a8 Mon Sep 17 00:00:00 2001 From: ktro2828 Date: Fri, 29 Aug 2025 15:57:45 +0900 Subject: [PATCH 1/2] style: update typing Signed-off-by: ktro2828 --- t4_devkit/dataclass/box.py | 2 +- t4_devkit/dataclass/trajectory.py | 4 ++-- t4_devkit/dataclass/transform.py | 22 ++++++++++++---------- t4_devkit/viewer/geography.py | 21 ++++++++++++--------- 4 files changed, 27 insertions(+), 22 deletions(-) diff --git a/t4_devkit/dataclass/box.py b/t4_devkit/dataclass/box.py index 62172fa..415f953 100644 --- a/t4_devkit/dataclass/box.py +++ b/t4_devkit/dataclass/box.py @@ -189,7 +189,7 @@ def translate(self, x: Vector3Like) -> None: Args: x (Vector3Like): 3D translation vector in the order of (x, y, z). """ - self.position += x + self.position += Vector3(x) if self.future is not None: self.future.translate(x) diff --git a/t4_devkit/dataclass/trajectory.py b/t4_devkit/dataclass/trajectory.py index bfee940..f43e39d 100644 --- a/t4_devkit/dataclass/trajectory.py +++ b/t4_devkit/dataclass/trajectory.py @@ -6,7 +6,7 @@ from attrs import define, field, validators from t4_devkit.common.converter import to_quaternion -from t4_devkit.typing import Trajectory +from t4_devkit.typing import Trajectory, Vector3 if TYPE_CHECKING: from t4_devkit.typing import NDArrayFloat, NDArrayInt, RotationLike, Vector3Like @@ -86,7 +86,7 @@ def translate(self, x: Vector3Like) -> None: Args: x (Vector3Like): 3D translation vector. """ - self.waypoints += x + self.waypoints += Vector3(x) def rotate(self, q: RotationLike) -> None: """Apply a rotation. diff --git a/t4_devkit/dataclass/transform.py b/t4_devkit/dataclass/transform.py index 5382b32..04c7cb8 100644 --- a/t4_devkit/dataclass/transform.py +++ b/t4_devkit/dataclass/transform.py @@ -8,7 +8,7 @@ from typing_extensions import Self from t4_devkit.common.converter import to_quaternion -from t4_devkit.typing import Matrix3x3, Matrix4x4, NDArray, Quaternion, Vector3 +from t4_devkit.typing import Matrix3x3, Matrix4x4, Quaternion, Vector3 if TYPE_CHECKING: from t4_devkit.typing import Matrix4x4Like, RotationLike, Vector3Like @@ -292,8 +292,7 @@ def rotate(self, *args, **kwargs) -> RotateItemLike: if {"position"} == set(inputs.keys()): return np.matmul(self.rotation_matrix, inputs["position"]) elif {"rotation"} == set(inputs.keys()): - rotation_matrix: NDArray = inputs["rotation"].rotation_matrix - return np.matmul(self.rotation_matrix, rotation_matrix) + return np.matmul(self.rotation_matrix, inputs["rotation"].rotation_matrix) elif {"matrix"} == set(inputs.keys()): matrix: HomogeneousMatrix = deepcopy(inputs["matrix"]) matrix.rotation = Quaternion( @@ -413,13 +412,16 @@ def _format_transform_args(*args, **kwargs) -> dict[str, Any]: if "matrix" in kwargs: raise KeyError("Cannot specify `position` and `matrix` at the same time.") elif "rotation" in kwargs: - return {"position": kwargs["position"], "rotation": kwargs["rotation"]} + return { + "position": Vector3(kwargs["position"]), + "rotation": to_quaternion(kwargs["rotation"]), + } else: - return {"position": kwargs["position"]} + return {"position": Vector3(kwargs["position"])} elif "rotation" in kwargs: if "matrix" in kwargs: raise KeyError("Cannot specify `rotation` and `matrix` at the same time.") - return {"rotation": kwargs["rotation"]} + return {"rotation": to_quaternion(kwargs["rotation"])} elif "matrix" in kwargs: return {"matrix": kwargs["matrix"]} else: @@ -436,15 +438,15 @@ def _format_transform_args(*args, **kwargs) -> dict[str, Any]: arg0 = np.asarray(arg0) if arg0.ndim == 1: if len(arg0) == 3: - return {"position": arg0} + return {"position": Vector3(arg0)} elif len(arg0) == 4: - return {"rotation": arg0} + return {"rotation": to_quaternion(arg0)} else: raise ValueError(f"Unexpected argument shape: {arg0.shape}.") else: if not arg0.shape != (3, 3): raise ValueError(f"Unexpected argument shape: {arg0.shape}.") - return {"rotation": arg0} + return {"rotation": to_quaternion(arg0)} elif num_kwargs == 1: if "rotation" not in kwargs: raise KeyError("Expected two arguments: position and rotation.") @@ -453,7 +455,7 @@ def _format_transform_args(*args, **kwargs) -> dict[str, Any]: raise ValueError(f"Too much arguments {num_args + num_kwargs}.") # >>> (position, rotation) elif num_args == 2: - return {"position": args[0], "rotation": args[1]} + return {"position": Vector3(args[0]), "rotation": to_quaternion(args[1])} else: raise ValueError(f"Too much arguments {num_args + num_kwargs}.") diff --git a/t4_devkit/viewer/geography.py b/t4_devkit/viewer/geography.py index 3ac43fa..7fdd6d4 100644 --- a/t4_devkit/viewer/geography.py +++ b/t4_devkit/viewer/geography.py @@ -1,10 +1,14 @@ from __future__ import annotations import math +from typing import TYPE_CHECKING import numpy as np -from t4_devkit.typing import Vector3 +from t4_devkit.typing import Vector2, Vector3 + +if TYPE_CHECKING: + from t4_devkit.typing import Vector2Like, Vector3Like __all__ = ["calculate_geodetic_point"] @@ -12,23 +16,22 @@ FLATTENING = 1 / 298.257223563 -def calculate_geodetic_point(position: Vector3, origin: tuple[float, float]) -> tuple[float, float]: +def calculate_geodetic_point(position: Vector3Like, origin: Vector2Like) -> Vector2: """Transform a position in a map coordinate system to a position in a geodetic coordinate system. Args: - position (Vector3): 3D position in a map coordinate system. - origin (tuple[float, float]): Map origin position in a geodetic coordinate system, + position (Vector3Like): 3D position in a map coordinate system. + origin (Vector2Like): Map origin position in a geodetic coordinate system, which is (latitude, longitude). Returns: - tuple[float, float]: Transformed position in a geodetic coordinate system, - which is (latitude, longitude). + Transformed position in a geodetic coordinate system, which is (latitude, longitude). """ - x, y, _ = position + x, y, _ = Vector3(position) bearing = math.atan2(x, y) distance = math.hypot(x, y) - latitude, longitude = np.radians(origin) + latitude, longitude = np.radians(Vector2(origin)) angular_distance = distance / EARTH_RADIUS_METERS target_latitude = math.asin( @@ -40,4 +43,4 @@ def calculate_geodetic_point(position: Vector3, origin: tuple[float, float]) -> math.cos(angular_distance) - math.sin(latitude) * math.sin(target_latitude), ) - return math.degrees(target_latitude), math.degrees(target_longitude) + return Vector2(math.degrees(target_latitude), math.degrees(target_longitude)) From ee52d8cc888e46e71dc6952aabae3fd02bad960c Mon Sep 17 00:00:00 2001 From: ktro2828 Date: Fri, 29 Aug 2025 16:03:06 +0900 Subject: [PATCH 2/2] fix: wrong condition Signed-off-by: ktro2828 --- t4_devkit/dataclass/transform.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/t4_devkit/dataclass/transform.py b/t4_devkit/dataclass/transform.py index 04c7cb8..0b3d46b 100644 --- a/t4_devkit/dataclass/transform.py +++ b/t4_devkit/dataclass/transform.py @@ -444,7 +444,7 @@ def _format_transform_args(*args, **kwargs) -> dict[str, Any]: else: raise ValueError(f"Unexpected argument shape: {arg0.shape}.") else: - if not arg0.shape != (3, 3): + if arg0.shape != (3, 3): raise ValueError(f"Unexpected argument shape: {arg0.shape}.") return {"rotation": to_quaternion(arg0)} elif num_kwargs == 1: