#include <uavoidcellgraph.h>

Public Member Functions
int	findBestCellPath (UAvoidParams par, UAvoidVertexCosts costAdd, bool debugDump)

int	getCellPathPoly (int idx, UPolygon *poly, double maxBoxEdgeValue)

bool	getCellPoly (int idx, UPolygon *poly, double maxBoxEdgeValue)

int	getCellsCnt ()

int	getCellsPathCnt ()

UAvoidCellVertex *	getVertexList ()

int	getVertexListCnt ()

void	makeCellDecomposition (UAvoidObst aogs[], int aogsCnt, UAvoidParams par, bool debugDump=false)

	UAvoidCellGraph ()

	~UAvoidCellGraph ()

Public Attributes
UPolygon40	polyCell

UPolygon40	polyVertex

Private Member Functions
int	fillVerticeList (UAvoidObst *aogs, const int aogsCnt, UAvoidVertexIdx verts, UAvoidVertexIdx **vertsp, const int vertMaxCnt)

UAvoidCell *	findCell (UPose pos)

int	findCellPath (UPose pStart, UPose pEnd, UAvoidVertexCosts *costAdd)

bool	findCellPointPath (UPose pStart, UPose pEnd)

int	getMaxVertexCount (UAvoidObst **aogs, const int aogsCnt)
	get total number of vertices in graph More...

UAvoidCell *	getNewCell ()

int	makeCells (UAvoidVertexIdx *vertsp, const int vertsCnt, UAvoidCell cells, bool debugDump)

void	printObstacleList (FILE dest, UAvoidObst *aogs, const int aogsCnt)

void	printVetrexList (FILE *dest)

Private Attributes
UAvoidCell *	cells

int	cellsCnt

int	cellsCntMax

ULogFile	debugLog

UAvoidCell **	pacs

int	pacsCnt

UAvoidCellVertex *	pavs

int	pavsCnt

int	pavsCntMax

UAvoidVertexIdx *	verts

int	vertsCnt

int	vertsMaxCnt

UAvoidVertexIdx **	vertsp

Detailed Description

Class for path planning using an exact cell decomposition method.

Constructor & Destructor Documentation

UAvoidCellGraph::UAvoidCellGraph ( )

Constructor

UAvoidCellGraph::~UAvoidCellGraph ( )

Destructor

Member Function Documentation

int UAvoidCellGraph::fillVerticeList	(	UAvoidObst **	aogs,
		const int	aogsCnt,
		UAvoidVertexIdx *	verts,
		UAvoidVertexIdx **	vertsp,
		const int	vertMaxCnt
	)

private

make a sortex list of valid vertices in the set of obstacles.

Parameters

aogs	is a list of obstacle groups
aogsCnt	is the number of obstacle groups
verts	vertex list for cell generation
vertsp	vertex pointer list for cell generation
vertMaxCnt	is the size of the allocated vertex list array

Returns: number of valid vertices.

Is this position inside another obstacle

Is the point near a side of another obstacle - then move the point

Is this obstacle embedded in any other from the group

References compareUAvoidVertexIdx(), UAvoidVertexIdx::extent4thPnt(), UPolygon::getCogXY(), UPolygon::getDistanceXYsigned2(), ULineSegment::getOtherEnd(), UPolygon::getPoint(), UPolygon::getPoints(), UPolygon::getPointsCnt(), ULine::getPositionOnLine(), UPolygon::getSegment(), UObstacle::getSerial(), UAvoidObst::grp, UAvoidObst::grpIdx, UPolygon::isEmbedded(), UPolygon::isOverlappingXYtype(), ULineSegment::length, UAvoidVertexIdx::obst, UAvoidObst::obst, UAvoidVertexIdx::obst2, UAvoidVertexIdx::oIdx, UAvoidVertexIdx::pos, ULine::pos, UAvoidVertexIdx::set(), UPosition::set(), ULineSegment::setFromPoints(), UPolygon::toConvex(), UAvoidVertexIdx::type, ULine::vec, U2Dpos::x, UPosition::x, U2Dpos::y, and UPosition::y.

int UAvoidCellGraph::findBestCellPath	(	UAvoidParams *	par,
		UAvoidVertexCosts *	costAdd,
		bool	debugDump
	)

Find best path in the constructed cells (in pacs array).

Parameters

par	is the obstacle avoidance parameters to use.
costAdd	additional cost to selected points
debugDump	saves a lot of values to file if true.

Returns: number of vertecies to e avoided.

References UAvoidParams::exitPose, UPose::h, limitToPi(), maxi(), UAvoidVertexIdx::obst, UAvoidVertexIdx::pos, UAvoidVertexCosts::setExtraCost(), UAvoidParams::startPose, U2Dpos::x, UPose::x, U2Dpos::y, and UPose::y.

