X-lanni
128 lines
3.1 KiB
Python
128 lines
3.1 KiB
Python
# Copyright (c) 2021, Matthew Booker
|
|
#
|
|
# Licensed under the Apache License, Version 2.0 (the "License");
|
|
# you may not use this file except in compliance with the License.
|
|
# You may obtain a copy of the License at
|
|
#
|
|
# http://www.apache.org/licenses/LICENSE-2.0
|
|
#
|
|
# Unless required by applicable law or agreed to in writing, software
|
|
# distributed under the License is distributed on an "AS IS" BASIS,
|
|
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
# See the License for the specific language governing permissions and
|
|
# limitations under the License. Reserved.
|
|
|
|
import numpy as np
|
|
|
|
|
|
def normalize_angle(angle):
|
|
"""
|
|
Normalize the angle to between [0, 2pi).
|
|
|
|
Args:
|
|
angle: float
|
|
The angle to normalize in radians
|
|
|
|
Returns
|
|
-------
|
|
The normalized angle in the range [0,2pi)
|
|
|
|
"""
|
|
while angle >= 2*np.pi:
|
|
angle -= 2*np.pi
|
|
|
|
while angle < 0:
|
|
angle += 2*np.pi
|
|
|
|
return angle
|
|
|
|
|
|
def angle_difference(angle_1, angle_2, left_turn=None):
|
|
"""
|
|
Calculate the difference between two angles based on a given direction.
|
|
|
|
Args:
|
|
angle_1: float
|
|
The starting angle in radians
|
|
angle_2: float
|
|
The ending angle in radians
|
|
left_turn: bool
|
|
The direction of turn. True if left, false if right
|
|
and None if smallest angular difference should be
|
|
returned
|
|
|
|
Returns
|
|
-------
|
|
The angular difference between the two angles according to
|
|
the specified turn direction
|
|
|
|
"""
|
|
if left_turn is None:
|
|
dif = abs(angle_1 - angle_2)
|
|
|
|
return dif if dif <= np.pi else 2 * np.pi - dif
|
|
|
|
elif left_turn:
|
|
|
|
if angle_2 >= angle_1:
|
|
return abs(angle_1 - angle_2)
|
|
else:
|
|
return 2 * np.pi - abs(angle_1 - angle_2)
|
|
|
|
else:
|
|
if angle_1 >= angle_2:
|
|
return abs(angle_1 - angle_2)
|
|
else:
|
|
return 2 * np.pi - abs(angle_1 - angle_2)
|
|
|
|
|
|
def interpolate_yaws(start_angle, end_angle, left_turn, steps):
|
|
"""
|
|
Create equally spaced yaws between two angles.
|
|
|
|
Args:
|
|
start_angle: float
|
|
The starting angle
|
|
end_angle: float
|
|
The ending angle
|
|
left_turn: bool
|
|
The direction of turn. True if left, False otherwise
|
|
steps: int
|
|
The number of yaws to generate between start and end
|
|
angle
|
|
|
|
Returns
|
|
-------
|
|
An array of yaws starting at start angle and ending at end
|
|
angle with steps number of angles between them
|
|
|
|
"""
|
|
if left_turn:
|
|
if start_angle > end_angle:
|
|
end_angle += 2 * np.pi
|
|
else:
|
|
if end_angle > start_angle:
|
|
end_angle -= 2 * np.pi
|
|
|
|
yaws = np.linspace(start_angle, end_angle, steps)
|
|
yaws = np.vectorize(normalize_angle)(yaws)
|
|
|
|
return yaws
|
|
|
|
|
|
def get_rotation_matrix(angle):
|
|
"""
|
|
Return a rotation matrix that is equivalent to a 2D rotation of angle.
|
|
|
|
Args:
|
|
angle: float
|
|
The angle to create a rotation matrix for
|
|
|
|
Returns
|
|
-------
|
|
A 2x2 matrix representing a 2D rotation by angle
|
|
|
|
"""
|
|
return np.array([[np.cos(angle), -np.sin(angle)],
|
|
[np.sin(angle), np.cos(angle)]])
|