428 lines
16 KiB
Python
428 lines
16 KiB
Python
from math import acos, asin, atan2, cos, pi, sin, sqrt, isnan
|
|
from scipy.spatial.transform import Rotation
|
|
import numpy as np
|
|
import sys
|
|
|
|
try:
|
|
GLOBAL_DIR = sys._MEIPASS # temporary folder with libs & data files
|
|
except:
|
|
GLOBAL_DIR = "."
|
|
|
|
|
|
class MathOps():
|
|
'''
|
|
This class provides general mathematical operations that are not directly available through numpy
|
|
'''
|
|
|
|
@staticmethod
|
|
def deg_sph2cart(spherical_vec):
|
|
''' Converts spherical coordinates in degrees to cartesian coordinates '''
|
|
r = spherical_vec[0]
|
|
h = spherical_vec[1] * pi / 180
|
|
v = spherical_vec[2] * pi / 180
|
|
return np.array([r * cos(v) * cos(h), r * cos(v) * sin(h), r * sin(v)])
|
|
|
|
@staticmethod
|
|
def deg_sin(deg_angle):
|
|
''' Returns sin of degrees '''
|
|
return sin(deg_angle * pi / 180)
|
|
|
|
@staticmethod
|
|
def deg_cos(deg_angle):
|
|
''' Returns cos of degrees '''
|
|
return cos(deg_angle * pi / 180)
|
|
|
|
@staticmethod
|
|
def to_3d(vec_2d, value=0) -> np.ndarray:
|
|
''' Returns new 3d vector from 2d vector '''
|
|
return np.append(vec_2d, value)
|
|
|
|
@staticmethod
|
|
def to_2d_as_3d(vec_3d) -> np.ndarray:
|
|
''' Returns new 3d vector where the 3rd dimension is zero '''
|
|
vec_2d_as_3d = np.copy(vec_3d)
|
|
vec_2d_as_3d[2] = 0
|
|
return vec_2d_as_3d
|
|
|
|
@staticmethod
|
|
def normalize_vec(vec) -> np.ndarray:
|
|
''' Divides vector by its length '''
|
|
size = np.linalg.norm(vec)
|
|
if size == 0: return vec
|
|
return vec / size
|
|
|
|
def rel_to_global_3d(local_pos_3d: np.ndarray, global_pos_3d: np.ndarray,
|
|
global_orientation_quat: np.ndarray) -> np.ndarray:
|
|
''' Converts a local 3d position to a global 3d position given the global position and orientation (quaternion) '''
|
|
|
|
rotation = Rotation.from_quat(global_orientation_quat)
|
|
rotated_vec = rotation.apply(local_pos_3d)
|
|
global_pos = global_pos_3d + rotated_vec
|
|
|
|
return global_pos
|
|
|
|
@staticmethod
|
|
def get_active_directory(dir: str) -> str:
|
|
global GLOBAL_DIR
|
|
return GLOBAL_DIR + dir
|
|
|
|
@staticmethod
|
|
def acos(val):
|
|
''' arccosine function that limits input '''
|
|
return acos(np.clip(val, -1, 1))
|
|
|
|
@staticmethod
|
|
def asin(val):
|
|
''' arcsine function that limits input '''
|
|
return asin(np.clip(val, -1, 1))
|
|
|
|
@staticmethod
|
|
def normalize_deg(val):
|
|
''' normalize val in range [-180,180[ '''
|
|
return (val + 180.0) % 360 - 180
|
|
|
|
@staticmethod
|
|
def normalize_rad(val):
|
|
''' normalize val in range [-pi,pi[ '''
|
|
return (val + pi) % (2 * pi) - pi
|
|
|
|
@staticmethod
|
|
def deg_to_rad(val):
|
|
''' convert degrees to radians '''
|
|
return val * 0.01745329251994330
|
|
|
|
@staticmethod
|
|
def rad_to_deg(val):
|
|
''' convert radians to degrees '''
|
|
return val * 57.29577951308232
|
|
|
|
@staticmethod
|
|
def vector_angle(vector, is_rad=False):
|
|
''' angle (degrees or radians) of 2D vector '''
|
|
if is_rad:
|
|
return atan2(vector[1], vector[0])
|
|
else:
|
|
return atan2(vector[1], vector[0]) * 180 / pi
|
|
|
|
@staticmethod
|
|
def vectors_angle(vec1, vec2, is_rad=False):
|
|
''' get angle between vectors (degrees or radians) '''
|
|
ang_rad = acos(np.dot(MathOps.normalize_vec(vec1), MathOps.normalize_vec(vec2)))
|
|
return ang_rad if is_rad else ang_rad * 180 / pi
|
|
|
|
@staticmethod
|
|
def vector_from_angle(angle, is_rad=False):
|
|
''' unit vector with direction given by `angle` '''
|
|
if is_rad:
|
|
return np.array([cos(angle), sin(angle)], float)
|
|
else:
|
|
return np.array([MathOps.deg_cos(angle), MathOps.deg_sin(angle)], float)
|
|
|
|
@staticmethod
|
|
def target_abs_angle(pos2d, target, is_rad=False):
|
|
''' angle (degrees or radians) of vector (target-pos2d) '''
|
|
if is_rad:
|
|
return atan2(target[1] - pos2d[1], target[0] - pos2d[0])
|
|
else:
|
|
return atan2(target[1] - pos2d[1], target[0] - pos2d[0]) * 180 / pi
|
|
|
|
@staticmethod
|
|
def target_rel_angle(pos2d, ori, target, is_rad=False):
|
|
''' relative angle (degrees or radians) of target if we're located at 'pos2d' with orientation 'ori' (degrees or radians) '''
|
|
if is_rad:
|
|
return MathOps.normalize_rad(atan2(target[1] - pos2d[1], target[0] - pos2d[0]) - ori)
|
|
else:
|
|
return MathOps.normalize_deg(atan2(target[1] - pos2d[1], target[0] - pos2d[0]) * 180 / pi - ori)
|
|
|
|
@staticmethod
|
|
def rotate_2d_vec(vec, angle, is_rad=False):
|
|
''' rotate 2D vector anticlockwise around the origin by `angle` '''
|
|
cos_ang = cos(angle) if is_rad else cos(angle * pi / 180)
|
|
sin_ang = sin(angle) if is_rad else sin(angle * pi / 180)
|
|
return np.array([cos_ang * vec[0] - sin_ang * vec[1], sin_ang * vec[0] + cos_ang * vec[1]])
|
|
|
|
@staticmethod
|
|
def distance_point_to_line(p: np.ndarray, a: np.ndarray, b: np.ndarray):
|
|
'''
|
|
Distance between point p and 2d line 'ab' (and side where p is)
|
|
|
|
Parameters
|
|
----------
|
|
a : ndarray
|
|
2D point that defines line
|
|
b : ndarray
|
|
2D point that defines line
|
|
p : ndarray
|
|
2D point
|
|
|
|
Returns
|
|
-------
|
|
distance : float
|
|
distance between line and point
|
|
side : str
|
|
if we are at a, looking at b, p may be at our "left" or "right"
|
|
'''
|
|
line_len = np.linalg.norm(b - a)
|
|
|
|
if line_len == 0: # assumes vertical line
|
|
dist = sdist = np.linalg.norm(p - a)
|
|
else:
|
|
sdist = np.cross(b - a, p - a) / line_len
|
|
dist = abs(sdist)
|
|
|
|
return dist, "left" if sdist > 0 else "right"
|
|
|
|
@staticmethod
|
|
def distance_point_to_point_2d(p1: np.ndarray, p2: np.ndarray) -> float:
|
|
''' Distance in 2d from point p1 to point p2'''
|
|
|
|
return np.sqrt((p1[0] - p2[0]) ** 2 + (p1[1] - p2[1]) ** 2)
|
|
|
|
@staticmethod
|
|
def is_point_between(p: np.ndarray, a: np.ndarray, b: np.ndarray) -> bool:
|
|
''' Verify if point 'p' is between 'a' and 'b' '''
|
|
|
|
p_2d = p[:2]
|
|
a_2d = a[:2]
|
|
b_2d = b[:2]
|
|
|
|
return np.all(np.minimum(a_2d, b_2d) <= p_2d) and np.all(p_2d <= np.maximum(a_2d, b_2d))
|
|
|
|
@staticmethod
|
|
def is_x_between(x1: np.ndarray, x2: np.ndarray, x3: np.ndarray) -> bool:
|
|
''' Verify if x-axis of point 'x1' is between 'x2' and 'x3' '''
|
|
|
|
return x1 >= min(x2, x3) and x1 <= max(x2, x3)
|
|
|
|
def distance_point_to_segment(p: np.ndarray, a: np.ndarray, b: np.ndarray):
|
|
''' Distance from point p to 2d line segment 'ab' '''
|
|
|
|
ap = p - a
|
|
ab = b - a
|
|
|
|
ad = MathOps.vector_projection(ap, ab)
|
|
|
|
# Is d in ab? We can find k in (ad = k * ab) without computing any norm
|
|
# we use the largest dimension of ab to avoid division by 0
|
|
k = ad[0] / ab[0] if abs(ab[0]) > abs(ab[1]) else ad[1] / ab[1]
|
|
|
|
if k <= 0:
|
|
return np.linalg.norm(ap)
|
|
elif k >= 1:
|
|
return np.linalg.norm(p - b)
|
|
else:
|
|
return np.linalg.norm(p - (ad + a)) # p-d
|
|
|
|
@staticmethod
|
|
def distance_point_to_ray(p: np.ndarray, ray_start: np.ndarray, ray_direction: np.ndarray):
|
|
''' Distance from point p to 2d ray '''
|
|
|
|
rp = p - ray_start
|
|
rd = MathOps.vector_projection(rp, ray_direction)
|
|
|
|
# Is d in ray? We can find k in (rd = k * ray_direction) without computing any norm
|
|
# we use the largest dimension of ray_direction to avoid division by 0
|
|
k = rd[0] / ray_direction[0] if abs(ray_direction[0]) > abs(ray_direction[1]) else rd[1] / ray_direction[1]
|
|
|
|
if k <= 0:
|
|
return np.linalg.norm(rp)
|
|
else:
|
|
return np.linalg.norm(p - (rd + ray_start)) # p-d
|
|
|
|
@staticmethod
|
|
def closest_point_on_ray_to_point(p: np.ndarray, ray_start: np.ndarray, ray_direction: np.ndarray):
|
|
''' Point on ray closest to point p '''
|
|
|
|
rp = p - ray_start
|
|
rd = MathOps.vector_projection(rp, ray_direction)
|
|
|
|
# Is d in ray? We can find k in (rd = k * ray_direction) without computing any norm
|
|
# we use the largest dimension of ray_direction to avoid division by 0
|
|
k = rd[0] / ray_direction[0] if abs(ray_direction[0]) > abs(ray_direction[1]) else rd[1] / ray_direction[1]
|
|
|
|
if k <= 0:
|
|
return ray_start
|
|
else:
|
|
return rd + ray_start
|
|
|
|
@staticmethod
|
|
def does_circle_intersect_segment(p: np.ndarray, r, a: np.ndarray, b: np.ndarray):
|
|
''' Returns true if circle (center p, radius r) intersect 2d line segment '''
|
|
|
|
ap = p - a
|
|
ab = b - a
|
|
|
|
ad = MathOps.vector_projection(ap, ab)
|
|
|
|
# Is d in ab? We can find k in (ad = k * ab) without computing any norm
|
|
# we use the largest dimension of ab to avoid division by 0
|
|
k = ad[0] / ab[0] if abs(ab[0]) > abs(ab[1]) else ad[1] / ab[1]
|
|
|
|
if k <= 0:
|
|
return np.dot(ap, ap) <= r * r
|
|
elif k >= 1:
|
|
return np.dot(p - b, p - b) <= r * r
|
|
|
|
dp = p - (ad + a)
|
|
return np.dot(dp, dp) <= r * r
|
|
|
|
@staticmethod
|
|
def vector_projection(a: np.ndarray, b: np.ndarray):
|
|
''' Vector projection of a onto b '''
|
|
b_dot = np.dot(b, b)
|
|
return b * np.dot(a, b) / b_dot if b_dot != 0 else b
|
|
|
|
@staticmethod
|
|
def do_noncollinear_segments_intersect(a, b, c, d):
|
|
'''
|
|
Check if 2d line segment 'ab' intersects with noncollinear 2d line segment 'cd'
|
|
Explanation: https://www.geeksforgeeks.org/check-if-two-given-line-segments-intersect/
|
|
'''
|
|
|
|
ccw = lambda a, b, c: (c[1] - a[1]) * (b[0] - a[0]) > (b[1] - a[1]) * (c[0] - a[0])
|
|
return ccw(a, c, d) != ccw(b, c, d) and ccw(a, b, c) != ccw(a, b, d)
|
|
|
|
@staticmethod
|
|
def intersection_segment_opp_goal(a: np.ndarray, b: np.ndarray):
|
|
''' Computes the intersection point of 2d segment 'ab' and the opponents' goal (front line) '''
|
|
vec_x = b[0] - a[0]
|
|
|
|
# Collinear intersections are not accepted
|
|
if vec_x == 0: return None
|
|
|
|
k = (15.01 - a[0]) / vec_x
|
|
|
|
# No collision
|
|
if k < 0 or k > 1: return None
|
|
|
|
intersection_pt = a + (b - a) * k
|
|
|
|
if -1.01 <= intersection_pt[1] <= 1.01:
|
|
return intersection_pt
|
|
else:
|
|
return None
|
|
|
|
@staticmethod
|
|
def intersection_circle_opp_goal(p: np.ndarray, r):
|
|
'''
|
|
Computes the intersection segment of circle (center p, radius r) and the opponents' goal (front line)
|
|
Only the y coordinates are returned since the x coordinates are always equal to 15
|
|
'''
|
|
|
|
x_dev = abs(15 - p[0])
|
|
|
|
if x_dev > r:
|
|
return None # no intersection with x=15
|
|
|
|
y_dev = sqrt(r * r - x_dev * x_dev)
|
|
|
|
p1 = max(p[1] - y_dev, -1.01)
|
|
p2 = min(p[1] + y_dev, 1.01)
|
|
|
|
if p1 == p2:
|
|
return p1 # return the y coordinate of a single intersection point
|
|
elif p2 < p1:
|
|
return None # no intersection
|
|
else:
|
|
return p1, p2 # return the y coordinates of the intersection segment
|
|
|
|
@staticmethod
|
|
def distance_point_to_opp_goal(p: np.ndarray):
|
|
''' Distance between point 'p' and the opponents' goal (front line) '''
|
|
|
|
if p[1] < -1.01:
|
|
return np.linalg.norm(p - (15, -1.01))
|
|
elif p[1] > 1.01:
|
|
return np.linalg.norm(p - (15, 1.01))
|
|
else:
|
|
return abs(15 - p[0])
|
|
|
|
@staticmethod
|
|
def circle_line_segment_intersection(circle_center, circle_radius, pt1, pt2, full_line=True, tangent_tol=1e-9):
|
|
""" Find the points at which a circle intersects a line-segment. This can happen at 0, 1, or 2 points.
|
|
|
|
:param circle_center: The (x, y) location of the circle center
|
|
:param circle_radius: The radius of the circle
|
|
:param pt1: The (x, y) location of the first point of the segment
|
|
:param pt2: The (x, y) location of the second point of the segment
|
|
:param full_line: True to find intersections along full line - not just in the segment. False will just return intersections within the segment.
|
|
:param tangent_tol: Numerical tolerance at which we decide the intersections are close enough to consider it a tangent
|
|
:return Sequence[Tuple[float, float]]: A list of length 0, 1, or 2, where each element is a point at which the circle intercepts a line segment.
|
|
|
|
Note: We follow: http://mathworld.wolfram.com/Circle-LineIntersection.html
|
|
"""
|
|
|
|
(p1x, p1y), (p2x, p2y), (cx, cy) = pt1, pt2, circle_center
|
|
(x1, y1), (x2, y2) = (p1x - cx, p1y - cy), (p2x - cx, p2y - cy)
|
|
dx, dy = (x2 - x1), (y2 - y1)
|
|
dr = (dx ** 2 + dy ** 2) ** .5
|
|
big_d = x1 * y2 - x2 * y1
|
|
discriminant = circle_radius ** 2 * dr ** 2 - big_d ** 2
|
|
|
|
if discriminant < 0: # No intersection between circle and line
|
|
return []
|
|
else: # There may be 0, 1, or 2 intersections with the segment
|
|
intersections = [
|
|
(cx + (big_d * dy + sign * (-1 if dy < 0 else 1) * dx * discriminant ** .5) / dr ** 2,
|
|
cy + (-big_d * dx + sign * abs(dy) * discriminant ** .5) / dr ** 2)
|
|
for sign in ((1, -1) if dy < 0 else (-1, 1))] # This makes sure the order along the segment is correct
|
|
if not full_line: # If only considering the segment, filter out intersections that do not fall within the segment
|
|
fraction_along_segment = [
|
|
(xi - p1x) / dx if abs(dx) > abs(dy) else (yi - p1y) / dy for xi, yi in intersections]
|
|
intersections = [pt for pt, frac in zip(
|
|
intersections, fraction_along_segment) if 0 <= frac <= 1]
|
|
# If line is tangent to circle, return just one point (as both intersections have same location)
|
|
if len(intersections) == 2 and abs(discriminant) <= tangent_tol:
|
|
return [intersections[0]]
|
|
else:
|
|
return intersections
|
|
|
|
# adapted from https://stackoverflow.com/questions/3252194/numpy-and-line-intersections
|
|
@staticmethod
|
|
def get_line_intersection(a1, a2, b1, b2):
|
|
"""
|
|
Returns the point of intersection of the lines passing through a2,a1 and b2,b1.
|
|
a1: [x, y] a point on the first line
|
|
a2: [x, y] another point on the first line
|
|
b1: [x, y] a point on the second line
|
|
b2: [x, y] another point on the second line
|
|
"""
|
|
s = np.vstack([a1, a2, b1, b2]) # s for stacked
|
|
h = np.hstack((s, np.ones((4, 1)))) # h for homogeneous
|
|
l1 = np.cross(h[0], h[1]) # get first line
|
|
l2 = np.cross(h[2], h[3]) # get second line
|
|
x, y, z = np.cross(l1, l2) # point of intersection
|
|
if z == 0: # lines are parallel
|
|
return np.array([float('inf'), float('inf')])
|
|
return np.array([x / z, y / z], float)
|
|
|
|
@staticmethod
|
|
def get_score_based_on_tanh(input, vertical_scaling=0.5, horizontal_scaling=1, vertical_shift=0.5,
|
|
horizontal_shift=0) -> float:
|
|
'''
|
|
Based on hiperbolic tangent function, it's calculated a score between 0 and 1 (or others, depending on the parameters)
|
|
|
|
Function: a * tanh(x * c + d) + b
|
|
|
|
:param input: x value of the function
|
|
:param vertical_scaling: strech (a > 1) or compress (0 < a < 1) the function
|
|
:param horizontal_scaling: strech (c > 1) or compress (0 < c < 1) the function
|
|
:param vertical_shift: shift up (b > 0) or down (b < 0) the function
|
|
:param horizontal_shift: shift right (d < 0) or left (d > 0) the function
|
|
'''
|
|
|
|
return vertical_scaling * np.tanh(input * horizontal_scaling + horizontal_shift) + vertical_shift
|
|
|
|
@staticmethod
|
|
def get_angle_to_origin_radians(point: np.ndarray):
|
|
x, y = point
|
|
|
|
if x == 0:
|
|
return pi / 2 if y > 0 else -pi / 2
|
|
|
|
result = atan2(y, x)
|
|
|
|
result = result if not isnan(result) else pi / 2
|
|
|
|
return MathOps.normalize_rad(result)
|