Dribble/math_ops/Math_Ops.py

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)