Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion t4_devkit/dataclass/box.py
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
4 changes: 2 additions & 2 deletions t4_devkit/dataclass/trajectory.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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.
Expand Down
24 changes: 13 additions & 11 deletions t4_devkit/dataclass/transform.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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(
Expand Down Expand Up @@ -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:
Expand All @@ -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):
if 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.")
Expand All @@ -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}.")

Expand Down
21 changes: 12 additions & 9 deletions t4_devkit/viewer/geography.py
Original file line number Diff line number Diff line change
@@ -1,34 +1,37 @@
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"]

EARTH_RADIUS_METERS = 6.378137e6
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(
Expand All @@ -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))
Loading