#include <uavoidobst.h>

Public Member Functions
bool	addtangentLines (UAvoidObst other, UAvoidLink ala, UAvoidLink alb, UAvoidLink alc, UAvoidLink *ald)

bool	addToGroupIfWithinMargin2 (UAvoidObst *other, double margin, bool noAdd)

void	clear ()

bool	crossingNonVisibilityLine (ULineSegment visLine, FILE logdbg)

double	getClosestPoint (UPosition to, double distLimit, UPosition closest, bool atVertex)

int	getTangentVertexFrom (UPosition pos, int idxCv, int idxCcv)

bool	isInSameGroup (UAvoidObst *other)

bool	passingNoGoEdge (bool entryCV, int entryVertex, int exitVertex)

bool	passingNoGoEdge2 (bool entryCV, int entryVertex, int exitVertex, UPosition pPrev, UPosition pNext, bool isStart, bool isExit)

void	testVisibility (FILE *logdbg)

	UAvoidObst ()

bool	validTangent (bool entryCV, int atVertex, UPosition p1)

Public Attributes
int	badEdges [UPolygon40::MAX_POINTS]

bool	embedEnd

bool	embedStart

UAvoidObst *	empty

UAvoidObst *	grp

int	grpIdx

bool	isExit

bool	isStart

UAvoidLink *	links

bool	nogoEdge [UPolygon40::MAX_POINTS]

bool	nogoVertex [UPolygon40::MAX_POINTS]

UAvoidNoVis	noVis [NO_VIS_SEG_CNT]

int	noVisCnt

UObstacle *	obst

Static Public Attributes
static const int	NO_VIS_SEG_CNT = 30

Detailed Description

Encapsulation of an obstacle with links that is used to obstacle groups where passage between the obstacles in the group is not possible.

Constructor & Destructor Documentation

UAvoidObst::UAvoidObst ( )

inline

Constructor

References addToGroupIfWithinMargin2(), clear(), and empty.

Member Function Documentation

bool UAvoidObst::addtangentLines	(	UAvoidObst *	other,
		UAvoidLink *	ala,
		UAvoidLink *	alb,
		UAvoidLink *	alc,
		UAvoidLink *	ald
	)

Get the (up to) 4 tangent lines between these two obstacles Assign the outher and cross tangens between these two obstacles. index : this other
0 : CV(L) CV(L) – cross tangent
1 : CCV(R) CV(L) – outher tangent
2 : CV(L) CCV(R) – outher tangent
3 : CCV(R) CCV(R). – cross tangent.
If obstacles har a slight overlap, then only two (outher - index 1 and 2) tangents are created. if obstacles cross each other then 4 outher tangents exist, in this case the additional set of links alc and ald are used to describe one of the sets. if second set of tangents (alc and ald) may be NULL.

Parameters

other	is the other obstacle.
ala	is describing the up to 4 link from this obstacle to the other
alb	is describing the up to 4 link from the other obstacle to this (is a mirror to ala, but has itself a mirror set to NULL).
alc	is describing up to 2 outher tangents if the obstacles cross each other (else 0)
ald	is describing up to 2 outher tangents if the obstacles cross each other (else 0) - is a mirror to alc.

Returns: true if second set of tangent descriptors are not needed. returns false if second set are needed. Returns also false if second set is needed but is set to NULL.

References UAvoidLink::aob, UAvoidLink::aobIdx, U2Dline::distanceSigned(), UPolygon::getPoints(), UPolygon::getPointsCnt(), UAvoidLink::idx, maxi(), UAvoidLink::mirror, nogoVertex, obst, U2Dline::set2P(), UAvoidLink::tangentCnt, UAvoidLink::tob, UAvoidLink::valid, UPosition::x, and UPosition::y.

Referenced by clear(), and UAvoidPath2::createTangentLinks().

bool UAvoidObst::addToGroupIfWithinMargin2	(	UAvoidObst *	other,
		double	margin,
		bool	noAdd
	)

