You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
362 lines
13 KiB
Python
362 lines
13 KiB
Python
from math import acos, asin, atan2, cos, pi, sin, sqrt
|
|
import numpy as np
|
|
import sys
|
|
|
|
try:
|
|
GLOBAL_DIR = sys._MEIPASS # temporary folder with libs & data files
|
|
except:
|
|
GLOBAL_DIR = "."
|
|
|
|
|
|
class Math_Ops():
|
|
'''
|
|
This class provides general mathematical operations that are not directly available through numpy
|
|
'''
|
|
|
|
@staticmethod
|
|
def deg_sph2cart(spherical_vec):
|
|
''' Converts SimSpark's 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
|
|
|
|
@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(Math_Ops.normalize_vec(vec1),Math_Ops.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([Math_Ops.deg_cos(angle), Math_Ops.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 Math_Ops.normalize_rad( atan2(target[1]-pos2d[1], target[0]-pos2d[0]) - ori )
|
|
else:
|
|
return Math_Ops.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_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 = Math_Ops.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 = Math_Ops.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 = Math_Ops.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 = Math_Ops.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)
|