#include <uavoidpath2.h>

Public Member Functions
void	clear ()

void	findObstAvoidRoute (bool debugDump)

UAvoidObst *	getAvoidObstGrp (int idx)

int	getAvoidObstGrpCnt ()

UAvoidLnkSeq *	getLnkSeq (int idx)

int	getLnkSeqsCnt ()

UManSeq *	getManSeq ()

double	getManTime ()

bool	getPathUsed ()

UAvoidPoint *	getPointList ()

int	getSerial ()

bool	isACrash ()

bool	isValid ()

void	setPar (UAvoidParams *parameters)

void	setPathUsed (bool value)

void	setValid (bool value)

	UAvoidPath2 ()

	~UAvoidPath2 ()

Public Attributes
UAvoidObst *	aogs [MAX_AOG_OBST_GRPS]

int	aogsCnt

UAvoidCellGraph *	avCellGraph

int	oldFootCnt

UPolygon *	polys [MAX_FOODPRINT_POLYS]

int	polysCnt

int	poolIdx

Static Public Attributes
static const int	MAX_AOG_OBST_GRPS = 60

static const int	MAX_FOODPRINT_POLYS = 200

Protected Member Functions
void	addStartAndExit ()

void	addToLnkSeqs (UAvoidLnkSeq *ls)

bool	convertToManSeq (UAvoidPoint ptLst, UManSeq manSeq, double minTurnRadius, UAvoidVertexCosts *costAdd)

UAvoidLnkSeq *	copyAvoidLnkSeq (UAvoidLnkSeq *source)

int	countValidVisibilityLines ()

int	countValidVisibilityLines (UAvoidLink *startLnk)

UAvoidPoint *	createAvoidPoinsPath (UAvoidLnkSeq *source)

bool	createMidPathPoints (UAvoidPoint ptLstBase, bool ommitBase, bool *movedBase)

UAvoidPoint *	createPointList (UAvoidLnkSeq *source)

void	createTangentLinks (double margin)

bool	crossTangent (UPose c1, UPose c2, bool cv, double manRad, UPose *tanpt)

bool	findRouteInALoop (UAvoidLnkSeq lsFirst, UAvoidObst obstTest, int edgeIndex1, int edgeIndex2)

void	findRoutes ()

void	findRoutesA ()

void	findRouteToDest (UAvoidLnkSeq lsFirst, UAvoidLnkSeq lsEnd, int n)

bool	getClosestObst2 (UManPPSeq manSeq, U2Dseg visLine, bool tight, UPosition oPos, double dist, UAvoidObst *oObst, int mHit, bool *avoidLeft, bool firstIsCurrentPose, bool nextIsDestination)

UAvoidLink *	getEmptyALnk (int serial)

UAvoidObst *	getEmptyAog ()

UAvoidLnkSeq *	getEmptyLnkSeq ()

UAvoidPoint *	getEmptyPoint ()

void	groupObstacles (double margin)

void	invalidateConcavities (UAvoidLink la, UAvoidLink lb)

void	invalidateEmbeddedVertices ()

void	invalidateObstaclesAtStartAndExit ()

void	logObstacleGroups ()

void	logPathSequences ()

bool	outherTangent (UPose c1, UPose c2, bool cv, double manRad, UPose *tanpt)

bool	pointTangent (UPose dest, UPose c2, bool cv, double c2rad, UPose *tanpt)

void	recycleLnkSeq (UAvoidLnkSeq *ls)

void	recyclePoint (UAvoidPoint *ap)

void	recyclePoints (UAvoidPoint *first)

void	removeEqualXvertices (UObstacle *obst)

void	removeTangentIfNoExitTangent ()

void	removeTangentInConcavities ()

bool	setMidPoint (UAvoidPoint ap1, double manRad, bool reversedAp2mCent=NULL)

bool	setTurnCentre (UAvoidPoint ap1, double turnCentreRadius, double availableOuther)

bool	testNonVisibilityLineCross (ULineSegment *visLine)

void	testNoVisSegVisibility (UAvoidObst aogb, int n, int *m)

void	testVisibility ()

Protected Attributes
UAvoidLink *	aLnkLast

UAvoidLink *	aLnkRoot

UAvoidObst *	aogLast

UAvoidObst *	aogRoot

UObstacle *	aogsObst [MAX_LOCAL_OBSTS]

int	aogsObstCnt

UAvoidPoint *	aPointRoot

int	avoidSerial

int	closeCnt

UAvoidLnkSeq *	closedSet

UAvoidVertexCosts	costAdd
	additional path cost for path finding More...

bool	crash

bool	endsInObst

UPosition	endsInObstHere

int	generation

UAvoidLnkSeq *	lnkSeqLast

UAvoidLnkSeq *	lnkSeqRoot

UAvoidLnkSeq *	lnkSeqs [MAX_LINK_SEQS]

int	lnkSeqsCnt

int	lnkSerial
	next serial number for fetch of linkSequence. More...

FILE *	logdbg

UManSeq *	manFull

double	maxAngleCost

double	maxDistCost

UAvoidLnkSeq *	openSet

UAvoidParams *	par

bool	pathUsed

UAvoidPoint *	pointListFinal

int	serialNext
	next serial number for inserted avoid points More...

int	solutionCnt
	debug count of mid-point solutions More...

UObstacle *	startExitObst [2]

int	tanSeqCnt

bool	valid

Static Protected Attributes
static const int	MAX_LINK_SEQS = 30

static const int	MAX_LOCAL_OBSTS = 200

Private Member Functions
void	addLnksToOpenSet (UAvoidObst aog, UAvoidLnkSeq prev)

void	addNoVisLinesAsObstacles (UAvoidObst *aogs[], const int aogsCnt)

void	addToClosedSet (UAvoidLnkSeq *ls)

void	addToOpenSet (UAvoidLnkSeq *ls)

bool	avoidToTheLeft (UPosition oPos, UAvoidObst oObs, bool postObst, UAvoidPoint ap1)

UAvoidPoint *	createCellBasedPointList (UAvoidCellVertex *cvs, const int cvsCnt)

bool	expandCellPntsToManSeq (UAvoidCellVertex *cvs, const int cvsCnt)

bool	expandOmegaTurn (UPose c1, UPose c2, double turnRad, double angle, UPose *mCent)

void	expandPolyToSegment (U2Dseg seg, UPolygon polyInc, UPolygon *polyExcl, bool manIsLeft, bool turningLeft)

bool	expandVisLinesToManSeq (UAvoidLnkSeq *ls)

UManPPSeq *	extendManoeuvre (UManSeq manSeq, UAvoidPoint ap2, UAvoidPoint *ap1)

UPolygon *	getFoodprintPoly ()

void	getManAsPolygon (UManPPSeq manSeq, UPolygon p40, double endsMargin)

bool	insertNewPointAfter (UAvoidPoint ap1, UAvoidPoint apNew)

bool	isAfterDestination (UPosition oPos, UAvoidPoint *ap1)

bool	isCrossingManPoly (UPolygon obst, UPosition pos, double dist, int polyMan0, int manHit, double limit)

bool	isInClosedSet (UAvoidLnkSeq candidate, UAvoidLnkSeq prev, double *edgeDist)

double	isInsidePoly (UPosition pos, int polyMan0, int *manHit, double limit)

void	setClosestPoint (UAvoidPoint *ap, double dMin)

bool	setPreStopPoint (bool cvLeft, UPosition oPos, UAvoidPoint apNew, UAvoidPoint ap1)

void	splitObstGroup (int grpIdx, double x, double y, double h)

bool	terminateWorseCandidatesInOpenSet (UAvoidLnkSeq candidate, UAvoidLnkSeq prev)

bool	toReplaceOldPoint (UAvoidPoint apNew, UAvoidPoint apOld, UAvoidPoint *excludedList)

double	triangleFinda (double b, double c, double C, double *minor=NULL)

bool	turnCentreCrossing (UAvoidPoint arg1, UAvoidPoint arg2, double margin)

bool	validNewClosePoint (UPosition oPos, double dist, UAvoidPoint ap2, UAvoidPoint ap1)

bool	withinAngleLimits (double a, bool cvLeft, double h1, double h2, double angMarg)

Private Attributes
long unsigned int	maxSerial

Detailed Description

One of a number of paths with a route from current pose to a specifix exit pose

    @author Christian <chrand@mail.dk>

Constructor & Destructor Documentation

UAvoidPath2::UAvoidPath2 ( )

Constructor

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., * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *

References aLnkLast, aLnkRoot, aogLast, aogRoot, aogsCnt, aogsObst, aogsObstCnt, aPointRoot, avCellGraph, avoidSerial, crash, lnkSeqLast, lnkSeqRoot, lnkSeqsCnt, lnkSerial, logdbg, manFull, MAX_FOODPRINT_POLYS, MAX_LOCAL_OBSTS, oldFootCnt, par, pathUsed, pointListFinal, polys, polysCnt, poolIdx, UObstacle::setSerial(), solutionCnt, startExitObst, and valid.

UAvoidPath2::~UAvoidPath2 ( )

Destructor

References aLnkLast, aLnkRoot, aogLast, aogRoot, aogsObst, avCellGraph, UAvoidLink::empty, UAvoidObst::empty, lnkSeqLast, lnkSeqRoot, logdbg, manFull, MAX_FOODPRINT_POLYS, MAX_LOCAL_OBSTS, UAvoidLnkSeq::next, polys, and startExitObst.

Member Function Documentation

void UAvoidPath2::addLnksToOpenSet	(	UAvoidObst *	aog,
		UAvoidLnkSeq *	prev
	)

private

Add all links leving this obstacle and is compatible with the previous (arriving link)

Parameters

aog	is the current obstacle arrived to by 'prev'
prev	is the link by whish we arrived to the current obstacle.