Add an obstacle 'other' to the group if within a margin distance or closer to any of the obstacles in the group already. If an existing obstacle is embedded in this new obstacle, then the embedded obstacle is added to the 'embList', as the group (no-visibility-lines) need to be recalculated. If 'embList' is NULL, then nothing is added, and the new obstacle is just added if there is an overlap.

Parameters

other	is the obstacle under test.
noAdd	can be set if obstacle is already added to a group, but may be in another group too. If the new obstacle is fully embedded in another obstacle, then it is not added (but function returnes true).

Returns: true if added.

This program is free software; you can redistribute it and/or modify * it under the terms of the GNU Library General Public License as * published by the Free Software Foundation; either version 2 of the * License, or (at your option) any later version. *
This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. *
You should have received a copy of the GNU Library General Public * License along with this program; if not, write to the * Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.

References UAvoidNoVis::clear(), grp, grpIdx, UPolygon::isOverlappingXYconvex2(), NO_VIS_SEG_CNT, noVis, noVisCnt, obst, and UAvoidNoVis::setNoVisSegment().

Referenced by UAvoidObst().

void UAvoidObst::clear ( void )

inline

Clear this group

References addtangentLines(), crossingNonVisibilityLine(), embedEnd, embedStart, getClosestPoint(), getTangentVertexFrom(), grp, isExit, isInSameGroup(), isStart, links, noVisCnt, obst, passingNoGoEdge(), passingNoGoEdge2(), testVisibility(), and validTangent().

Referenced by UAvoidPath2::getEmptyAog(), and UAvoidObst().

bool UAvoidObst::crossingNonVisibilityLine	(	ULineSegment *	visLine,
		FILE *	logdbg
	)

Is this line segment crossing any of the no-visibility lines in this obstacle group.

Parameters

visLine	the visibility line to test for crossing of non-visibility lines
logdbg	handle to a logfile, if further debug data is to be logged. true if segment is crossing a no-visibility line.

References UAvoidNoVis::getNoVisSegment(), ULine::getPositionOnLine(), UObstacle::getSerial(), ULine::getXYCrossing(), grp, ULineSegment::length, noVis, noVisCnt, obst, ULine::pos, ULine::vec, UPosition::x, and UPosition::y.

Referenced by clear(), and UAvoidPath2::testNonVisibilityLineCross().

double UAvoidObst::getClosestPoint	(	UPosition	to,
		double	distLimit,
		UPosition *	closest,
		bool *	atVertex
	)

Get closest point on on polygon to this point - either a vertex or a position on a side.

Parameters

to	is the position to evaluate (x,y is used only)
distLimit	a distance closer than this is to be returned only.
closest	is where the position on the polygon is returned (if valid within dist limit)
atVertex	is set to true if the closest point (within relevant distance) is a vertex. the distance to the polygon if it is within the max relevant distance 'maxRelevantDist' and the actual closest position in '*closest'.

Returns: a distance above maxRelevantDist when nothing within distLimit and then no change to '*closest'.

References UPolygon::getClosestDistance(), getClosestPoint(), grp, mind(), obst, UPosition::x, and UPosition::y.

Referenced by clear(), getClosestPoint(), and UAvoidPath2::setClosestPoint().

int UAvoidObst::getTangentVertexFrom	(	UPosition	pos,
		int *	idxCv,
		int *	idxCcv
	)

Test if a tangent entry at index 'vertexIn' and exit at 'vertexOut' uses edges that is marked hidden by other obstacles in the 'noVis' list. When the polygon is traversed in CV or CCV direction as indicated by the 'cv' parameter. Truerns true if at least one hidden edge is discovered. Get tangent points to this obstacle seen from the position 'pos'. The function returns the number of tangent points found, i.e. 0 if 'pos' is inside the obstacle or 2 if 'pos' is outside (the x,y part of the position and polygon is used only). The index to the tangent vertices are returned in 'idxCv' and 'idxCcv'. The idxCv is the left tangent when seen from 'pos'.

