import math
from typing import Iterable, TypeVar
import numpy as np
T = TypeVar("T")
[docs]
def clip(x: "T", min_value: "T", max_value: "T") -> "T":
"""Clip value x between min_value and max_value."""
return max(min(x, max_value), min_value)
[docs]
def scale(
arr: "Iterable[float]", low: float = -1.0, high: float = 1.0, clip_min: float = -1.0, clip_max: float = 1.0
) -> np.ndarray:
"""Clip array to [clip_min, clip_max] then scale to [low, high]."""
# Clamp data between [clip_min, clip_max]
data = np.clip(arr, a_min=low, a_max=high)
# Scale values between [low, high]
data = (data - clip_min) / (clip_max - clip_min)
data = data * (high - low) + low
return data
[docs]
def wrap_angle(angle: float, wrap_offset: float = -np.pi, degrees: bool = False):
"""Wrap angle to range [wrap_offset, wrap_offset + 2π] or degree equivalent."""
if degrees:
angle = np.mod(angle + 2 * np.rad2deg(np.pi), np.rad2deg(2 * np.pi)) # wrap to [0, 360]
angle = angle + np.rad2deg(wrap_offset) # shift to [wrap_offset, wrap_offset+360]
else:
angle = np.mod(angle + 2 * np.pi, 2 * np.pi) # wrap to [0, 2π]
angle = angle + wrap_offset # shift to [wrap_offset, wrap_offset+2π]
return angle
[docs]
def dist(source_pos: "Iterable[float]", target_pos: "Iterable[float]") -> float:
"""Calculate Euclidean distance between two positions."""
return sum((a - b) ** 2 for a, b in zip(source_pos, target_pos)) ** 0.5
[docs]
def azimuth(source_pos: "Iterable[float]", target_pos: "Iterable[float]", source_heading: float = 0):
"""Calculate relative angle from source to target accounting for source heading."""
relative_angle = math.atan2(target_pos[1] - source_pos[1], target_pos[0] - source_pos[0])
return wrap_angle(relative_angle - source_heading)
[docs]
def calculate_curvature(points: "list[np.ndarray]") -> float:
"""Calculate curvature from three points using triangle area formula. Returns value clipped to [0, 1]."""
# get next three points
a = np.array(points[0])
b = np.array(points[1])
c = np.array(points[2])
area = abs(a[0] * (b[1] - c[1]) + b[0] * (c[1] - a[1]) + c[0] * (a[1] - b[1])) / 2
curvature = (4 * area) / (np.linalg.norm(a - b) * np.linalg.norm(b - c) * np.linalg.norm(c - a))
return np.clip(curvature, 0.0, 1.0)
[docs]
def position(source_pos: "Iterable[float]", distance: "Iterable[float]", az: float) -> "Iterable[float]":
"""
Calculate the position of a point given a source position, distance and azimuth.
Args
----
source_pos:
Position of the source point
distance:
Distance from the source point
az:
Azimuth from the source point
Returns
-------
Position of the point
"""
return [
source_pos[0] + distance * np.cos(az), # TODO verify sin vs cos
source_pos[1] + distance * np.sin(az), # TODO verify cos vs sin
]
[docs]
def euler_to_quaternion(roll: float, pitch: float, yaw: float, degrees: bool = False) -> np.ndarray:
"""Convert Euler angles to quaternion representation.
Args
----
roll:
Roll angle
pitch:
Pitch angle
yaw:
Yaw angle
degrees:
If True, input angles are in degrees, otherwise radians
Returns
-------
Quaternion as array (x, y, z, w)
"""
if degrees:
deg2rad = np.pi / 180
roll *= deg2rad
pitch *= deg2rad
yaw *= deg2rad
cy = np.cos(yaw * 0.5)
sy = np.sin(yaw * 0.5)
cr = np.cos(roll * 0.5)
sr = np.sin(roll * 0.5)
cp = np.cos(pitch * 0.5)
sp = np.sin(pitch * 0.5)
w = cy * cr * cp + sy * sr * sp
x = cy * sr * cp - sy * cr * sp
y = cy * cr * sp + sy * sr * cp
z = sy * cr * cp - cy * sr * sp
return np.array((x, y, z, w))
[docs]
def quaternion_to_euler(x: float, y: float, z: float, w: float, degrees: bool = False) -> np.ndarray:
"""Convert quaternion to Euler angle representation.
Args
----
x:
Quaternion x component
y:
Quaternion y component
z:
Quaternion z component
w:
Quaternion w component
degrees:
If True, output angles are in degrees, otherwise radians
Returns
-------
Euler angles as array (roll, pitch, yaw)
"""
if degrees:
deg = 180 / np.pi
else:
deg = 1
sinr_cosp = 2 * (w * x + y * z)
cosr_cosp = 1 - 2 * (x * x + y * y)
roll = np.arctan2(sinr_cosp, cosr_cosp) * deg
sinp = 2 * (w * y - z * x)
pitch = np.arcsin(sinp) * deg
siny_cosp = 2 * (w * z + x * y)
cosy_cosp = 1 - 2 * (y * y + z * z)
yaw = np.arctan2(siny_cosp, cosy_cosp) * deg
return np.array((roll, pitch, yaw))