References addToOpenSet(), UAvoidLink::aob, UAvoidLink::aobIdx, UAvoidLnkSeq::costAngle, UAvoidLnkSeq::costDist, UAvoidLnkSeq::costF, UAvoidLnkSeq::costG, UAvoidLnkSeq::costH, UAvoidParams::exitPose, UPose::getDistance(), getEmptyLnkSeq(), UAvoidParams::getMinTurnRad(), ULineSegment::getOtherEnd(), UPolygon::getPoint(), UPolygon::getPointsCnt(), UAvoidLink::getTangentLine(), ULine::getXYHeading(), UPose::h, UAvoidLink::idx, UAvoidObst::isExit, isInClosedSet(), UAvoidObst::isStart, ULineSegment::length, limitToPi(), UAvoidObst::links, UAvoidLink::mirror, UAvoidLink::next, UAvoidLnkSeq::next, UAvoidObst::obst, par, UAvoidObst::passingNoGoEdge2(), UAvoidLnkSeq::prev, recycleLnkSeq(), UAvoidLnkSeq::tangIdx, UAvoidLnkSeq::tangLine, terminateWorseCandidatesInOpenSet(), UAvoidLink::tob, UAvoidLink::valid, and valid.

Referenced by findRoutesA().

void UAvoidPath2::addNoVisLinesAsObstacles	(	UAvoidObst *	aogs[],
		const int	aogsCnt
	)

private

Convert no-visibility lines to obstacles, and add the new 2-point obstacles to the obstacle groups.

Parameters

aogs	is an array of existing obstacle groups.
aogsCnt	is number of established groups

References UPolygon::add(), UAvoidNoVis::aobOther, UAvoidNoVis::aobThis, aogsCnt, aogsObst, aogsObstCnt, UAvoidObst::badEdges, UPolygon::clear(), UPolygon::copyTo(), UPolygon::extractConvexTo(), getEmptyAog(), UAvoidNoVis::getNoVisSegment(), ULineSegment::getOtherEnd(), UPolygon::getPoint(), UPolygon::getPointsCnt(), UObstacle::getPoseFirst(), ULine::getPositionOnLine(), UPolygon::getSegment(), UAvoidObst::grp, UAvoidObst::grpIdx, UPolygon::isOverlappingXYconvex2(), ULineSegment::length, MAX_AOG_OBST_GRPS, MAX_LOCAL_OBSTS, maxSerial, UAvoidObst::nogoEdge, UAvoidObst::nogoVertex, UAvoidObst::noVis, UAvoidObst::noVisCnt, UAvoidObst::obst, ULine::pos, UPolygon::setPoint(), UObstacle::setPoseFirst(), UObstacle::setSerial(), UObstacle::setValid(), ULine::vec, UPosition::x, and UPosition::y.

Referenced by findObstAvoidRoute().

void UAvoidPath2::addStartAndExit ( )

protected

Add two final obstacle groups for the start and end positions. NB! there must be space for these 2 extra obstacles in the aogs array.

References UPolygon::add(), aogs, aogsCnt, UAvoidObst::badEdges, UObstacle::clear(), UAvoidParams::exitPose, getEmptyAog(), UPoseV::getPose(), UAvoidObst::grpIdx, UObstacle::initPoseFirst(), UAvoidObst::isExit, UAvoidObst::isStart, UAvoidObst::nogoEdge, UAvoidObst::nogoVertex, UTime::Now(), UAvoidObst::obst, par, UPoseTime::setPt(), UObstacle::setValid(), startExitObst, UAvoidParams::startPose, UPose::x, and UPose::y.

Referenced by findObstAvoidRoute().

void UAvoidPath2::addToClosedSet ( UAvoidLnkSeq * ls )

private

Add this edge to the closed set of nodes

References closedSet, and UAvoidLnkSeq::next.

Referenced by findRoutesA().

void UAvoidPath2::addToLnkSeqs ( UAvoidLnkSeq * ls )

protected

Add this link sequence to the list of best tangent line sequences

References UAvoidLnkSeq::costAngle, UAvoidLnkSeq::costDist, dist, UAvoidParams::exitPose, UAvoidLnkSeq::getDistance(), UPose::h, lnkSeqs, lnkSeqsCnt, MAX_LINK_SEQS, maxAngleCost, maxDistCost, par, recycleLnkSeq(), and UAvoidParams::startPose.

Referenced by findRoutes(), and findRouteToDest().

void UAvoidPath2::addToOpenSet ( UAvoidLnkSeq * ls )

private

Add this link sequence entry to the open set of potential solutions. Is added to openSet in sorted order with the lowest F-cost (cost so far + cost to destination) first.

Parameters

ls	is the new possible solution candidate - with front position (probably not destination)

References UAvoidLnkSeq::costF, UAvoidLnkSeq::next, and openSet.

Referenced by addLnksToOpenSet(), and findRoutesA().

bool UAvoidPath2::avoidToTheLeft	(	UPosition	oPos,
		UAvoidObst *	oObs,
		bool	postObst,
		UAvoidPoint *	ap1
	)

private

Is this obstacle point to be avoided to the left.

Parameters

oPose	is the obstacle point position.
oObs	is the obstacle on which the obstacle position belongs.
postObst	is it after the destination pose.
ap1	the destination point.

Returns: true if the oPos point is to be avoided to the left

References UAvoidPoint::aob, UAvoidPoint::aPos, UAvoidPoint::avoidLeft, U2Dlined::distanceSigned(), UPose::getMapToPose(), UAvoidParams::getSafeDistance(), UAvoidObst::grpIdx, UPose::h, UAvoidPoint::mid, par, UAvoidPoint::prev, setClosestPoint(), U2Dseg::setFromPoints(), splitObstGroup(), UPosition::x, UPose::x, UPosition::y, and UPose::y.

Referenced by convertToManSeq().

void UAvoidPath2::clear ( void )

Clear path to a startup state

References closedSet, crash, logdbg, manFull, openSet, pathUsed, UManSeq::releaseAllMan(), and valid.

Referenced by convertToManSeq(), and UAvoidPathPool::findPathToHere2().

bool UAvoidPath2::convertToManSeq	(	UAvoidPoint *	ptLst,
		UManSeq *	manSeq,
		double	minTurnRadius,
		UAvoidVertexCosts *	costAdd
	)

protected

Convert a sequence of points from current position to destination passing a number 0 to N to a manoeuver sequence.

Parameters

ptLst	is the list of points (UAvoidPnt) - first is current pose, last is destination
manSeq	is the manoeuvre sequence for the result
minTurnRadius	is the allowed minimum turn radius for manoeuvre planning (not driveon)
costAdd	is list of path points to be punished in next search if manoeuvre is not possible.

Returns: true if route is valid.

References UAvoidParams::acceptAfterSolution, UAvoidPoint::aPos, UAvoidPoint::avoidLeft, avoidToTheLeft(), clear(), closeCnt, costAdd, createMidPathPoints(), dist, UPosition::dist(), extendManoeuvre(), UAvoidParams::followLineLastPose, UAvoidParams::frontLeft, UAvoidParams::frontRight, UAvoidPoint::generation, generation, getClosestObst2(), UPose::getDistance(), getEmptyPoint(), UManSeq::getEndPoseV(), UAvoidParams::getMinTurnRad(), UManSeq::getP2PCnt(), UAvoidParams::getSafeDistance(), UPose::h, UAvoidParams::ignoreCloseObstAfter, UAvoidPoint::insertAfter(), isAfterDestination(), UAvoidPoint::isTight(), limitToPi(), UAvoidPoint::mCent, UAvoidPoint::mid, UAvoidPoint::next, UAvoidParams::obstClearanceDesired, UAvoidParams::obstClearanceMinimum, oldFootCnt, par, polys, polysCnt, UAvoidPoint::prePoint, UAvoidPoint::prev, recyclePoint(), recyclePoints(), UManSeq::releaseLast(), UAvoidPoint::serial, setClosestPoint(), UAvoidVertexCosts::setExtraCost(), U2Dseg::setFromPoints(), setPreStopPoint(), solutionCnt, toReplaceOldPoint(), UManSeq::truncate(), UManPPSeq::truncate(), UAvoidPoint::unlinkNext(), UAvoidPoint::useTight, validNewClosePoint(), UPosition::x, UPose::x, UPosition::y, and UPose::y.

Referenced by expandVisLinesToManSeq(), and findObstAvoidRoute().

UAvoidLnkSeq * UAvoidPath2::copyAvoidLnkSeq ( UAvoidLnkSeq * source )

protected

Copy a sequence of tangent lines with root in 'source' in newly allocated UAvoidLnkSeq objects

References getEmptyLnkSeq(), and UAvoidLnkSeq::next.

Referenced by findRouteToDest().

int UAvoidPath2::countValidVisibilityLines ( )

protected

Count the number of valid tangent lines defined for all obstacles for all obstacle groups, including start and end point.

References aogs, aogsCnt, UAvoidObst::grp, and UAvoidObst::links.

Referenced by findObstAvoidRoute(), and findRoutes().

int UAvoidPath2::countValidVisibilityLines ( UAvoidLink * startLnk )

protected

Count valid visibility lines in this series of link lines. Each link to other obstacles can have up to 4 links - two cross ond two outher.

Parameters

startLnk first link in the series.

Returns: count of valid lines - up to 4 for each link.

References UAvoidLink::mirror, UAvoidLink::next, UAvoidLink::tangentCnt, and UAvoidLink::valid.

UAvoidPoint * UAvoidPath2::createAvoidPoinsPath ( UAvoidLnkSeq * source )

protected

Create sequence of points to avoid, The points are the sequence of vertex points along the visibility path. The link sequence is tanget lines only, so point list is supplemented with points along polygon obstacles.

Parameters

source is the sequence of tangent lines (links) between obstacles.

Returns: the point list in aPointList a pointer to the created list of points to be avoided.

References UAvoidPoint::aob, UAvoidPoint::avoidLeft, createPointList(), UAvoidPoint::generation, generation, UAvoidParams::getSafeDistance(), UAvoidObst::isExit, UAvoidObst::isStart, UAvoidPoint::next, par, and setClosestPoint().

Referenced by expandVisLinesToManSeq().

UAvoidPoint * UAvoidPath2::createCellBasedPointList	(	UAvoidCellVertex *	cvs,
		const int	cvsCnt
	)

private

Convert cell points to an avoidance point list. cvs is the cell vertex points found in the cell graph. cvsCnt is the number of cells. The start and end points are not included.