References U2Dlined::distanceSigned(), UPolygon::getPoints(), UPolygon::getPointsCnt(), obst, U2Dlined::set2P(), UPosition::x, and UPosition::y.

Referenced by clear().

bool UAvoidObst::isInSameGroup ( UAvoidObst * other )

Is the other obstacle in the same group as this group. Searches from this obstacle onwards in group, i.e. if full group is to be searched, then start with first obstacle in group.

Parameters

other is the other obstacle.

Returns: true if same group.

References grp, and isInSameGroup().

Referenced by clear(), and isInSameGroup().

bool UAvoidObst::passingNoGoEdge	(	bool	entryCV,
		int	entryVertex,
		int	exitVertex
	)

Has this obstacle a nogo edge between these two vertex numbers?. If the vertex numbers are equal, then returnes true. If entryCV is used to determine the vertex sequence.

References UPolygon::getPointsCnt(), nogoEdge, and obst.

Referenced by clear(), and validTangent().

bool UAvoidObst::passingNoGoEdge2	(	bool	entryCV,
		int	entryVertex,
		int	exitVertex,
		UPosition	pPrev,
		UPosition	pNext,
		bool	isStart,
		bool	isExit
	)

Is the line from previous position to entry-vertex and continuing from exit-ertex to next position passing nogo lines, i.e. passing through this obstacle group.

Parameters

entryCV	entry is touching this obstacle in a CV direction.
entryVertex	is the vertex number where the entry hits the obstacle.
exit	vertex is the vertex where the exit tangen touches the obstacle. in between there may be a number of obstacle edges touched on the go.
pPrev	is the start position.
pNext	is the destination after the obstacle.
isStart	is true if the from position is the current robot position.
isExit	is true if the next position is the target.

Returns: true if the route passes the internal of the obstacle group.

References a, UAvoidNoVis::aobOther, UPolygon::getCogXY(), UPolygon::getPoint(), UPolygon::getPointsCnt(), noVis, noVisCnt, obst, UPosition::x, and UPosition::y.

Referenced by UAvoidPath2::addLnksToOpenSet(), clear(), and UAvoidPath2::findRouteToDest().

void UAvoidObst::testVisibility ( FILE * logdbg )

Test established tangent lines for visibility.

Parameters

logdbg handle to a debug file (NULL if not debugging) All link objects to other obstacles are tested against all other obstacles. If a tangent line is found to be crossing another obstacle, the line is declared invisible.

References UAvoidLink::aob, bool2str(), embedEnd, embedStart, UObstacle::getSerial(), grpIdx, isExit, isStart, links, UAvoidLink::mirror, UAvoidLink::next, obst, UAvoidLink::valid, and UAvoidLink::visibilityTest().

Referenced by clear(), and UAvoidPath2::testVisibility().

bool UAvoidObst::validTangent	(	bool	entryCV,
		int	atVertex,
		UPosition	p1
	)

Test if this tangent has a valid connection that do not pass a nogoEdge. A nogoEdge is marked in the nogoEdge array during obstacle grouping function. If entry and exit vertex is identical, a further test is performed to see if the two tangents cross a no-visibility line - 'p1' is used in this test. Returns true if a connection is possible.

References UPolygon::getPointsCnt(), UAvoidLink::idx, links, UAvoidLink::next, nogoVertex, obst, passingNoGoEdge(), and UAvoidLink::valid.

Referenced by clear(), and UAvoidPath2::removeTangentIfNoExitTangent().

Member Data Documentation

int UAvoidObst::badEdges[UPolygon40::MAX_POINTS]

List of edges and vertices that are unusable in a path graph. Bad edge 3 is between vertex 3 and 4. If vetex count is 4, then the edge is between vertex 3 and 0. A bad edge is marked with bit 0 (i.e. value is 1 or 3). A bad vertex is arked with bit 1 (i.e. value 2 or 3 - value 2 is not used, if vertex is invalid, then edge is too.

Referenced by UAvoidPath2::addNoVisLinesAsObstacles(), UAvoidPath2::addStartAndExit(), UAvoidPath2::groupObstacles(), UAvoidPath2::invalidateConcavities(), UAvoidPath2::logObstacleGroups(), and UAvoidPath2::removeTangentInConcavities().

bool UAvoidObst::embedEnd

This obstacle embeds the exit position

Referenced by clear(), UAvoidPath2::invalidateObstaclesAtStartAndExit(), and testVisibility().

bool UAvoidObst::embedStart

This obstacle embeds the start position

Referenced by clear(), UAvoidPath2::invalidateObstaclesAtStartAndExit(), and testVisibility().

UAvoidObst* UAvoidObst::empty

Pointer to the next obstacle group in the empty (free) list

Referenced by UAvoidPath2::getEmptyAog(), UAvoidObst(), and UAvoidPath2::~UAvoidPath2().

UAvoidObst* UAvoidObst::grp

int UAvoidObst::grpIdx

group number of this obstacle

Referenced by UAvoidPath2::addNoVisLinesAsObstacles(), UAvoidPath2::addStartAndExit(), addToGroupIfWithinMargin2(), UAvoidPath2::avoidToTheLeft(), UAvoidPath2::createTangentLinks(), UAvoidCellGraph::fillVerticeList(), UAvoidPath2::groupObstacles(), UAvoidPath2::removeTangentInConcavities(), UAvoidPath2::setClosestPoint(), UAvoidPath2::splitObstGroup(), testVisibility(), UAvoidPath2::testVisibility(), and UAvoidPath2::toReplaceOldPoint().

bool UAvoidObst::isExit

This obstacle is the exit position

Referenced by UAvoidPath2::addLnksToOpenSet(), UAvoidPath2::addStartAndExit(), clear(), UAvoidPath2::createAvoidPoinsPath(), UAvoidPath2::createCellBasedPointList(), UAvoidPath2::findRouteToDest(), UAvoidPath2::removeTangentIfNoExitTangent(), UAvoidPath2::testNoVisSegVisibility(), and testVisibility().

bool UAvoidObst::isStart

This obstacle is the start position

Referenced by UAvoidPath2::addLnksToOpenSet(), UAvoidPath2::addStartAndExit(), clear(), UAvoidPath2::createAvoidPoinsPath(), UAvoidPath2::createCellBasedPointList(), UAvoidPath2::createMidPathPoints(), UAvoidPath2::findRouteToDest(), UAvoidPath2::removeTangentIfNoExitTangent(), UAvoidPath2::testNoVisSegVisibility(), and testVisibility().

UAvoidLink* UAvoidObst::links

Vertex connection list for this obstacle to all other obstacles

Referenced by UAvoidPath2::addLnksToOpenSet(), clear(), UAvoidPath2::countValidVisibilityLines(), UAvoidPath2::createTangentLinks(), UAvoidPath2::findRoutes(), UAvoidPath2::findRoutesA(), UAvoidPath2::findRouteToDest(), UAvoidPath2::logObstacleGroups(), UAvoidPath2::removeTangentIfNoExitTangent(), UAvoidPath2::removeTangentInConcavities(), UFunctionAvoid::sendCurrentAvoidPath(), UAvoidPath2::testNoVisSegVisibility(), testVisibility(), and validTangent().

const int UAvoidObst::NO_VIS_SEG_CNT = 30

static

Maximum number of no-visibility segments

Referenced by addToGroupIfWithinMargin2().

bool UAvoidObst::nogoEdge[UPolygon40::MAX_POINTS]

Hidden edges and vertices. A hidden vertex is marked with bit 1 and markes that the adjacent edges are not usable in a path either. Bit 2 markes that the edge (between this and the next vertex) is hidden (not to pass) only.

Referenced by UAvoidPath2::addNoVisLinesAsObstacles(), UAvoidPath2::addStartAndExit(), UAvoidPath2::groupObstacles(), UAvoidPath2::logObstacleGroups(), passingNoGoEdge(), and UAvoidNoVis::setNoVisSegment().