Reza Adousi
256 lines
8.4 KiB
Python
256 lines
8.4 KiB
Python
# geometry.py
|
|
#
|
|
# Small, shared helpers for working with ROTATED rectangles (an
|
|
# "OBB" - oriented bounding box) in screen space: a rectangle
|
|
# stickers can be glued on crooked, so an obstacle's position is no
|
|
# longer just an axis-aligned (x, y, width, height) - it also has
|
|
# an angle. This module is the single place that knows how to:
|
|
#
|
|
# - turn a raw cv2.minAreaRect() result into a STABLE, canonical
|
|
# (center, long side, short side, angle) representation that
|
|
# doesn't jitter or jump between frames (see normalize_angle
|
|
# and detection.py for why that's needed),
|
|
# - blend/interpolate that angle smoothly across frames
|
|
# (blend_angle),
|
|
# - convert between world space and the rectangle's own local,
|
|
# axis-aligned frame (world_to_local / local_to_world) - this
|
|
# is what lets both collision detection AND collision response
|
|
# be written as plain axis-aligned math that "just works" for
|
|
# any rotation, by doing the rotation once at the boundary
|
|
# instead of re-deriving rotated formulas everywhere,
|
|
# - compute the 4 corners of a rectangle for drawing
|
|
# (obb_corners),
|
|
# - pull all of the above out of a detected-obstacle dict in one
|
|
# place, with one shared definition of what counts as a valid
|
|
# obstacle (extract_obb), so detection/physics/rendering can
|
|
# never disagree about it.
|
|
#
|
|
# Angle convention: an un-rotated rectangle (long side horizontal)
|
|
# has angle 0. Positive angles rotate the long side counter-
|
|
# clockwise in standard math coordinates - visually CLOCKWISE on
|
|
# screen, since screen y grows downward. Angles are always kept in
|
|
# the canonical range (-90, 90], because a rectangle looks
|
|
# identical after a 180-degree rotation (it has no "arrow" printed
|
|
# on it), so anything outside that range is a redundant duplicate
|
|
# of an angle already inside it. Collapsing to one canonical range
|
|
# is what keeps blend_angle's shortest-path interpolation well
|
|
# defined instead of it occasionally spinning the "wrong" way.
|
|
|
|
import math
|
|
|
|
|
|
# ============================================================
|
|
# Angle canonicalization
|
|
# ============================================================
|
|
|
|
def normalize_angle(angle_degrees, period=180.0):
|
|
"""
|
|
Fold an orientation angle into the canonical (-period/2,
|
|
period/2] range.
|
|
|
|
A rectangle has 180-degree rotational symmetry, so any angle is
|
|
equivalent to one inside a 180-degree-wide window (period=180).
|
|
A (nearly) SQUARE sticker is even more symmetric - it looks the
|
|
same after every 90 degrees - so for those callers pass
|
|
period=90. Always folding into the SAME window is what makes
|
|
two angles describing the same physical orientation compare and
|
|
blend as equal instead of looking like they are far apart.
|
|
"""
|
|
half = period / 2.0
|
|
|
|
angle = angle_degrees % period
|
|
|
|
if angle > half:
|
|
angle -= period
|
|
|
|
return angle
|
|
|
|
|
|
def blend_angle(old_angle, new_angle, alpha, period=180.0):
|
|
"""
|
|
Move old_angle toward new_angle by fraction alpha along the
|
|
SHORTEST path, for orientations that repeat every `period`
|
|
degrees.
|
|
|
|
A naive `old + alpha * (new - old)` breaks near the wrap-around
|
|
point: 89 degrees and -89 degrees are really only 2 degrees
|
|
apart for a rectangle, but the naive subtraction sees 178 and
|
|
blends the wrong way (and for a near-square sticker, averaging
|
|
across its 90-degree ambiguity would even produce a completely
|
|
wrong in-between angle). Wrapping the raw difference into
|
|
(-period/2, period/2] first avoids both.
|
|
"""
|
|
half = period / 2.0
|
|
|
|
diff = ((new_angle - old_angle + half) % period) - half
|
|
|
|
return normalize_angle(old_angle + alpha * diff, period)
|
|
|
|
|
|
def paint_dimensions(rect_long, rect_short, inset_ratio):
|
|
"""
|
|
Size of the white patch painted on the monitor for a detected
|
|
sticker: the sticker trimmed by inset_ratio * short side on every
|
|
edge. Shared by rendering (to draw it) and detection (to blank
|
|
exactly that area out before looking for stickers) so the two can
|
|
never disagree about what was painted.
|
|
"""
|
|
inset = inset_ratio * rect_short
|
|
|
|
return (
|
|
max(1.0, rect_long - 2.0 * inset),
|
|
max(1.0, rect_short - 2.0 * inset),
|
|
)
|
|
|
|
|
|
# ============================================================
|
|
# World <-> local (rectangle-aligned) frame conversion
|
|
# ============================================================
|
|
|
|
def world_to_local(x, y, angle_degrees):
|
|
"""
|
|
Rotate a world-space vector (x, y) by -angle_degrees, i.e. into
|
|
the rectangle's own local frame, where its long side lies along
|
|
the local x-axis and its short side along the local y-axis.
|
|
|
|
This takes a VECTOR, not a point - to transform a point, first
|
|
subtract the rectangle's center, call this, and the result is
|
|
the point's position relative to the rectangle in its own
|
|
frame. The same function (with no subtraction) also correctly
|
|
rotates direction-only quantities like velocity, since rotation
|
|
doesn't care about position.
|
|
"""
|
|
theta = math.radians(angle_degrees)
|
|
|
|
cos_t = math.cos(theta)
|
|
sin_t = math.sin(theta)
|
|
|
|
return (
|
|
x * cos_t + y * sin_t,
|
|
-x * sin_t + y * cos_t,
|
|
)
|
|
|
|
|
|
def local_to_world(x, y, angle_degrees):
|
|
"""
|
|
The exact inverse of world_to_local: rotate a local-frame
|
|
vector back by +angle_degrees into world space.
|
|
"""
|
|
theta = math.radians(angle_degrees)
|
|
|
|
cos_t = math.cos(theta)
|
|
sin_t = math.sin(theta)
|
|
|
|
return (
|
|
x * cos_t - y * sin_t,
|
|
x * sin_t + y * cos_t,
|
|
)
|
|
|
|
|
|
# ============================================================
|
|
# Corners (for drawing / debugging)
|
|
# ============================================================
|
|
|
|
def obb_corners(center_x, center_y, rect_long, rect_short, angle_degrees):
|
|
"""
|
|
Return the 4 corners of a rotated rectangle, in order around
|
|
its perimeter, as world-space (x, y) tuples. Safe to hand
|
|
straight to pygame.draw.polygon or cv2.polylines.
|
|
"""
|
|
half_long = rect_long / 2.0
|
|
half_short = rect_short / 2.0
|
|
|
|
local_corners = (
|
|
(half_long, half_short),
|
|
(half_long, -half_short),
|
|
(-half_long, -half_short),
|
|
(-half_long, half_short),
|
|
)
|
|
|
|
corners = []
|
|
|
|
for local_x, local_y in local_corners:
|
|
world_dx, world_dy = local_to_world(
|
|
local_x,
|
|
local_y,
|
|
angle_degrees,
|
|
)
|
|
|
|
corners.append(
|
|
(
|
|
center_x + world_dx,
|
|
center_y + world_dy,
|
|
)
|
|
)
|
|
|
|
return corners
|
|
|
|
|
|
# ============================================================
|
|
# Obstacle dict -> OBB extraction
|
|
# ============================================================
|
|
|
|
def extract_obb(obstacle):
|
|
"""
|
|
Pull (center_x, center_y, rect_long, rect_short, angle) out of
|
|
a detected-obstacle dict, or return None if the obstacle is
|
|
missing/malformed/degenerate. This is the ONE place that
|
|
decides what counts as a usable oriented rectangle, so physics,
|
|
rendering, and validity-checking can never disagree with each
|
|
other about it.
|
|
|
|
Falls back to treating the obstacle as an axis-aligned
|
|
(angle=0) rectangle built from x/y/width/height if it has no
|
|
"center"/"rect_long"/"rect_short"/"angle" fields, so any code
|
|
that still hands in a plain axis-aligned obstacle dict keeps
|
|
working exactly as before.
|
|
"""
|
|
if not isinstance(obstacle, dict):
|
|
return None
|
|
|
|
try:
|
|
if "center" in obstacle:
|
|
center_x, center_y = obstacle["center"]
|
|
else:
|
|
center_x = (
|
|
obstacle["x"] + obstacle["width"] / 2.0
|
|
)
|
|
center_y = (
|
|
obstacle["y"] + obstacle["height"] / 2.0
|
|
)
|
|
|
|
rect_long = obstacle.get(
|
|
"rect_long",
|
|
obstacle.get("width"),
|
|
)
|
|
rect_short = obstacle.get(
|
|
"rect_short",
|
|
obstacle.get("height"),
|
|
)
|
|
angle = obstacle.get("angle", 0.0)
|
|
|
|
center_x = float(center_x)
|
|
center_y = float(center_y)
|
|
rect_long = float(rect_long)
|
|
rect_short = float(rect_short)
|
|
angle = float(angle)
|
|
except (KeyError, TypeError, ValueError):
|
|
return None
|
|
|
|
if not all(
|
|
math.isfinite(value)
|
|
for value in (
|
|
center_x,
|
|
center_y,
|
|
rect_long,
|
|
rect_short,
|
|
angle,
|
|
)
|
|
):
|
|
return None
|
|
|
|
if rect_long <= 0 or rect_short <= 0:
|
|
return None
|
|
|
|
return center_x, center_y, rect_long, rect_short, angle
|