References UAvoidPoint::aob, aogs, aogsCnt, UAvoidPoint::aPos, UAvoidPoint::avoidLeft, UAvoidCellVertex::avoidLeft, bool2str(), UAvoidParams::exitPose, UAvoidPoint::generation, generation, getEmptyPoint(), UAvoidParams::getSafeDistance(), UAvoidPoint::insertAfter(), UAvoidObst::isExit, UAvoidObst::isStart, UAvoidPoint::next, UAvoidCellVertex::obst, par, UAvoidCellVertex::pos, UPosition::set(), setClosestPoint(), UAvoidParams::startPose, UPosition::x, UPose::x, UPosition::y, and UPose::y.

Referenced by findObstAvoidRoute().

bool UAvoidPath2::createMidPathPoints	(	UAvoidPoint *	ptLstBase,
		bool *	ommitBase,
		bool *	movedBase
	)

protected

Convert a sequence of points from current position to destination passing a number 0 to N to a manoeuver sequence.

Parameters

ptLstBase	is the list of points (UAvoidPoint) - first is current pose, last is destination
ommitBase	is the point ptLstBase surplus, then this flag is set true, and function returns false.
movedBase	is set true if mid-path point for the base avoid-point is moved. This may require the caller to redoo some previous calculations.

Returns: true if route is valid. false if no solution, or if ap2 is to be ommitted.

References UAvoidParams::acceptAfterSolution, UPose::add(), UAvoidPoint::aob, UAvoidPoint::aPos, UAvoidPoint::avoidLeft, crossTangent(), UPosition::dist(), expandOmegaTurn(), UAvoidPoint::followLineLastPose, UAvoidParams::frontLeft, UAvoidParams::frontRight, UAvoidPoint::generation, generation, U2Dseg::getCircleCrossings(), UPose::getDistance(), getEmptyPoint(), UPose::getMapToPose(), UPose::getMapToPosePose(), UAvoidParams::getMinTurnRad(), UPose::getPoseToMap(), UPose::getPoseToMapPose(), U2Dseg::getPositionOnLine(), UAvoidParams::getSafeDistance(), UPose::h, insertNewPointAfter(), UAvoidPoint::isFreeSpace(), UAvoidObst::isStart, UAvoidPoint::isTight(), limitToPi(), UAvoidPoint::mCent, UAvoidPoint::mid, UAvoidPoint::next, UAvoidParams::obstClearanceMinimum, UAvoidPoint::oob, UAvoidPoint::oPos, UAvoidPoint::otDist, outherTangent(), par, UAvoidPoint::prePoint, UAvoidPoint::prev, UAvoidPoint::printPoses(), recyclePoint(), UAvoidPoint::serial, UAvoidPoint::setAngNext(), setClosestPoint(), U2Dseg::setFromPose(), setMidPoint(), setTurnCentre(), UAvoidPoint::setTurnCentreFromMidPose(), solutionCnt, UAvoidParams::startPose, UAvoidPoint::tPointVisible(), UAvoidPoint::tPos, triangleFinda(), turnCentreCrossing(), UAvoidPoint::unlinkNext(), UAvoidPoint::useTight, U2Dpos::x, UPosition::x, UPose::x, U2Dpos::y, UPosition::y, and UPose::y.

Referenced by convertToManSeq().

UAvoidPoint * UAvoidPath2::createPointList ( UAvoidLnkSeq * source )

protected

Returns a point list from link sequence. The list holds all endpoints of all links, including start and endpoint, and all polygon points between tangent lines. This is the basis for path planning.

References UAvoidPoint::angNext, UAvoidPoint::aob, UAvoidLink::aob, UAvoidLink::aobIdx, UAvoidPoint::aPos, UAvoidPoint::avoidLeft, UAvoidParams::exitPose, UAvoidPoint::followLineLastPose, UAvoidParams::followLineLastPose, getEmptyPoint(), UPolygon::getPointsCnt(), UAvoidLink::getThisEnd(), UPose::h, UAvoidLink::idx, UAvoidPoint::insertAfter(), limitToPi(), UAvoidLnkSeq::next, UAvoidObst::obst, par, UAvoidPoint::prev, UPosition::print(), UAvoidPoint::setAvoidObst(), UAvoidLnkSeq::tangIdx, UAvoidLnkSeq::tangLine, and UAvoidLink::tob.

Referenced by createAvoidPoinsPath().

void UAvoidPath2::createTangentLinks ( double margin )

protected

Create 4 tangent lines between all pairs of obstacles, and set verteces and edges that is not usable for visibility tangent lines when creating a path. They are not usable if they at any point are closer than margin to another obstacle in the same obstale group.

References UAvoidObst::addtangentLines(), aogs, aogsCnt, getEmptyALnk(), UAvoidObst::grp, UAvoidObst::grpIdx, invalidateConcavities(), UAvoidObst::links, UAvoidLink::next, UAvoidLink::setBadEdgeAndVertex(), and UAvoidLink::tangentCnt.

Referenced by findObstAvoidRoute().

bool UAvoidPath2::crossTangent	(	UPose	c1,
		UPose	c2,
		bool	cv,
		double	manRad,
		UPose *	tanpt
	)

protected

Get tangent point on c1 circle for the cross tangent between these two circles. where the tangent from c2 to c1 touches the c1 circle in a clockwise direction (if cv is true).

Parameters

c1	is the centre of the main circle.
c2	is the centre of the 'from' circle
cv	is the tangent direction relative to circle 1
manRad	is the circle radius (both circles)
tanpt	is a pointer to a pose where the tangent point should be returned.

Returns: true if a cross tangent is possible (circles are not touching)

References UPose::add(), UPose::h, limitToPi(), UPose::x, and UPose::y.

Referenced by createMidPathPoints(), and setMidPoint().

bool UAvoidPath2::expandCellPntsToManSeq	(	UAvoidCellVertex *	cvs,
		const int	cvsCnt
	)

private

Convert the vertex list produced by the cell based obstacle avoidance path finder to the avoid-point list needed by the manoeuvre expansion function. And convert to a man-sequence.

Parameters

cvs	is the cell based vertex list elements - only those with obstacle reference are valid.
cvsCnt	is the total number of elements in the cvs array.

Returns: true if a valid manoeuvre sequence is formed.

bool UAvoidPath2::expandOmegaTurn	(	UPose	c1,
		UPose	c2,
		double	turnRad,
		double	angle,
		UPose *	mCent
	)

private

Expand the pre-point away from the tight entrance, so that the manoeuvre leading to the pre-point is possible, i.e. the turn circles do not touch. Assumes both points have same avoidance direction.

Parameters

c1	is the current position of the turn centre that has to move
c2	is the turn centre that needs to be at a manoeuverable distance
turnRad	is the used turn radius for the manoeuvres.
angle	is direction of straight safe path towards next narrow entrance. May be more alligned with opening - this should be safe too (makes it more omega-like).
mCent	is the new position of the turn centre.

Returns: true if possible.

References a, UPose::getDistance(), UPose::getPoseToMapPose(), UPose::h, limitToPi(), triangleFinda(), UPose::x, and UPose::y.

Referenced by createMidPathPoints(), and insertNewPointAfter().

void UAvoidPath2::expandPolyToSegment	(	U2Dseg *	seg,
		UPolygon *	polyInc,
		UPolygon *	polyExcl,
		bool	manIsLeft,
		bool	turningLeft
	)

private

Takes the footprint polygons and expand the inclusive polygon to the direct line between the end-points. The exclude polygon is removed if embedded between the inclusive polygon and the segment.

Parameters

seg	is the 2D line segment from start to end of manoeuvre.
polyInc	is the manoeuvere footprint (convex)
polyExcl	is the potential concavity area, thet shpuld be cut from the polyInc.
manIsLeft	is true if the manoeuvre is to the left of the direct line - determines which part of the exclude polygon to delete.
turningLeft	is the turn coveret left or right.

References UPolygon::add(), UPolygon::clear(), UPolygon::copyTo(), UPolygon::cut(), UPolygon::cutPoints(), U2Dlined::distanceSigned(), UPolygon::extractConvexTo(), UPolygon::getPoint(), UPolygon::getPointsCnt(), U2Dseg::getPositionOnLine(), U2Dseg::length, U2Dpos::x, UPosition::x, U2Dpos::y, and UPosition::y.

bool UAvoidPath2::expandVisLinesToManSeq ( UAvoidLnkSeq * ls )

private

Expand this set of visibility lines to amanoeuvre sequence. Uses class variable aPointList as an interim list of waypoints.

Parameters

ls	is the visiblity tangent line sequence

Returns: manoeuvre sequence in class variable 'man'

References convertToManSeq(), costAdd, createAvoidPoinsPath(), generation, manFull, UAvoidParams::minTurnRadius, par, pointListFinal, recyclePoints(), and serialNext.

Referenced by findObstAvoidRoute().

UManPPSeq * UAvoidPath2::extendManoeuvre	(	UManSeq *	manSeq,
		UAvoidPoint *	ap2,
		UAvoidPoint *	ap1
	)

private

Extend the manoeuvre from avoid point ap2 to avoid point ap1.

Parameters

manSeq	is the manoeuvre sequence to extend.
ap2	is the structure with the start pose.
ap1	is the structure with the end pose.

Returns: the adeed pose to pose manoeuvre sequence.

References UManPPSeq::add(), UManSeq::addMan(), UManSeq::addManDriveon(), UAvoidPoint::avoidLeft, UAvoidParams::exitPose, UManPPSeq::getEndPoseV(), UAvoidParams::getMinTurnRad(), UManSeq::getP2P(), UManSeq::getP2PCnt(), UPose::h, limitToPi(), UAvoidParams::maxAcceleration, UAvoidParams::maxTurnAcceleration, UAvoidPoint::mid, UAvoidPoint::next, par, UManPPSeq::setEndPoseV(), UManArc::setTurnAngle(), UManArc::setTurnRadius(), UAvoidParams::useDriveon, and UPoseV::vel.

Referenced by convertToManSeq().

void UAvoidPath2::findObstAvoidRoute ( bool debugDump )

Find route to the already loaded exit point, obstacles and road-lines. If there is a need to spawn alternative routes then use the provided pointer to the path-pool to fetch more empty paths.

Parameters

debugDump will trigger some data dump to logfile (if open).

References a, addNoVisLinesAsObstacles(), addStartAndExit(), aLnkLast, aLnkRoot, aogLast, aogRoot, aogs, aogsCnt, avCellGraph, UAvoidVertexCost::avoidLeft, UAvoidParams::avoidSerial, avoidSerial, bool2str(), UAvoidParams::cellRev2, closeCnt, convertToManSeq(), UAvoidVertexCost::cost, costAdd, UAvoidVertexCosts::costs, UAvoidVertexCosts::costsCnt, countValidVisibilityLines(), createCellBasedPointList(), createTangentLinks(), dist, UAvoidParams::driveonGA, UAvoidParams::driveonGD, UAvoidParams::exitPose, expandVisLinesToManSeq(), UAvoidCellGraph::findBestCellPath(), findRoutesA(), generation, UAvoidCellGraph::getCellsCnt(), UPose::getDistance(), UManSeq::getDistance(), UAvoidParams::getMinOpening(), UTime::getTimePassed(), UAvoidCellGraph::getVertexList(), UAvoidCellGraph::getVertexListCnt(), groupObstacles(), invalidateEmbeddedVertices(), invalidateObstaclesAtStartAndExit(), lnkSeqs, lnkSeqsCnt, logdbg, logObstacleGroups(), logPathSequences(), UAvoidCellGraph::makeCellDecomposition(), manFull, MAX_AOG_OBST_GRPS, UAvoidParams::maxTangentToMan, UAvoidParams::minTurnRadius, UTime::now(), par, pointListFinal, UAvoidVertexCost::pos, recyclePoints(), UManSeq::releaseAllMan(), removeTangentIfNoExitTangent(), serialNext, UPosition::set(), ULineSegment::setFromPoints(), UAvoidParams::startPose, testVisibility(), UAvoidParams::useAnyResult, UAvoidParams::useDriveon, valid, U2Dpos::x, UPose::x, U2Dpos::y, and UPose::y.

Referenced by UAvoidPathPool::findPathToHere2().

bool UAvoidPath2::findRouteInALoop	(	UAvoidLnkSeq *	lsFirst,
		UAvoidObst *	obstTest,
		int	edgeIndex1,
		int	edgeIndex2
	)

protected

Test the 'lsTest' tangent line. it may not end in an object already in the tangent line sequence if it would reuse any of the polygon index points already reserved. If edgeIndex1 and edgeIndex2 are different, then Alle the edge points from edgeIndex1 increasing to edgeIndex2 (both inclusive) are tested

References a, UAvoidLink::aob, UAvoidLink::aobIdx, UPolygon::getPointsCnt(), UAvoidLink::idx, UAvoidLnkSeq::next, UAvoidObst::obst, UAvoidLnkSeq::tangIdx, and UAvoidLnkSeq::tangLine.

Referenced by findRouteToDest().

void UAvoidPath2::findRoutes ( )

protected

Find routes in the set of obstacle link lines available from current position to target position. The target position is the last of the obstacle groups and the current position the one just before that. The path is searched in a depth first manner.

References a, addToLnkSeqs(), UAvoidLink::aob, aogs, aogsCnt, UAvoidLnkSeq::costAngle, UAvoidLnkSeq::costDist, countValidVisibilityLines(), findRouteToDest(), UAvoidParams::forwardOnly, getEmptyLnkSeq(), UPose::getMapToPose(), UAvoidLink::getOtherEnd(), UAvoidLink::getTangentLine(), ULine::getXYHeading(), UPose::h, ULineSegment::length, limitToPi(), UAvoidObst::links, lnkSeqs, lnkSeqsCnt, maxAngleCost, maxDistCost, UAvoidLink::next, UTime::now(), par, recycleLnkSeq(), UAvoidParams::startPose, UAvoidLink::tangentCnt, UAvoidLnkSeq::tangIdx, UAvoidLnkSeq::tangLine, tanSeqCnt, UAvoidLink::valid, UPosition::x, and UPosition::y.

void UAvoidPath2::findRoutesA ( )

protected

Find routes (best route) using an A* method.

References addLnksToOpenSet(), addToClosedSet(), addToOpenSet(), UAvoidLink::aob, aogs, aogsCnt, UAvoidLnkSeq::costAngle, UAvoidLnkSeq::costDist, UAvoidLnkSeq::costF, UAvoidLnkSeq::costG, UAvoidLnkSeq::costH, UAvoidParams::exitPose, UAvoidParams::forwardOnly, UPose::getDistance(), getEmptyLnkSeq(), UAvoidParams::getMinTurnRad(), ULineSegment::getOtherEnd(), UAvoidLink::getTangentLine(), ULine::getXYHeading(), UPose::h, ULineSegment::length, limitToPi(), UAvoidObst::links, lnkSeqs, lnkSeqsCnt, lnkSerial, maxAngleCost, maxDistCost, UAvoidLink::next, UAvoidLnkSeq::next, openSet, par, UAvoidLnkSeq::prev, recycleLnkSeq(), UAvoidParams::startPose, UAvoidLink::tangentCnt, UAvoidLnkSeq::tangIdx, UAvoidLnkSeq::tangLine, tanSeqCnt, and UAvoidLink::valid.

Referenced by findObstAvoidRoute().

void UAvoidPath2::findRouteToDest	(	UAvoidLnkSeq *	lsFirst,
		UAvoidLnkSeq *	lsEnd,
		int	n
	)

protected

Search the tangent lines from lsEnd to find an ordered tangent line sequence to advance from current position to the destination. No loops may occur with the already used tangents in the tangent line sequence from lsFirst. 'n' is the number of tangent lines already in the list. The function tests all tangent lines for one obstacle and is called recursively until the destination "obstacle" is found. Returns true if the destination is found and false if no destination is found (a blind end)

References addToLnkSeqs(), UAvoidLink::aob, UAvoidLink::aobIdx, aogs, aogsCnt, copyAvoidLnkSeq(), UAvoidLnkSeq::costAngle, UAvoidLnkSeq::costDist, findRouteInALoop(), getEmptyLnkSeq(), UPolygon::getPoint(), UPolygon::getPointsCnt(), UAvoidLink::getTangentLine(), ULine::getXYHeading(), UAvoidLink::idx, UAvoidObst::isExit, UAvoidObst::isStart, ULineSegment::length, limitToPi(), UAvoidObst::links, maxAngleCost, maxDistCost, UAvoidParams::maxTangentDepth, UAvoidLink::mirror, UAvoidLink::next, UAvoidLnkSeq::next, UAvoidObst::obst, par, UAvoidObst::passingNoGoEdge2(), UAvoidLnkSeq::pathCrossing(), recycleLnkSeq(), UAvoidLnkSeq::tangIdx, UAvoidLnkSeq::tangLine, tanSeqCnt, UAvoidLink::tob, UAvoidLink::valid, valid, UPosition::x, and UPosition::y.

Referenced by findRoutes().

UAvoidObst* UAvoidPath2::getAvoidObstGrp ( int idx )

inline

Get the obstacle groups with index idx

Referenced by UFunctionAvoid::sendCurrentAvoidPath().

int UAvoidPath2::getAvoidObstGrpCnt ( )

inline

Get number of obstacle groups

Referenced by UFunctionAvoid::sendCurrentAvoidPath().

bool UAvoidPath2::getClosestObst2	(	UManPPSeq *	manSeq,
		U2Dseg *	visLine,
		bool	tight,
		UPosition *	oPos,
		double *	dist,
		UAvoidObst **	oObst,
		int *	mHit,
		bool *	avoidLeft,
		bool	firstIsCurrentPose,
		bool	nextIsDestination
	)

protected

get most offending point for this manoeuvre, i.e. will the manoeuvering hit anything on the way.

Parameters

manSeq	is the manoeuvre sequence to test
visLine	is the visibility line we are trying to follow
tight	if true, then the tight values are used - rather than the desired clearence
pos	is the offending position
dist	is the distance from path
oObst	found offending obstacle
avoidLeft	is set to avoid route of found obstacle
firstIsCurrentPose	if true, then obstacles close to robot (current pose) is ignored as false detection or a self detection.
nextIsDestination	if true, then this is to destination, and should be tested regardless how small.

Returns: true if a point is found.; the point position in pos, the obstacle (index into aogs array) and the distance from the returned point to the path. get most offending point for this manoeuvre, i.e. will the manoeuvering hit anything on the way.

Parameters

manSeq	is the manoeuvre sequence to test
visLine	is the visibility line we are trying to follow
tight	if true, then the tight values are used - rather than the desired clearence
pos	is the offending position
dist	is the distance from path
oObst	found offending obstacle
avoidLeft	is set to avoid route of found obstacle
firstIsCurrentPose	if true, then obstacles close to robot (current pose) is ignored as false detection or a self detection.
nextIsDestination	if true, then this is to destination, and should be tested regardless how small.

Returns: true if a point is found.; the point position in pos, the obstacle (index into aogs array) and the distance from the returned point to the path.

there could be exceptions before or after the visibility line that is not handled.

References UPolygon::add(), aogs, aogsCnt, UPolygon::extractConvexTo(), UAvoidParams::frontLeft, UAvoidParams::frontRight, UPolygon::getClosestDistance(), UPolygon::getCogXY(), UAvoidParams::getDiagonal(), UManoeuvre::getDistance(), UPolygon::getDistance(), U2Dseg::getDistanceSigned(), UManPPSeq::getDistanceXYSigned(), UManoeuvre::getEndPoseV(), UManPPSeq::getEndPoseV(), U2Dseg::getFirstEnd(), getFoodprintPoly(), UManoeuvre::getFoodprintPolygon(), UManPPSeq::getMan(), UAvoidParams::getMinTurnRad(), U2Dseg::getOtherEnd(), UPolygon::getPoint(), UPolygon::getPointsCnt(), U2Dseg::getPositionOnLine(), UAvoidParams::getSafeDistance(), UManPPSeq::getSeqCnt(), UManPPSeq::getStartPoseV(), UAvoidObst::grp, isCrossingManPoly(), isInsidePoly(), UPolygon::isOverlappingXYconvex2(), U2Dseg::length, UAvoidObst::obst, UAvoidParams::obstClearanceDesired, UAvoidParams::obstClearanceMinimum, par, polysCnt, U2Dseg::setFromPoints(), UPosition::x, UPose::x, UPosition::y, and UPose::y.