Referenced by UAvoidPath2::findObstAvoidRoute().

UAvoidCell * UAvoidCellGraph::findCell ( UPose pos )

private

Find the cell with this position

Parameters

pos	is the position for looking for a cell.

Returns: NULL if no cell is found - position is inside an obstacle.

References U2Dlined::distanceSigned(), U2Dlined::set2P(), UPose::x, and UPose::y.

int UAvoidCellGraph::findCellPath	(	UPose	pStart,
		UPose	pEnd,
		UAvoidVertexCosts *	costAdd
	)

private

Find shortest path between these obstacles.

Parameters

pStart	is the start pose and
pEnd	is the endPose
costAdd	list of positions with added cost.

Returns: number of path vertices returned in the 'pavs' array.

References UAvoidCell::cells, UPose::getDistance(), UAvoidVertexCosts::getExtraCost(), UAvoidCell::oPoss, U2Dpos::set(), U2Dpos::x, UPose::x, U2Dpos::y, and UPose::y.

bool UAvoidCellGraph::findCellPointPath	(	UPose	pStart,
		UPose	pEnd
	)

private

Find avoidance points from list of cells - from this start end end position. The result is saved in the 'pavs' array and the pavsCnt is set to the number of cells-> If a cell has an obstacle that is not NULL, then it is a point to avoid.

Parameters

pStart	is the start pose and
pEnd	is the endPose

Returns: true if the 'pavs' array is set sucessfully

References UAvoidCellVertex::avoidLeft, UAvoidCell::cells, U2Dlined::distanceSigned(), UAvoidCellVertex::exitSeg, UAvoidCellVertex::finished, UAvoidCell::getCellIdx(), U2Dlined::getCrossing(), U2Dseg::getFirstEnd(), U2Dseg::getOtherEnd(), UAvoidCell::id, UAvoidCellVertex::isFwd(), UAvoidCellVertex::nextIdx, UAvoidCellVertex::obst, UAvoidCell::obsts, UAvoidCell::oPoss, UAvoidCellVertex::pos, UPosition::set(), U2Dpos::set(), U2Dseg::setFromPoints(), U2Dseg::setFromPose(), U2Dpos::x, UPose::x, U2Dpos::y, and UPose::y.

int UAvoidCellGraph::getCellPathPoly	(	int	idx,
		UPolygon *	poly,
		double	maxBoxEdgeValue
	)

Get a specific cell used in the path converted to a 4-point polygon

Parameters

idx	is the cell to convert
poly	is a pointer to the polygon structure, where to put the data.
maxBoxEdgeValue	limits the value of produced polygon vertex.

Returns: ID of the cell (cell number).

References UPolygon::clear(), UAvoidCell::getPoly(), and UAvoidCell::id.

Referenced by UResAvoid::copyCellPolys().

bool UAvoidCellGraph::getCellPoly	(	int	idx,
		UPolygon *	poly,
		double	maxBoxEdgeValue
	)

Get a specific cell converted to a 4-point polygon

Parameters

idx	is the cell to convert
poly	is a pointer to the polygon structure, where to put the data.
maxBoxEdgeValue	limits the value of produced polygon vertex.

Returns: true if the polygon structure is set.

References UPolygon::clear(), and UAvoidCell::getPoly().

Referenced by UResAvoid::copyCellPolys().

int UAvoidCellGraph::getCellsCnt ( )

inline

Get number of produced cells

Referenced by UResAvoid::copyCellPolys(), and UAvoidPath2::findObstAvoidRoute().

int UAvoidCellGraph::getCellsPathCnt ( )

inline

Get number of cells in path

Referenced by UResAvoid::copyCellPolys().

int UAvoidCellGraph::getMaxVertexCount	(	UAvoidObst **	aogs,
		const int	aogsCnt
	)

private

get total number of vertices in graph

References UPolygon::getPointsCnt(), UAvoidObst::grp, and UAvoidObst::obst.

UAvoidCell * UAvoidCellGraph::getNewCell ( )

private

Get new initialized cell.

References UAvoidCell::init().

UAvoidCellVertex* UAvoidCellGraph::getVertexList ( )

inline

Get list of obstacle points to avoid. This is the result of the cell-based obstacle avoidance path. The points to use is those where obstacle pointer is not NULL. Points are in the order from start pose to end pose, but start and exit point is not in the list.

Returns: a pointer to the vertex list array.

Referenced by UAvoidPath2::findObstAvoidRoute().

int UAvoidCellGraph::getVertexListCnt ( )

inline

Get number of valid elements in the vertex list

