navigation.py 文件源码

def segment_angle(line1, line2):
        """ Angle between two segments """
        vector_a = np.array([line1[0][0]-line1[1][0],
                             line1[0][1]-line1[1][1]])
        vector_b = np.array([line2[0][0]-line2[1][0],
                             line2[0][1]-line2[1][1]])
        # Get dot prod
        dot_prod = np.dot(vector_a, vector_b)
        # Get magnitudes
        magnitude_a = np.dot(vector_a, vector_a)**0.5
        magnitude_b = np.dot(vector_b, vector_b)**0.5
        # Get angle in radians and then convert to degrees
        angle = np.arccos(dot_prod/magnitude_b/magnitude_a)
        # Basically doing angle <- angle mod 360
        ang_deg = np.rad2deg(angle)%360

        if ang_deg-180 >= 0:
            # As in if statement
            return 360 - ang_deg
        else:
            return ang_deg