Referenced by convertToManSeq().

UAvoidLink * UAvoidPath2::getEmptyALnk ( int serial )

protected

Get an empty obstacle link entry with 4 oblscle connection lines. Either from the empty list or a newly created (and added to the empty list)

References aLnkLast, aLnkRoot, UAvoidLink::clear(), UAvoidLink::empty, and UAvoidLink::serial.

Referenced by createTangentLinks().

UAvoidObst * UAvoidPath2::getEmptyAog ( )

protected

Get an empty obstacle group entry for one obstcle. Either from the empty list or a newly created (and added to the empty list)

References aogLast, aogRoot, UAvoidObst::clear(), and UAvoidObst::empty.

Referenced by addNoVisLinesAsObstacles(), addStartAndExit(), and groupObstacles().

UAvoidLnkSeq * UAvoidPath2::getEmptyLnkSeq ( )

protected

Get a new (empty) link sequence entry from the reuse pool.

References UAvoidLnkSeq::clear(), lnkSeqRoot, lnkSerial, UAvoidLnkSeq::next, and UAvoidLnkSeq::serial.

Referenced by addLnksToOpenSet(), copyAvoidLnkSeq(), findRoutes(), findRoutesA(), and findRouteToDest().

UAvoidPoint * UAvoidPath2::getEmptyPoint ( )

protected

Get an empti avoidance point from the recycle list. If recycle list is empty, then one is created on heap.

References aPointRoot, UAvoidPoint::clear(), UAvoidPoint::next, UAvoidPoint::serial, and serialNext.

Referenced by convertToManSeq(), createCellBasedPointList(), createMidPathPoints(), and createPointList().

UPolygon * UAvoidPath2::getFoodprintPoly ( )

private

Get a new polygon from the man-space polygon heap.

Returns: a pointer to a valid UPolygon40 - the next free. If no more space, the polygon heap is recycled (and a debug message issued)

References MAX_FOODPRINT_POLYS, polys, and polysCnt.

Referenced by getClosestObst2().

UAvoidLnkSeq* UAvoidPath2::getLnkSeq ( int idx )

inline

Get tangent sequence with this index number. NB! no range check.

Referenced by UFunctionAvoid::sendCurrentAvoidPath().

int UAvoidPath2::getLnkSeqsCnt ( )

inline

Get number of available tangent sequences

Referenced by UFunctionAvoid::sendCurrentAvoidPath().

void UAvoidPath2::getManAsPolygon	(	UManPPSeq *	manSeq,
		UPolygon *	p40,
		double	endsMargin
	)

private

Convert this manoeuvre to a polygon, where one side is the original to-from line.

Parameters

manSeq	the pose-to-pose sequence
p40	the destination sequence.
endsMargin	is a part near start and end that is to be ommitted.

Returns: nothing

References UPolygon::add(), UPolygon::clear(), UPolygon::extractConvexTo(), UManoeuvre::getDistance(), UManoeuvre::getEndPoseV(), UManPPSeq::getMan(), UManPPSeq::getManDist(), UManoeuvre::getManType(), UPose::getPoseToMap(), UManPPSeq::getSeqCnt(), UManPPSeq::getStartPoseV(), UManArc::getTurnAngle(), UManArc::getTurnRadius(), UManoeuvre::MAN_ARC, UManoeuvre::MAN_LINE, U2Dpos::x, UPose::x, U2Dpos::y, and UPose::y.

UManSeq* UAvoidPath2::getManSeq ( )

inline

Get manoeuver sequence for planned drive path

Referenced by UResAvoid::findPathToHere(), UAvoidPathPool::findPathToHere2(), UAvoidPathPool::getNewAvoidPath(), and UFunctionAvoid::sendCurrentAvoidPath().

double UAvoidPath2::getManTime ( )

Get the time to exit pose for this path

References UManSeq::getTime(), and manFull.

bool UAvoidPath2::getPathUsed ( )

inline

Is this path actually used for robot control

Referenced by UFunctionAvoid::sendCurrentAvoidPath().

UAvoidPoint* UAvoidPath2::getPointList ( )

inline

Get list of points to be avoided

References a, UAvoidMidPose::angle, and dist.

Referenced by UFunctionAvoid::sendCurrentAvoidPath().

int UAvoidPath2::getSerial ( )

inline

Get serial number for the obstacle calculation for this path. All paths generated in one calculateion has the same serial number.

void UAvoidPath2::groupObstacles ( double margin )

protected

Group obstacles based on this minimal passable margin between any two obstacles.

References aogs, aogsCnt, UAvoidObst::badEdges, bool2str(), UAvoidParams::cellRev2, UPolygon::getCogXY(), getEmptyAog(), UReacObstGrps::getGroup(), UReacObstGrps::getGroupsCnt(), UObstacleGroup::getObstacle(), UObstacleGroup::getObstsCnt(), UPolygon::getPointsCnt(), UObstacle::getSerial(), UObstacleGroup::getSerial(), UAvoidObst::grp, UAvoidObst::grpIdx, UObstacle::isValid(), logdbg, MAX_AOG_OBST_GRPS, maxSerial, UAvoidObst::nogoEdge, UAvoidObst::nogoVertex, UAvoidObst::obst, UAvoidParams::obsts, par, removeEqualXvertices(), UPosition::x, and UPosition::y.

Referenced by findObstAvoidRoute().

bool UAvoidPath2::insertNewPointAfter	(	UAvoidPoint *	ap1,
		UAvoidPoint *	apNew
	)

private

Insert this new (unlinked) point just before the ap1 point. The new point is validated against the previous point. The path mid-points are reevaluated, and if need be an omega manoeuvre expansion is attempted.

Parameters

ap1	is the current point.
apNew	is the new point to be inserted.

Returns: true if successfully inserted and validated.

References UAvoidPoint::angNext, UAvoidPoint::aPos, UAvoidPoint::avoidLeft, expandOmegaTurn(), UPose::getMapToPosePose(), UAvoidParams::getMinTurnRad(), U2Dseg::getPositionOnLine(), UAvoidParams::getSafeDistance(), UPose::h, UAvoidPoint::insertAfter(), UAvoidPoint::isFreeSpace(), U2Dseg::length, limitToPi(), UAvoidPoint::mCent, UAvoidPoint::mid, par, UAvoidPoint::prev, UAvoidPoint::serial, setClosestPoint(), U2Dseg::setFromPoints(), setMidPoint(), U2Dpos::x, UPosition::x, UPose::x, U2Dpos::y, UPosition::y, and UPose::y.

Referenced by createMidPathPoints().

void UAvoidPath2::invalidateConcavities	(	UAvoidLink *	la,
		UAvoidLink *	lb
	)

protected

Invalidate the vertices between two obstacles in the same group

References a, UAvoidObst::badEdges, UPolygon::getPointsCnt(), UAvoidLink::idx, UAvoidObst::obst, UAvoidLink::tob, and UAvoidLink::valid.

Referenced by createTangentLinks().

void UAvoidPath2::invalidateEmbeddedVertices ( )

protected

Mark vertices that is embedded inside other obstacles in the group as a nogo edge. As no tangent should be created from this vertex.

References aogs, aogsCnt, UPolygon::getClosestDistance(), UPolygon::getPoints(), UPolygon::getPointsCnt(), UAvoidObst::grp, UAvoidObst::nogoVertex, UAvoidObst::obst, UPosition::x, and UPosition::y.

Referenced by findObstAvoidRoute().

void UAvoidPath2::invalidateObstaclesAtStartAndExit ( )

protected

Test if start or exit pose is embedded in an obstacle, and if so invalidate these obstacles

References aogs, aogsCnt, UAvoidObst::embedEnd, UAvoidObst::embedStart, UAvoidParams::exitPose, UPose::getPos(), UAvoidObst::grp, UPolygon::isInsideConvex(), UAvoidObst::obst, par, UAvoidParams::startPose, UPosition::x, and UPosition::y.

Referenced by findObstAvoidRoute().

bool UAvoidPath2::isACrash ( )

inline

Is the route maintained for crash analysis

Referenced by UAvoidPathPool::findPathToHere2(), and UFunctionAvoid::sendCurrentAvoidPath().

bool UAvoidPath2::isAfterDestination	(	UPosition	oPos,
		UAvoidPoint *	ap1
	)

private

Is this obstacle position after the desination pose in ap1.mid.

Parameters

oPos	is the obstacle position.
ap1	is the destination point

Returns: true if the point is after the destination.

References UAvoidPoint::aPos, UPosition::dist(), UAvoidParams::getDiagonal(), UPose::getMapToPose(), U2Dseg::getPositionOnLine(), U2Dseg::length, UAvoidPoint::mid, par, UAvoidPoint::prev, U2Dseg::setFromPoints(), UPosition::x, and UPosition::y.

Referenced by convertToManSeq().

bool UAvoidPath2::isCrossingManPoly	(	UPolygon *	obst,
		UPosition *	pos,
		double *	dist,
		int	polyMan0,
		int *	manHit,
		double	limit
	)

private

Test if this (obstacle) polygon crosses one of the manoeuvre polygons, and that the most inside point is a valid point, i.e. not inside the corresponding exclusive polygon. It is assumed that the envalope polygons are created and available in the polys[] array.

Parameters

obst	is the polygon, that may cross one of the manoeuvre polygons.
pos	is the returned position inside the hit manoeuvre polygon (if crossing)
dist	is the distance to the polygon from 'pos' - is negative inside.
polyMan0	is the polygon number for manoeuvre number 0
manHit	is the first manoeuvre crossing the obstacle (if crossing)
limit	is the required distance inside the incl polygon to be declared 'found'.

Returns: true if valid crossing is found.

References UPosition::distXY(), UPolygon::getClosestDistance(), UPolygon::getDistance(), UPolygon::getPointsCnt(), UPolygon::isOverlappingXY(), polys, polysCnt, UPosition::x, and UPosition::y.

Referenced by getClosestObst2().

bool UAvoidPath2::isInClosedSet	(	UAvoidLnkSeq *	candidate,
		UAvoidLnkSeq *	prev,
		double *	edgeDist
	)

private

Is this link line's end point already in the closed set

Parameters

candidate	is the line to be tested.
prev	is the previous line.
edgeDist	returns if not found the cost from last tangent to candidate tangent along edge of this obstacle.