References UAvoidVertexCost::pos.

Referenced by UAvoidPath2::findObstAvoidRoute().

void UAvoidCellGraph::makeCellDecomposition	(	UAvoidObst *	aogs[],
		int	aogsCnt,
		UAvoidParams *	par,
		bool	debugDump = `false`
	)

Convert obstacles into a cell map using an absolute cell decomposition method

Parameters

aogs	is the obstacles to avoid
aogsCnt	is the number of obstacle groups.
par	is the obstacle avoidance parameters to use.
debugDump	saves a lot of values to file if true.

References UTime::getTimePassed(), UTime::now(), and UAvoidParams::scanSerial.

Referenced by UAvoidPath2::findObstAvoidRoute().

int UAvoidCellGraph::makeCells	(	UAvoidVertexIdx **	vertsp,
		const int	vertsCnt,
		UAvoidCell *	cells,
		bool	debugDump
	)

private

Make cells from these vertice points

Parameters

vertsp	is a sorted list (in x order) of verteces
vertsCnt	is the number of vertices
cells	is an array of cells for the resulting cells (max size is twice then number of vertices.)

Returns: the number of produced cells.

References UAvoidCell::cells, UAvoidVertexIdx::getOther(), UAvoidVertexIdx::getPktType(), UObstacle::getSerial(), UAvoidVertexIdx::getUpper(), UAvoidCell::id, UAvoidCell::link(), UAvoidVertexIdx::obst, UAvoidObst::obst, UAvoidVertexIdx::obst2, UAvoidCell::obsts, UAvoidCell::oPoss, UAvoidVertexIdx::pos, UAvoidCell::print(), U2Dpos::set(), UAvoidCell::setCuts(), UAvoidVertexIdx::type, UAvoidCell::unlinkOpen(), U2Dpos::x, and U2Dpos::y.

void UAvoidCellGraph::printObstacleList	(	FILE *	dest,
		UAvoidObst **	aogs,
		const int	aogsCnt
	)

private

Dump obstacles to console

Parameters

dest	is a destination streem handle.
aogs	is array of obstacle groups
aogsCnt	is number of obstacle groups

References UPolygon::getPoint(), UPolygon::getPointsCnt(), UObstacle::getSerial(), UAvoidObst::grp, UAvoidObst::noVisCnt, UAvoidObst::obst, UAvoidVertexIdx::pos, UPosition::x, and UPosition::y.

void UAvoidCellGraph::printVetrexList ( FILE * dest )

private

Dump vertex list to console

Parameters

dest	is a destination streem handle.

References UAvoidVertexIdx::getPktType(), UObstacle::getSerial(), UAvoidVertexIdx::obst, UAvoidObst::obst, UAvoidVertexIdx::obst2, UAvoidVertexIdx::oIdx, UAvoidVertexIdx::pos, UAvoidVertexIdx::type, U2Dpos::x, and U2Dpos::y.

Member Data Documentation

UAvoidCell* UAvoidCellGraph::cells

private

Array of cells that constitute the path - reallocated if needed

int UAvoidCellGraph::cellsCnt

private

Number of actually used cells

int UAvoidCellGraph::cellsCntMax

private

Number of allocated cells

ULogFile UAvoidCellGraph::debugLog

private

Debug logfile

UAvoidCell** UAvoidCellGraph::pacs

private

Array of cells from start to end.

int UAvoidCellGraph::pacsCnt

private

Number of cells in path

UAvoidCellVertex* UAvoidCellGraph::pavs

private

Vertex list to pass to get from start to end, i.e. best path through the obstacles.

int UAvoidCellGraph::pavsCnt

private

Used number of vertices in verts

int UAvoidCellGraph::pavsCntMax

private

Maksimum number of elements in verts

UPolygon40 UAvoidCellGraph::polyCell

list of less wanted vertices, i.e additional cost when passing these vertex positions Number of valid positions in costs list Number of available positions in costs list Polygon with raw cell path

Referenced by UResAvoid::copyCellPolys().

UPolygon40 UAvoidCellGraph::polyVertex

Polygon with cell path vertex list

Referenced by UResAvoid::copyCellPolys().

UAvoidVertexIdx* UAvoidCellGraph::verts

private

Pointer to array of vertices in current obstacle configuration

int UAvoidCellGraph::vertsCnt

private

Current number of vertices in array

int UAvoidCellGraph::vertsMaxCnt

private

Number of valid entries in vertex array

UAvoidVertexIdx** UAvoidCellGraph::vertsp

private

Array of pointers in increasing x order to vertices in current obstacle configuration

The documentation for this class was generated from the following files:

Public Member Functions

Public Attributes

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation