# Source code for simulation.utils.geometry.polygon

```"""Polygon."""

from contextlib import suppress
from typing import List

import geometry_msgs.msg as geometry_msgs
import numpy as np
import shapely.affinity as affinity
import shapely.geometry  # Base class

from simulation.utils.geometry.point import Point
from simulation.utils.geometry.transform import Transform

from .frame import validate_and_maintain_frames

[docs]class Polygon(shapely.geometry.polygon.Polygon):
"""Polygon class inheriting from shapely's Polygon class.

Inheriting from shapely enables to use their powerful operations in combination \
with other objects,
e.g. polygon intersections.

Initialization can be done in one of the following ways.

Args:
1 ([Point]): List of points or anything that can be initialized as a point,
e.g. Vector, geometry_msgs.Point,np.array
2 (geometry_msgs.Polygon)
3 ((Line,Line)): Two lines which are interpreted as the left \
and right boundary of the polygon, e.g. a road lane
"""

def __init__(self, *args, frame=None):
"""Polygon initialization."""

# Due to recursive calling of the init function, the frame should be set
# in the first call within the recursion only.
if not hasattr(self, "_frame"):
self._frame = frame

# Create points out of arguments:
with suppress(NotImplementedError, IndexError, TypeError):
args = ([Point(p) for p in args[0]], 0)  # Make tuple

# Try to initialize directly
with suppress(Exception):
super().__init__(*args)
return

# Try to initialize from list of points
with suppress(Exception):
args = [[p.x, p.y, p.z] for p in args[0]]
super().__init__(args)
return

# Try to initialize from geometry_msgs/Polygon
with suppress(Exception):
super().__init__([[p.x, p.y, p.z] for p in args[0].points])
return

# Try to initialize from two lines
with suppress(AttributeError):
points = args[1].get_points()
points.extend(reversed(args[0].get_points()))

self.__init__(points)
return

# None of the initializations worked
raise NotImplementedError(
f"Polygon initialization not implemented for {type(args[0])}"
)

[docs]    def get_points(self) -> List[Point]:
"""Points of polygon.

Returns:
list of points on the polygon.
"""
return [Point(x, y, z, frame=self._frame) for x, y, z in self.exterior.coords]

[docs]    def to_geometry_msg(self):
"""To ROS geometry_msg.

Returns:
This polygon as a geometry_msgs.Polygon.
"""
msg = geometry_msgs.Polygon()
msg.points = [geometry_msgs.Point32(*p.to_numpy()) for p in self.get_points()]
return msg

[docs]    def to_numpy(self):
"""To numpy array.

Returns:
Polygon as a numpy array of np.arrays.
"""
return np.array([p.to_numpy() for p in self.get_points()])

@validate_and_maintain_frames
def __rmul__(self, tf: Transform):
"""Transform this polygon.

Args:
tf (Transform): Transformation to apply

Rotate the polygon tf.rotation around (0,0,0) and translate by tf.xyz

Returns:
Transformed polygon.
"""
if not type(tf) is Transform:
return NotImplemented

# Get affine matrix, turn into list and restructure how shapely expects the input
flat = list(tf.to_affine_matrix().flatten())
flat = flat[0:3] + flat[4:7] + flat[8:11] + [flat[3], flat[7], flat[11]]

transformed = affinity.affine_transform(self, flat)

return self.__class__(transformed.exterior.coords)

@validate_and_maintain_frames
def __eq__(self, polygon: "Polygon"):
"""Compare two polygons using shapely's almost_equals.

Also allow the points to be provided in the reversed order.
"""
return self.almost_equals(polygon) or self.almost_equals(
Polygon(reversed(polygon.get_points()))
)

def __repr__(self) -> str:
return f"{self.__class__.__qualname__}({self.get_points()})" + (
f",frame={self._frame.name}" if self._frame is not None else ""
)
```