Returns: true if a passed vertex either on this obstacle, or arrived to on the next obstacle is in the closed set

References UAvoidLink::aob, UAvoidLink::aobIdx, closedSet, UPolygon::getPoint(), UPolygon::getPointsCnt(), UAvoidLink::idx, UAvoidLnkSeq::next, UAvoidObst::obst, UAvoidLnkSeq::tangIdx, UAvoidLnkSeq::tangLine, UAvoidLink::tob, UPosition::x, and UPosition::y.

Referenced by addLnksToOpenSet().

double UAvoidPath2::isInsidePoly	(	UPosition	pos,
		int	polyMan0,
		int *	manHit,
		double	limit
	)

private

Test if this point is inside the incl polygon for the manHist manoeuvre or the previous manoeuvre, but not inside the corresponding exclusive polygon. It is assumed that the envalope polygons are created and available in the polys[] array.

Parameters

pos	is the position to be sested.
polyMan0	is the polygon number for manoeuvre number 0 (first manoeuvre polygon to be examined)
manHit	is the (last) manoeuvre number to be tested
limit	is the required distance inside the incl polygon to be declared 'found'.

Returns: the distance inside the incl polygon if the position is at least limit inside the incl polygon, excluding the excl polygon.; limit if not inside.

References UPolygon::getDistance(), UPolygon::getPointsCnt(), maxi(), polys, UPosition::x, and UPosition::y.

Referenced by getClosestObst2().

bool UAvoidPath2::isValid ( )

inline

Is route valid

Referenced by UResAvoid::findPathToHere(), UAvoidPathPool::findPathToHere2(), UAvoidPathPool::getNewAvoidPath(), UAvoidPathPool::getValidAvoidPathsCnt(), and UFunctionAvoid::sendCurrentAvoidPath().

void UAvoidPath2::logObstacleGroups ( )

protected

Write obstacle avoidance status to logfile

References UAvoidNoVis::aobOther, aogs, aogsCnt, UAvoidObst::badEdges, UPolygon::getCogXY(), UAvoidNoVis::getNoVisSegment(), ULineSegment::getOtherEnd(), UPolygon::getPoint(), UPolygon::getPointsCnt(), UObstacle::getSerial(), UAvoidObst::grp, UAvoidObst::links, logdbg, UAvoidObst::nogoEdge, UAvoidObst::nogoVertex, UAvoidObst::noVis, UAvoidObst::noVisCnt, UAvoidObst::obst, ULine::pos, UPosition::x, and UPosition::y.

Referenced by findObstAvoidRoute().

void UAvoidPath2::logPathSequences ( )

protected

debug log of tangent sequuences in each of the found paths.

References UAvoidLink::aob, UAvoidLink::aobIdx, UAvoidLnkSeq::costAngle, UAvoidLnkSeq::costDist, UPolygon::getPoint(), UObstacle::getSerial(), UAvoidLink::idx, lnkSeqs, lnkSeqsCnt, logdbg, MAX_LINK_SEQS, UAvoidParams::maxTangentDepth, UAvoidLnkSeq::next, UAvoidObst::obst, par, UAvoidLink::serial, UAvoidLnkSeq::tangIdx, UAvoidLnkSeq::tangLine, UAvoidLink::tob, UPosition::x, and UPosition::y.

Referenced by findObstAvoidRoute().

bool UAvoidPath2::outherTangent	(	UPose	c1,
		UPose	c2,
		bool	cv,
		double	manRad,
		UPose *	tanpt
	)

protected

Get tangent point on c1. where the outher tangent from c2 to c1 touches the c1 circle in a clockwise direction (if cv is true).

Parameters

c1	is the centre of the main circle.
c2	is the centre of the 'from' circle
cv	is the tangent direction relative to circle 1
manRad	is the circle radius (both circles)
tanpt	is a pointer to a pose where the tangent point should be returned.

Returns: true if a tangent is possible (c1 and c2 are not equal)

References UPose::add(), UPose::h, limitToPi(), UPose::x, and UPose::y.

Referenced by createMidPathPoints(), and setMidPoint().

bool UAvoidPath2::pointTangent	(	UPose	dest,
		UPose	c2,
		bool	cv,
		double	c2rad,
		UPose *	tanpt
	)

protected

Get tangent point on c2. where the tangent in a clockwise direction (if cv is true) hits dest.

Parameters

dest	is the destination point.
c2	is the centre of the 'from' circle
cv	is the tangent direction from circle 2
c2Rad	is the circle radius
tanpt	is a pointer to a pose where the tangent point should be returned.

Returns: true if a destination is outside the circle (else tanpt is not changed)

References a, UPose::h, limitToPi(), UPose::x, and UPose::y.

void UAvoidPath2::recycleLnkSeq ( UAvoidLnkSeq * ls )

protected

Add this sequence of link sequence to the recycle list

References lnkSeqLast, lnkSeqRoot, UAvoidLnkSeq::next, and UAvoidLnkSeq::prev.

Referenced by addLnksToOpenSet(), addToLnkSeqs(), findRoutes(), findRoutesA(), findRouteToDest(), and terminateWorseCandidatesInOpenSet().

void UAvoidPath2::recyclePoint ( UAvoidPoint * ap )

protected

Return this avoidance point to the recycle list. Recycles this point only.

References aPointRoot, and UAvoidPoint::next.

Referenced by convertToManSeq(), and createMidPathPoints().

void UAvoidPath2::recyclePoints ( UAvoidPoint * first )

protected

Return this avoidance point and all connected points in the next-chain to the recycle list.

References aPointRoot, and UAvoidPoint::next.

Referenced by convertToManSeq(), expandVisLinesToManSeq(), and findObstAvoidRoute().

void UAvoidPath2::removeEqualXvertices ( UObstacle * obst )

protected

Check for vertex positions that are equal in x-value. This may lead to cell generation problems. If a problem is found, then 1mm is aded to the x-position, and the polygon is further tested and corrected if not convex.

Parameters

obst	is the obstacle polygon to test.

References compareXposition(), UPolygon::copyTo(), UPolygon::extractConvexTo(), UPolygon::getPoints(), UPolygon::getPointsCnt(), UPolygon::getPPoints(), UObstacle::setValid(), and UPosition::x.

Referenced by groupObstacles().

void UAvoidPath2::removeTangentIfNoExitTangent ( )

protected

Test if a tangent has an exit tangent to connect to without passing an edge that is too close to another obstacle (in the group)

References UAvoidLink::aob, UAvoidLink::aobIdx, aogs, aogsCnt, UPolygon::getPoint(), UAvoidObst::grp, UAvoidLink::idx, UAvoidObst::isExit, UAvoidObst::isStart, UAvoidObst::links, UAvoidLink::mirror, UAvoidLink::next, UAvoidObst::obst, UAvoidLink::tangentCnt, UAvoidLink::tob, UAvoidLink::valid, and UAvoidObst::validTangent().

Referenced by findObstAvoidRoute().

void UAvoidPath2::removeTangentInConcavities ( )

protected

Remove tangents between group members that start in a voncavity vertex

References UAvoidLink::aob, aogs, aogsCnt, UAvoidObst::badEdges, UAvoidObst::grp, UAvoidObst::grpIdx, UAvoidLink::idx, UAvoidObst::links, UAvoidLink::mirror, UAvoidLink::next, UAvoidLink::tangentCnt, and UAvoidLink::valid.

void UAvoidPath2::setClosestPoint	(	UAvoidPoint *	ap,
		double	dMin
	)

private

Set the closest neighboring obstacle point and the distance to it.

Parameters

ap	is the point on the path - an obstacle point - to be avoided The result is set into this class.
dMin	is the relevant distance to look for obstacles. Current obstacle is not evaluated.

References UAvoidPoint::aob, aogs, aogsCnt, UAvoidPoint::aPos, UAvoidPoint::avoidLeft, UPosition::dist(), UAvoidParams::frontLeft, UAvoidParams::frontRight, UAvoidObst::getClosestPoint(), ULineSegment::getDistanceXYSigned(), UPose::getPoseToMap(), ULine::getPositionOnLine(), UAvoidObst::grpIdx, ULineSegment::length, UAvoidPoint::next, UAvoidParams::obstClearanceMinimum, UAvoidPoint::oob, UAvoidPoint::oobDist, UAvoidPoint::oPos, UAvoidPoint::otDist, UAvoidPoint::oVertex, par, UAvoidPoint::prePoint, UAvoidPoint::prev, ULineSegment::setFromPoints(), UAvoidParams::startPose, UAvoidPoint::tPos, UAvoidPoint::tVertex, UAvoidPoint::useTight, UPosition::x, and UPosition::y.

Referenced by avoidToTheLeft(), convertToManSeq(), createAvoidPoinsPath(), createCellBasedPointList(), createMidPathPoints(), and insertNewPointAfter().

bool UAvoidPath2::setMidPoint	(	UAvoidPoint *	ap1,
		double	manRad,
		bool *	reversedAp2mCent = `NULL`
	)

protected

Set new mid-pose for this obstacle point. That is a destination pose from previous mid-point. Assumes mCent is set correctly.

Parameters

ap1	is the obstacle point, that needs new mid-pose.
manRad	turn radius at the ap1 point.
reversedAp2mCent	set to true if reversed turn centre on previous point.

Returns: false if new midpoint can not set - i.e. manoeuvre is impossible (with current rules).

References UAvoidPoint::avoidLeft, crossTangent(), UAvoidParams::exitPose, UAvoidPoint::followLineLastPose, UPose::getMapToPosePose(), UPose::h, UAvoidPoint::mCent, UAvoidPoint::mid, UAvoidPoint::next, outherTangent(), par, UAvoidPoint::prePoint, UAvoidPoint::prev, UPose::set(), UPose::x, and UPose::y.

Referenced by createMidPathPoints(), insertNewPointAfter(), and setPreStopPoint().

void UAvoidPath2::setPar ( UAvoidParams * parameters )

inline

Set the start pose and velocity

Referenced by UAvoidPathPool::findPathToHere2().

void UAvoidPath2::setPathUsed ( bool value )

inline

Is this path actually used for robot control

Referenced by UAvoidPathPool::findPathToHere2(), and UAvoidPathPool::getNewAvoidPath().

