sentry_plan_control/omni_regulated_pure_pursuit.../doc/circle-segment-intersection...

171 lines
5.1 KiB
Plaintext

{
"cells": [
{
"cell_type": "markdown",
"id": "97dbdadd-7a94-4939-8ed5-c8551b662917",
"metadata": {},
"source": [
"# Circle Segment Intersection (for interpolation)\n",
"Here is an interactive plot that demonstrates the functionality of the formula to calculate the intersection of a line segment, and a circle centered at the origin."
]
},
{
"cell_type": "code",
"execution_count": 8,
"id": "d31dc723-a6dc-400d-8b31-fe84ea6d5e45",
"metadata": {},
"outputs": [
{
"data": {
"application/vnd.jupyter.widget-view+json": {
"model_id": "cbfad4e8309a4ee2bef53994add83330",
"version_major": 2,
"version_minor": 0
},
"text/plain": [
"VBox(children=(Label(value='A and B can be moved with the mouse. One must be inside the circle, and one must b…"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"from bqplot import *\n",
"import numpy as np\n",
"import ipywidgets as widgets\n",
"\n",
"\n",
"def circle_segment_intersection(p1, p2, r):\n",
" x1, y1 = p1\n",
" x2, y2 = p2\n",
" dx = x2 - x1\n",
" dy = y2 - y1\n",
" dr2 = dx ** 2 + dy ** 2\n",
" D = x1 * y2 - x2 * y1\n",
" d1 = x1 ** 2 + y1 ** 2\n",
" d2 = x2 ** 2 + y2 ** 2\n",
" dd = d2 - d1\n",
" sqrt_term = np.sqrt(r ** 2 * dr2 - D ** 2)\n",
" x = (D * dy + np.copysign(1.0, dd) * dx * sqrt_term) / dr2\n",
" y = (-D * dx + np.copysign(1.0, dd) * dy * sqrt_term) / dr2\n",
" return x, y\n",
"\n",
"\n",
"MAX = 5.0\n",
"x_sc = LinearScale(min=-MAX, max=MAX)\n",
"y_sc = LinearScale(min=-MAX, max=MAX)\n",
"\n",
"ax_x = Axis(label=\"x\", scale=x_sc, tick_format=\"0.0f\")\n",
"ax_y = Axis(label=\"y\", scale=y_sc, orientation=\"vertical\", tick_format=\"0.0f\")\n",
"\n",
"points = Scatter(\n",
" names=[\"A\", \"B\"], x=[0.0, 3.0], y=[2.0, 4.0], scales={\"x\": x_sc, \"y\": y_sc}, enable_move=True\n",
")\n",
"\n",
"\n",
"def get_circle(r):\n",
" t = np.linspace(0, 2 * np.pi)\n",
" x = r * np.cos(t)\n",
" y = r * np.sin(t)\n",
" return x, y\n",
"\n",
"radius_slider = widgets.FloatSlider(min=0.0, max=MAX, value=3.0, description=\"Circle radius\")\n",
"circle_x, circle_y = get_circle(radius_slider.value)\n",
"\n",
"circle = Lines(x=circle_x, y=circle_y, scales={\"x\": x_sc, \"y\": y_sc}, colors=[\"green\"])\n",
"\n",
"x1, x2 = points.x\n",
"y1, y2 = points.y\n",
"xi, yi = circle_segment_intersection((x1, y1), (x2, y2), radius_slider.value)\n",
"\n",
"intersection = Scatter(\n",
" names=[\"C\"],\n",
" x=[xi],\n",
" y=[yi],\n",
" scales={\"x\": x_sc, \"y\": y_sc},\n",
" enable_move=False,\n",
" colors=[\"purple\"],\n",
")\n",
"\n",
"fig = Figure(axes=[ax_x, ax_y], marks=[circle, points, intersection])\n",
"\n",
"fig.max_aspect_ratio = 1\n",
"fig.min_aspect_ratio = 1\n",
"\n",
"\n",
"def both_inside_or_both_outside_circle(points, r):\n",
" x1, x2 = points.x\n",
" y1, y2 = points.y\n",
" d1 = x1 ** 2 + y1 ** 2\n",
" d2 = x2 ** 2 + y2 ** 2\n",
" if d1 < r ** 2 and d2 < r ** 2:\n",
" return True\n",
" elif d1 > r ** 2 and d2 > r ** 2:\n",
" return True\n",
" else:\n",
" return False\n",
"\n",
"\n",
"def update_circle(message):\n",
" circle_x, circle_y = get_circle(radius_slider.value)\n",
" circle.x = circle_x\n",
" circle.y = circle_y\n",
" update_intersection(message)\n",
"\n",
"\n",
"def update_intersection(message):\n",
" x1, x2 = points.x\n",
" y1, y2 = points.y\n",
" r = radius_slider.value\n",
" if both_inside_or_both_outside_circle(points, r):\n",
" circle.colors = [\"red\"]\n",
" intersection.x = []\n",
" intersection.y = []\n",
" else:\n",
" circle.colors = [\"green\"]\n",
" xi, yi = circle_segment_intersection((x1, y1), (x2, y2), r)\n",
" intersection.x = [xi]\n",
" intersection.y = [yi]\n",
"\n",
"\n",
"points.observe(update_intersection, [\"x\", \"y\"])\n",
"\n",
"radius_slider.observe(update_circle, \"value\")\n",
"\n",
"widgets.VBox(\n",
" [\n",
" widgets.Label(\n",
" \"A and B can be moved with the mouse. One must be inside the circle, and one must be outside.\",\n",
" fixed=True,\n",
" ),\n",
" radius_slider,\n",
" fig,\n",
" ]\n",
")"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "Python 3",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.8.10"
}
},
"nbformat": 4,
"nbformat_minor": 5
}