find_angle_viz.py

import math

import matplotlib.pyplot as plt
import numpy as np
from matplotlib.widgets import Button, Slider


def find_angle_of_vector_that_comes_within(distance, point, direction="left"):
    x_p, y_p = point

    if x_p != 0:
        theta_min = np.arctan2(y_p, x_p)
    else:
        theta_min = np.pi / 2 if y_p > 0 else -np.pi / 2

    # Now, we need to adjust theta_min to achieve the exact specified distance
    # The current minimum distance is the perpendicular distance from (x_p, y_p)
    # to the line at theta_min
    current_distance = np.abs(np.sin(theta_min) * x_p - np.cos(theta_min) * y_p)

    # If the current distance is greater than the specified distance, adjust theta accordingly
    if current_distance != distance:
        theta_adjust = np.arcsin(distance / np.sqrt(x_p**2 + y_p**2))
        if direction == "left":
            theta = theta_min + theta_adjust
        else:
            theta = theta_min - theta_adjust
    else:
        theta = theta_min

    # Calculate the point on the line that is exactly 'distance' away from the given point
    # The line passes through the point (x_p, y_p) and has the angle 'theta'
    if direction == "left":
        x_line = x_p - distance * np.sin(theta)
        y_line = y_p + distance * np.cos(theta)
    else:
        x_line = x_p + distance * np.sin(theta)
        y_line = y_p - distance * np.cos(theta)

    return theta, (x_line, y_line)


def update_plot(val):
    x = x_slider.val
    y = y_slider.val
    distance = distance_slider.val

    ax.clear()
    ax.set_aspect("equal")
    ax.set_xlim(-10, 10)
    ax.set_ylim(-10, 10)
    ax.axhline(0, color="black", lw=0.5)
    ax.axvline(0, color="black", lw=0.5)

    # Plot the point based on x and y values
    ax.plot(x, y, "bo")

    # Compute the endpoint of the line originating from the origin
    try:
        theta, (point_x, point_y) = find_angle_of_vector_that_comes_within(
            distance, (x, y), direction
        )
        ax.plot(point_x, point_y, "ro", label="Approach Point")
        end_x = math.cos(theta) * 30
        end_y = math.sin(theta) * 30
        start_x = math.cos(theta) * -30
        start_y = math.sin(theta) * -30
        # Plot the line from the origin to the endpoint
        ax.plot([start_x, end_x], [start_y, end_y], color="red")
    except ValueError:
        pass  # Handle case where distance is too large for the given point

    fig.canvas.draw_idle()


def toggle_direction(event):
    global direction
    direction = "right" if direction == "left" else "left"
    direction_button.label.set_text(f"Direction: {direction}")
    update_plot(None)


# Set up the plot
fig, ax = plt.subplots()
plt.subplots_adjust(left=0.25, bottom=0.45)
ax.set_aspect("equal")
ax.set_xlim(-10, 10)
ax.set_ylim(-10, 10)
ax.axhline(0, color="black", lw=0.5)
ax.axvline(0, color="black", lw=0.5)

# Sliders for x, y, and distance
ax_x = plt.axes([0.25, 0.25, 0.65, 0.03])
ax_y = plt.axes([0.25, 0.20, 0.65, 0.03])
ax_distance = plt.axes([0.25, 0.15, 0.65, 0.03])

x_slider = Slider(ax_x, "X", -10.0, 10.0, valinit=3.0)
y_slider = Slider(ax_y, "Y", -10.0, 10.0, valinit=4.0)
distance_slider = Slider(ax_distance, "Distance", 0.1, 10.0, valinit=5.0)

# Button for toggling direction
ax_button = plt.axes([0.4, 0.05, 0.2, 0.05])
direction_button = Button(ax_button, "Direction: left")
direction_button.on_clicked(toggle_direction)

# Initial direction
direction = "left"

# Update plot when sliders are changed
x_slider.on_changed(update_plot)
y_slider.on_changed(update_plot)
distance_slider.on_changed(update_plot)

# Initial plot
update_plot(None)

plt.show()