bool UAvoidPath2::setPreStopPoint	(	bool	cvLeft,
		UPosition	oPos,
		UAvoidPoint *	apNew,
		UAvoidPoint *	ap1
	)

private

Set new point as a pre-point and set it before the ap1-end point to stop before the obstacle.

References UAvoidPoint::aPos, UAvoidPoint::avoidLeft, UAvoidParams::frontLeft, UAvoidParams::frontRight, UAvoidPoint::generation, generation, UPose::getMapToPose(), UAvoidParams::getMinTurnRad(), UPose::getPoseToMap(), UPose::getPoseToMapPose(), UAvoidParams::getSafeDistance(), UPose::h, UAvoidPoint::mCent, UAvoidPoint::mid, UAvoidParams::obstClearanceDesired, par, UAvoidPoint::prePoint, UAvoidPoint::prev, setMidPoint(), UPosition::x, UPose::x, UPosition::y, and UPose::y.

Referenced by convertToManSeq().

bool UAvoidPath2::setTurnCentre	(	UAvoidPoint *	ap1,
		double	turnCentreRadius,
		double *	availableOuther
	)

protected

Set the turn centre to avoid the obstacle, the turn centre is planned so that the closest point in the manoeuvre is near the obstacle position, and any facing obstacle is avoided too.

Parameters

ap1	is the obstacle point, that needs new mid-pose.
turnCentreRadius	is the radius of turn arc circle,
availableOuther	is set to the available space for turning before hitting an opposing obstacle (including margin) - this may be used to plan an extra mid-point

Returns: false if too narrow for any turn radius, i.e. an extra mid-point is needed

References UAvoidPoint::angNext, UAvoidPoint::aPos, UAvoidPoint::avoidLeft, UPosition::dist(), UAvoidParams::getSafeDistance(), UAvoidPoint::isFreeSpace(), UAvoidPoint::isTight(), UAvoidPoint::mCent, UAvoidPoint::next, UAvoidPoint::oPos, UAvoidPoint::otDist, par, UAvoidPoint::prePoint, UAvoidPoint::prev, UPose::set(), UAvoidPoint::tPos, UPosition::x, and UPosition::y.

Referenced by createMidPathPoints().

void UAvoidPath2::setValid ( bool value )

inline

Set route valid flag

Referenced by UAvoidPathPool::findPathToHere2(), and UAvoidPathPool::getNewAvoidPath().

void UAvoidPath2::splitObstGroup	(	int	grpIdx,
		double	x,
		double	y,
		double	h
	)

private

Split an obstacle group - used to allow exit and entry of cavities in an obstracle group, e.g. enter a closed room or a dead end. Otherwise closest point could not be at the other side of the opening. Splits the group left and right of a splitting line from the provided position (x,y) and the COG of the obstacles in the group furthest away from this point. NB! leaves the non-visibility lines between obstacles in the splittet group, as these still are valid.

Parameters

grpIde	is the group to split.
x,y,h	is the splitting line.

References aogs, aogsCnt, U2Dlined::distanceSigned(), UPolygon::getCogXY(), UAvoidObst::grp, UAvoidObst::grpIdx, MAX_AOG_OBST_GRPS, UAvoidObst::obst, U2Dlined::setPH(), UPosition::x, and UPosition::y.

Referenced by avoidToTheLeft().

bool UAvoidPath2::terminateWorseCandidatesInOpenSet	(	UAvoidLnkSeq *	candidate,
		UAvoidLnkSeq *	prev
	)

private

Look for unfinished sets in the open-set list, that ends at the same vertex as this or ends on a vertex on candidate's home obstacle that is on the path from the previous tangent. When a unfinished entry is found in the open set it is deleted, if it has a worse score than the current.

Parameters

candidate	is the current candidate exit link.
prev	is the previous link, when arrived to candidates obstacle.

Returns: true if an unfinished entry has a better score than the candidate (then the candidate should be closed).

References UAvoidLink::aob, UAvoidLink::aobIdx, UAvoidLnkSeq::costG, UPolygon::getPoint(), UPolygon::getPointsCnt(), UAvoidLink::idx, UAvoidLnkSeq::next, UAvoidObst::obst, openSet, recycleLnkSeq(), UAvoidLnkSeq::tangIdx, UAvoidLnkSeq::tangLine, UAvoidLink::tob, UPosition::x, and UPosition::y.

Referenced by addLnksToOpenSet().

bool UAvoidPath2::testNonVisibilityLineCross ( ULineSegment * visLine )

protected

Test this line against all non-visibility lines for all groups. Returns true if the line crosses a non-visibility line.

References aogs, aogsCnt, UAvoidObst::crossingNonVisibilityLine(), and logdbg.

Referenced by testNoVisSegVisibility().

void UAvoidPath2::testNoVisSegVisibility	(	UAvoidObst *	aogb,
		int *	n,
		int *	m
	)

protected

Test the tangent lines for this obstacle against the non-visibility lines defined for all obstacle groups. (n is a debug parameter)

References UAvoidLink::aob, UAvoidLink::aobIdx, UPolygon::getPoint(), UPolygon::getPointsCnt(), UAvoidLink::idx, UAvoidObst::isExit, UAvoidObst::isStart, UAvoidObst::links, UAvoidLink::mirror, UAvoidLink::next, UAvoidObst::noVisCnt, UAvoidObst::obst, ULineSegment::setFromPoints(), UAvoidLink::tangentCnt, testNonVisibilityLineCross(), UAvoidLink::tob, UAvoidLink::valid, UPosition::x, UPosition::y, and UPosition::z.

Referenced by testVisibility().

void UAvoidPath2::testVisibility ( )

protected

Test tangent lines for visibility for all obstacles in all groups.

References aogs, aogsCnt, UAvoidObst::grp, UAvoidObst::grpIdx, logdbg, testNoVisSegVisibility(), and UAvoidObst::testVisibility().

Referenced by findObstAvoidRoute().

bool UAvoidPath2::toReplaceOldPoint	(	UAvoidPoint *	apNew,
		UAvoidPoint *	apOld,
		UAvoidPoint *	excludedList
	)

private

Test if this new point potentially could replace the old obstacle avoidance point

Parameters

apNew	the new just added point.
apOld	the point that may be replaced.
excludedList	is a list of points previously excluded. If the new point is already in this exclude list then it can not be replaced.

Returns: true if the apOld point can be replaced (potentially) by the new.

References UAvoidPoint::aob, UAvoidPoint::aPos, UPosition::dist(), UAvoidParams::getMinOpening(), UAvoidParams::getMinTurnRad(), UAvoidObst::grpIdx, UAvoidPoint::next, par, UAvoidPoint::prePoint, and UAvoidPoint::prev.

Referenced by convertToManSeq().

double UAvoidPath2::triangleFinda	(	double	b,
		double	c,
		double	C,
		double *	minor = `NULL`
	)

private

Solve a triangle using cosine relations with vertex A,B,C and oppsite sides a,b,c. Find a in: c^2 = a^2 + b^2 - 2 a b cos(C);

Parameters

