utils.py

import numpy as np
MAX_NUMANCHOR = 6


def is_position_jump_too_large(current_position,last_position,max_distance=2.0):

    distance = np.linalg.norm(np.array(current_position) - np.array(last_position))
    
    if distance > max_distance:
        return False
    
    return True

def calculate_anchor_bounds(anchor_positions):

    anchor_positions = np.array(anchor_positions)
    Xmin = np.min(anchor_positions[:, 0])
    Xmax = np.max(anchor_positions[:, 0])
    Ymin = np.min(anchor_positions[:, 1])
    Ymax = np.max(anchor_positions[:, 1])
    return Xmin, Xmax, Ymin, Ymax

def is_within_bounds(positionCur, anchor_positions, margin=0.5):

    Xmin, Xmax, Ymin, Ymax = calculate_anchor_bounds(anchor_positions)

    x, y = positionCur
    return (Xmin - margin <= x <= Xmax + margin) and (Ymin - margin <= y <= Ymax + margin)