#include <algorithm>
#include <cmath>
#include <list>
#include <stack>
#include <vector>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Partition_is_valid_traits_2.h>
#include <CGAL/Partition_traits_2.h>
#include <CGAL/partition_2.h>
#include <CGAL/point_generators_2.h>
#include <CGAL/polygon_function_objects.h>
#include <CGAL/random_polygon_2.h>
#include <ros/ros.h>
#include <geometry_msgs/Point.h>

Include dependency graph for cgutil.hpp:

This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Typedefs
using	PointVector = std::vector< geometry_msgs::Point >

using	LineSegment = std::array< geometry_msgs::Point, 2 >

using	LineSegmentVector = std::vector< LineSegment >

using	K = CGAL::Exact_predicates_inexact_constructions_kernel

using	Traits = CGAL::Partition_traits_2< K >

using	Polygon_2 = Traits::Polygon_2

using	Point_2 = Traits::Point_2

using	Vertex_iterator = Polygon_2::Vertex_const_iterator

using	Polygon_list = std::list< Polygon_2 >

Functions
double	calculateSignedArea (const geometry_msgs::Point &p1, const geometry_msgs::Point &p2, const geometry_msgs::Point &p3)
	Calculates signed area of given triangle. More...

bool	operator== (const geometry_msgs::Point &p1, const geometry_msgs::Point &p2)
	Check equality of two points. More...

bool	operator!= (const geometry_msgs::Point &p1, const geometry_msgs::Point &p2)
	Check equality of two points. More...

LineSegmentVector	generateEdgeVector (const PointVector &vec, bool isClosed)
	Generate Vector of line segment from given PointVector. More...

double	calculateDistance (const geometry_msgs::Point &p1, const geometry_msgs::Point &p2)
	Calculates distance between given two points. More...

double	calculateVertexAngle (const geometry_msgs::Point &p1, const geometry_msgs::Point &p2, const geometry_msgs::Point &p3)
	Calculates angle between segment p1p2 and p1p3. More...

double	calculateHorizontalAngle (const geometry_msgs::Point &p1, const geometry_msgs::Point &p2)
	Calculates angle between segment p1p2 and horizontal line. More...

double	calculateDistance (const LineSegment &edge, const geometry_msgs::Point &vertex)
	Calculates distance between given edge and vertex. More...

PointVector	computeConvexHull (PointVector points)
	Returns convex hull of given points. More...

bool	isConvex (PointVector points)
	Checks if given polygon is convex or not. More...

bool	hasIntersection (const LineSegment &edge1, const LineSegment &edge2)
	Checks if given edges intersect each other. More...

bool	hasIntersection (const LineSegmentVector &vec1, const LineSegmentVector &vec2)
	Checks if given vectors of edges have at least one intersection. More...

geometry_msgs::Point	localizeIntersection (const LineSegment &edge1, const LineSegment &edge2)
	Find the location where given edges intersect each other. More...

PointVector	rotatePoints (const PointVector &points, double angle_rad)
	Rotate points by given angle around the origin. More...

std::vector< PointVector >	decomposePolygon (const PointVector &polygon)
	Decompose given polygon. More...

Typedef Documentation

using K = CGAL::Exact_predicates_inexact_constructions_kernel

Definition at line 42 of file cgutil.hpp.

using LineSegment = std::array<geometry_msgs::Point, 2>

Definition at line 38 of file cgutil.hpp.

using LineSegmentVector = std::vector<LineSegment>

Definition at line 39 of file cgutil.hpp.

using Point_2 = Traits::Point_2

Definition at line 45 of file cgutil.hpp.

using PointVector = std::vector<geometry_msgs::Point>

Definition at line 37 of file cgutil.hpp.

using Polygon_2 = Traits::Polygon_2

Definition at line 44 of file cgutil.hpp.

using Polygon_list = std::list<Polygon_2>

Definition at line 47 of file cgutil.hpp.

using Traits = CGAL::Partition_traits_2<K>

Definition at line 43 of file cgutil.hpp.

using Vertex_iterator = Polygon_2::Vertex_const_iterator

Definition at line 46 of file cgutil.hpp.

Function Documentation

double calculateDistance	(	const geometry_msgs::Point &	p1,
		const geometry_msgs::Point &	p2
	)

inline

Calculates distance between given two points.

Parameters

p1
p2

Returns: double Distance between two points

Definition at line 158 of file cgutil.hpp.

double calculateDistance	(	const LineSegment &	edge,
		const geometry_msgs::Point &	vertex
	)

Calculates distance between given edge and vertex.

Parameters

edge	An edge of given polygon
vertex	A vertex of given polygon

Returns: double Distance between edge and vertex

Definition at line 216 of file cgutil.hpp.

double calculateHorizontalAngle	(	const geometry_msgs::Point &	p1,
		const geometry_msgs::Point &	p2
	)

Calculates angle between segment p1p2 and horizontal line.

Parameters

p1	A vertex which is the origin of segment p1p2
p2	The other vertex of segment p1p2

Returns: Vertex angle of p1 in radian [-pi, pi]

Definition at line 202 of file cgutil.hpp.

double calculateSignedArea	(	const geometry_msgs::Point &	p1,
		const geometry_msgs::Point &	p2,
		const geometry_msgs::Point &	p3
	)

inline

Calculates signed area of given triangle.

Parameters

p1	The origin of vector $\vec{p_1p_2}$ and $\vec{p_1p_3}$
p2	The end point of vector $\vec{p_1p_2}$
p3	The end point of vector $\vec{p_1p_3}$

Returns: Signed area of given triangle

Signed area of triangle $ S(p1, p2, p3) $ is half of the outer product of vector $\vec{p_1p_2}$ and $\vec{p_1p_3}$ .

$S(p_1, p_2, p_3) = \frac{1}{2} \vec{p_1p_2} \times \vec{p_1p_3}$

And that can be written as follows,

$\begin{eqnarray*} S(p_1, p_2, p_3) & = & \frac{1}{2} \left| \begin{array}{ccc} p_1.x & p_1.y & 1 \\ p_2.x & p_2.y & 1 \\ p_3.x & p_3.y & 1 \end{array} \right| \\ & = & p_1.x(p_2.y - p_3.y) - p_1.y(p_2.x - p_3.x) - (p_2.x\times p_3.y - p_2.y\times p_3.x) \end{eqnarray*}$

Definition at line 74 of file cgutil.hpp.

double calculateVertexAngle	(	const geometry_msgs::Point &	p1,
		const geometry_msgs::Point &	p2,
		const geometry_msgs::Point &	p3
	)

Calculates angle between segment p1p2 and p1p3.

Parameters

p1	A vertex which is the origin of segment p1p2 and p1p3
p2	The other point of segment p1p2
p3	The other point of segment p1p3

Returns: Angle between segment p1p2 and p1p3 in radian [0, pi)

Definition at line 170 of file cgutil.hpp.

PointVector computeConvexHull ( PointVector points )

Returns convex hull of given points.

Parameters

points A set of points in the plane

Returns: Convex hull of given points

This function is based on graham scan algorithm

Definition at line 249 of file cgutil.hpp.

std::vector<PointVector> decomposePolygon ( const PointVector & polygon )

Decompose given polygon.

Parameters

polygon Polygon to be decomposed

Returns: std::vector<PointVector> Decomposed polygons

This function uses CGAL::optimal_convex_partition_2 in order to perform optimal polygon decomposition. Note that this function has O(n^4) time complexity and O(n^3) space complexity. Use approx_convex_partition_2 instead if the number of vertices are big because its time complexity is O(n). But apptox_convex_partition_2 generates more polygons. For detail, see https://doc.cgal.org/latest/Partition_2/index.html

Definition at line 468 of file cgutil.hpp.

LineSegmentVector generateEdgeVector	(	const PointVector &	vec,
		bool	isClosed
	)

Generate Vector of line segment from given PointVector.

Parameters

vec
isClosed	Set true if the first point and the last are connected

Returns: LineSegmentVector Vector of line segment a.k.a. std::vector<std::array<geometry_msgs::Point, 2>>

Definition at line 116 of file cgutil.hpp.

bool hasIntersection	(	const LineSegment &	edge1,
		const LineSegment &	edge2
	)

Checks if given edges intersect each other.

Parameters

edge1	An edge
edge2	An edge

Returns: True if two edges intersect

Definition at line 335 of file cgutil.hpp.

bool hasIntersection	(	const LineSegmentVector &	vec1,
		const LineSegmentVector &	vec2
	)

Checks if given vectors of edges have at least one intersection.

Parameters

vec1	Vector of line segments
vec2	Vector of line segments

Returns: True if given two vectors of edges have at least one intersection

Definition at line 375 of file cgutil.hpp.

bool isConvex ( PointVector points )

inline

Checks if given polygon is convex or not.

Parameters

points Points consisting of polygon is to be checked

Returns: True if given polygon is convex, false if it's not convex

Definition at line 324 of file cgutil.hpp.

geometry_msgs::Point localizeIntersection	(	const LineSegment &	edge1,
		const LineSegment &	edge2
	)

Find the location where given edges intersect each other.

Parameters

edge1
edge2

Returns: geometry_msgs::Point Point of intersection

See http://mf-atelier.sakura.ne.jp/mf-atelier/modules/tips/program/algorithm/a1.html

Definition at line 398 of file cgutil.hpp.

bool operator!=	(	const geometry_msgs::Point &	p1,
		const geometry_msgs::Point &	p2
	)

Check equality of two points.

Parameters

p1
p2

Returns: bool

Definition at line 105 of file cgutil.hpp.

bool operator==	(	const geometry_msgs::Point &	p1,
		const geometry_msgs::Point &	p2
	)

Check equality of two points.

Parameters

p1
p2

Returns: bool

See https://stackoverflow.com/questions/4010240/comparing-doubles

Definition at line 87 of file cgutil.hpp.

PointVector rotatePoints	(	const PointVector &	points,
		double	angle_rad
	)

Rotate points by given angle around the origin.

Parameters

points	Points to be rotated
angle_rad	Rotation angle in radian

Returns: PointVector Rotated points

Definition at line 437 of file cgutil.hpp.

Typedefs

Functions

Typedef Documentation

Function Documentation