b	side length next to vertex C.
c	side length opposite vertex C.
C	angle at vertex C
minor	the smaller solution is returned here (negative if C is > 90deg or c > b
returns	length of side a (longest solution (2 solutions if C is < 90deg and c < b)) (positive).
returns	-1.0 if determinant is negative

References a, and sqr().

Referenced by createMidPathPoints(), and expandOmegaTurn().

bool UAvoidPath2::turnCentreCrossing	(	UAvoidPoint *	arg1,
		UAvoidPoint *	arg2,
		double	margin
	)

private

Are the turn centre of these two near path points crossing. This also returns true, if turn centres are closer than a minimum margin to be crossing.

Parameters

ap1	is the most forward point on the route
ap2	is the previous point on the route
margin	an additional margin from crossing

Returns: true if turncentres cross.

References UAvoidPoint::aPos, UAvoidPoint::avoidLeft, U2Dlined::distanceSigned(), UPose::getDistance(), UAvoidPoint::mCent, U2Dseg::setFromPoints(), UPosition::x, UPose::x, UPosition::y, and UPose::y.

Referenced by createMidPathPoints().

bool UAvoidPath2::validNewClosePoint	(	UPosition	oPos,
		double	dist,
		UAvoidPoint *	ap2,
		UAvoidPoint *	ap1
	)

private

Validate this obstacle relative to the prevoiusly found obstacles - especially the points found in ap1 and ap2.

Parameters

oPos	is the found obstacle position.
dist	is the distance to the manoeuvre envalope - positive is outside the envalope and away from the visibility line between ap2 to ap1.
ap2	is the previous point and is the next point, and the obstacle point is found in the manoeuvre between these points.
ap1	is the end point of the manoeuvre.

Returns: true if the found point is valid (to be avoided).

References UAvoidPoint::aPos, UAvoidPoint::avoidLeft, UPosition::dist(), UPose::getDistance(), UAvoidParams::getSafeDistance(), limitToPi(), UAvoidPoint::mid, UAvoidPoint::next, UAvoidParams::obstClearanceMinimum, UAvoidPoint::oob, UAvoidPoint::oPos, par, UAvoidPoint::prev, UAvoidParams::startPose, UAvoidParams::withinRobotOutline(), UPosition::x, UPose::x, UPosition::y, and UPose::y.

Referenced by convertToManSeq().

bool UAvoidPath2::withinAngleLimits	(	double	a,
		bool	cvLeft,
		double	h1,
		double	h2,
		double	angMarg
	)

private

Test if this angle is within some start and end angles, when turning either left or right.

Parameters

a	is the angle under test (radians, within +/- PI)
cvLeft	if true, then h2 is more clockwise than h1 - else the other way
h1	is the start angle
h2	is the end angle
angMarg	is an (optional) extra margin to be added to the h1...h2 range.

Returns: true, if a is within the limits.

Member Data Documentation

UAvoidLink* UAvoidPath2::aLnkLast

protected

Obstackle groups free list - last used object, the free pointer is not used.

Referenced by findObstAvoidRoute(), getEmptyALnk(), UAvoidPath2(), and ~UAvoidPath2().

UAvoidLink* UAvoidPath2::aLnkRoot

protected

Obstackle groups baset on separation (passable separation)

Referenced by findObstAvoidRoute(), getEmptyALnk(), UAvoidPath2(), and ~UAvoidPath2().

UAvoidObst* UAvoidPath2::aogLast

protected

Obstackle groups free list - last used object, the free pointer is not used.

Referenced by findObstAvoidRoute(), getEmptyAog(), UAvoidPath2(), and ~UAvoidPath2().

UAvoidObst* UAvoidPath2::aogRoot

protected

Obstackle groups baset on separation (passable separation)

Referenced by findObstAvoidRoute(), getEmptyAog(), UAvoidPath2(), and ~UAvoidPath2().

UAvoidObst* UAvoidPath2::aogs[MAX_AOG_OBST_GRPS]

Obstackle groups baset on separation (passable separation)

Referenced by addStartAndExit(), UResAvoid::copyAvoidObstacles(), countValidVisibilityLines(), createCellBasedPointList(), createTangentLinks(), findObstAvoidRoute(), findRoutes(), findRoutesA(), findRouteToDest(), getClosestObst2(), groupObstacles(), invalidateEmbeddedVertices(), invalidateObstaclesAtStartAndExit(), logObstacleGroups(), removeTangentIfNoExitTangent(), removeTangentInConcavities(), setClosestPoint(), splitObstGroup(), testNonVisibilityLineCross(), and testVisibility().

int UAvoidPath2::aogsCnt

UObstacle* UAvoidPath2::aogsObst[MAX_LOCAL_OBSTS]

protected

Obstackle groups baset on separation (passable separation)

Referenced by addNoVisLinesAsObstacles(), UAvoidPath2(), and ~UAvoidPath2().

int UAvoidPath2::aogsObstCnt

protected

Number of used obstacle groups

Referenced by addNoVisLinesAsObstacles(), and UAvoidPath2().

UAvoidPoint* UAvoidPath2::aPointRoot

protected

Obstackle groups baset on separation (passable separation)

Referenced by getEmptyPoint(), recyclePoint(), recyclePoints(), and UAvoidPath2().

UAvoidCellGraph* UAvoidPath2::avCellGraph

Celle based avoidance path structure

Referenced by findObstAvoidRoute(), UResAvoid::findPathToHere(), UAvoidPath2(), and ~UAvoidPath2().

int UAvoidPath2::avoidSerial

protected

Serial number for this obstacle avoidance path

Referenced by findObstAvoidRoute(), and UAvoidPath2().

int UAvoidPath2::closeCnt

protected

Number of too close obstacles during this path.

Referenced by convertToManSeq(), and findObstAvoidRoute().

UAvoidLnkSeq* UAvoidPath2::closedSet

protected

Array of possible link sequences in prioritized order.

Referenced by addToClosedSet(), clear(), and isInClosedSet().

UAvoidVertexCosts UAvoidPath2::costAdd

protected

additional path cost for path finding

Referenced by convertToManSeq(), expandVisLinesToManSeq(), and findObstAvoidRoute().

bool UAvoidPath2::crash

protected

Is this path failed, i.e. no path found to exit pose i.e. ended in a crash

Referenced by clear(), and UAvoidPath2().

bool UAvoidPath2::endsInObst

protected

Flag for desired exit point is in an obstacle. This may be the case in especially DIRECT type movement, and may require special treatmnet (tell calculator).

UPosition UAvoidPath2::endsInObstHere

protected

Position where route enters obstacle

int UAvoidPath2::generation

protected

The current generation of point creation.

Referenced by convertToManSeq(), createAvoidPoinsPath(), createCellBasedPointList(), createMidPathPoints(), expandVisLinesToManSeq(), findObstAvoidRoute(), and setPreStopPoint().

UAvoidLnkSeq* UAvoidPath2::lnkSeqLast

protected

The last used link

Referenced by recycleLnkSeq(), UAvoidPath2(), and ~UAvoidPath2().

UAvoidLnkSeq* UAvoidPath2::lnkSeqRoot

protected

Linked list of sequence objects for reuse. This pointer is the first element in the reuse list, the first may be used, and the next to reuse in in the last pointer.

Referenced by getEmptyLnkSeq(), recycleLnkSeq(), UAvoidPath2(), and ~UAvoidPath2().

UAvoidLnkSeq* UAvoidPath2::lnkSeqs[MAX_LINK_SEQS]

protected

Array of possible link sequences in prioritized order.

Referenced by addToLnkSeqs(), findObstAvoidRoute(), findRoutes(), findRoutesA(), and logPathSequences().

int UAvoidPath2::lnkSeqsCnt

protected

Count of used sequences

Referenced by addToLnkSeqs(), findObstAvoidRoute(), findRoutes(), findRoutesA(), logPathSequences(), and UAvoidPath2().

int UAvoidPath2::lnkSerial

protected

next serial number for fetch of linkSequence.

Referenced by findRoutesA(), getEmptyLnkSeq(), and UAvoidPath2().

FILE* UAvoidPath2::logdbg

protected

Logfile for debug

Referenced by clear(), findObstAvoidRoute(), groupObstacles(), logObstacleGroups(), logPathSequences(), testNonVisibilityLineCross(), testVisibility(), UAvoidPath2(), and ~UAvoidPath2().

UManSeq* UAvoidPath2::manFull

protected

The manoeuvre sequence found in this path

Referenced by clear(), expandVisLinesToManSeq(), findObstAvoidRoute(), getManTime(), UAvoidPath2(), and ~UAvoidPath2().

const int UAvoidPath2::MAX_AOG_OBST_GRPS = 60

static

Max number of UAvoidObstacleGrouups (AOG) groups

Referenced by addNoVisLinesAsObstacles(), findObstAvoidRoute(), groupObstacles(), and splitObstGroup().

const int UAvoidPath2::MAX_FOODPRINT_POLYS = 200

static

Referenced by getFoodprintPoly(), UAvoidPath2(), and ~UAvoidPath2().

const int UAvoidPath2::MAX_LINK_SEQS = 30

staticprotected

Maximum number of saved link sequences

Referenced by addToLnkSeqs(), and logPathSequences().

const int UAvoidPath2::MAX_LOCAL_OBSTS = 200

staticprotected

Max number of local obstacles - to replace no-visiblity lines

Referenced by addNoVisLinesAsObstacles(), UAvoidPath2(), and ~UAvoidPath2().

double UAvoidPath2::maxAngleCost

protected

Max angle cost allowed for any sequence, based on currently most promising solution

Referenced by addToLnkSeqs(), findRoutes(), findRoutesA(), and findRouteToDest().

double UAvoidPath2::maxDistCost

protected

Max distance cost allowed for any sequence, based on currently most promising solution

Referenced by addToLnkSeqs(), findRoutes(), findRoutesA(), and findRouteToDest().

long unsigned int UAvoidPath2::maxSerial

private

A debug variable used to allocate serial numbers to added connection obstacles (from no-visibility lines)

Referenced by addNoVisLinesAsObstacles(), and groupObstacles().

int UAvoidPath2::oldFootCnt

number of footprint polygons before reset

Referenced by convertToManSeq(), UResAvoid::findPathToHere(), and UAvoidPath2().

UAvoidLnkSeq* UAvoidPath2::openSet

protected

Array of possible link sequences in prioritized order.

Referenced by addToOpenSet(), clear(), findRoutesA(), and terminateWorseCandidatesInOpenSet().

UAvoidParams* UAvoidPath2::par

protected

Is this path save for crash analysis. if so the valid flag is still true, but should not be included when searching for best path

Referenced by addLnksToOpenSet(), addStartAndExit(), addToLnkSeqs(), avoidToTheLeft(), convertToManSeq(), createAvoidPoinsPath(), createCellBasedPointList(), createMidPathPoints(), createPointList(), expandVisLinesToManSeq(), extendManoeuvre(), findObstAvoidRoute(), findRoutes(), findRoutesA(), findRouteToDest(), getClosestObst2(), groupObstacles(), insertNewPointAfter(), invalidateObstaclesAtStartAndExit(), isAfterDestination(), logPathSequences(), setClosestPoint(), setMidPoint(), setPreStopPoint(), setTurnCentre(), toReplaceOldPoint(), UAvoidPath2(), and validNewClosePoint().

bool UAvoidPath2::pathUsed

protected

Is path used, i.e. this is determined to be the best aviailable path

Referenced by clear(), and UAvoidPath2().

UAvoidPoint* UAvoidPath2::pointListFinal

protected

The final point list of points to be avoided

Referenced by expandVisLinesToManSeq(), findObstAvoidRoute(), and UAvoidPath2().

UPolygon* UAvoidPath2::polys[MAX_FOODPRINT_POLYS]

Debug polygons for dynamic free path polygons

Referenced by convertToManSeq(), UResAvoid::copyFootprintPolys(), getFoodprintPoly(), isCrossingManPoly(), isInsidePoly(), UAvoidPath2(), and ~UAvoidPath2().

int UAvoidPath2::polysCnt

Number of used polygons

Referenced by convertToManSeq(), UResAvoid::copyFootprintPolys(), getClosestObst2(), getFoodprintPoly(), isCrossingManPoly(), and UAvoidPath2().

int UAvoidPath2::poolIdx

Index of this path in the path pool (used for logging putposes)

Referenced by UAvoidPath2().

int UAvoidPath2::serialNext

protected

next serial number for inserted avoid points

Referenced by expandVisLinesToManSeq(), findObstAvoidRoute(), and getEmptyPoint().

int UAvoidPath2::solutionCnt

protected

debug count of mid-point solutions

Referenced by convertToManSeq(), createMidPathPoints(), and UAvoidPath2().

UObstacle* UAvoidPath2::startExitObst[2]

protected

Fake obstacles for start and exit position

Referenced by addStartAndExit(), UAvoidPath2(), and ~UAvoidPath2().

int UAvoidPath2::tanSeqCnt

protected

Count of tangents searched while finding af tangent sequence

Referenced by findRoutes(), findRoutesA(), and findRouteToDest().

bool UAvoidPath2::valid

protected

Is path valid, that is there is a calculated route, and it may be evaluated for best path - if crash flag is not set.

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

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

Public Member Functions

Public Attributes

Static Public Attributes

Protected Member Functions

Protected Attributes

Static Protected Attributes

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation