#include <uline.h>

Inheritance diagram for ULine:

Public Member Functions
virtual void	clear ()

int	getCylinderCrossings (UPosition center, double r, double t1, double t2)

virtual const char *	getDataType ()

double	getDistance (UPosition *point)

double	getDistanceSq (UPosition *point)

double	getDistanceSq (UPosition point)

UPosition	getPlaneLineCrossing (ULine crossingLine, bool *notParallel=NULL)

double	getPositionOnLine (UPosition *point)

double	getPositionOnLine (UPosition point)

UPosition	getPositionOnLine (const double t)

double	getPositionOnLineXY (UPosition *point)

double	getPositionOnLineXY (UPosition point)

UPosition	getPositionOnLineXY (const double t)

int	getSphereCrossings (UPosition center, double r, double t1, double t2)

bool	getXYCrossing (ULine other, UPosition *crossingXY)

double	getXYHeading ()

double	getXYsignedDistance (UPosition pos)

bool	isValid ()

void	normalize ()

void	print (const char *prestring=NULL)

void	set (UPosition iPos, UPosition iVec)

void	set2D (double x, double y, double heading)

virtual void	setFromPoints (const UPosition pos1, const UPosition pos2)

virtual void	setFromPoints (UPosition pos1, UPosition pos2)

void	setFromPoints (double x1, double y1, double z1, double x2, double y2, double z2)

void	show (const char *prestring=NULL)

virtual void	snprint (const char prestring, char buff, const int buffCnt)

void	turn90degXY (bool left)

	ULine ()

	ULine (double Px, double Py, double Pz, double Vx, double Vy, double Vz)

virtual	~ULine ()

Public Member Functions inherited from UDataBase
bool	isA (const char *typeString)

virtual bool	isAlsoA (const char *typeString)

	UDataBase ()

virtual	~UDataBase ()

Public Attributes
UPosition	pos

UPosition	vec

Detailed Description

Author: Christian Andersen This is a general purpose class for 3D lines and 3D planes, with a few simple functions to create and use the line.

Constructor & Destructor Documentation

ULine::ULine ( )

Constructor

References clear().

ULine::ULine	(	double	Px,
		double	Py,
		double	Pz,
		double	Vx,
		double	Vy,
		double	Vz
	)

Constructr from position and vector

References pos, UPosition::toUnitVector(), vec, UPosition::x, UPosition::y, and UPosition::z.

ULine::~ULine ( )

virtual

Destructor

Member Function Documentation

void ULine::clear ( void )

virtual

Clear

Reimplemented in ULineSegment.

References UPosition::clear(), pos, and vec.

Referenced by ULineSegment::clear(), getDataType(), ULineSegment::getDataType(), and ULine().

int ULine::getCylinderCrossings	(	UPosition	center,
		double	r,
		double *	t1,
		double *	t2
	)

Get 2D crosings with a circle (3d cylinder along Z axis) of radius r and center (x,y only) at 'center'. The results are in t1, t2 (vector distance on line) and if solutions are found). Returns the number of solutions - either 0 or 2. If only one solution is found, then this solution in in both t1 and t2. Solved from the equations: Line = P + V*t (or P(t) = [P_x + V_x * t, P_y + V_y * t])
Circle (x - C_x)^2 + (y - C_y)^2 = r^2
so by inserting line into circle:
((P_x + V_x * t) - C_x)^2 + ((P_x + V_x * t) - C_x)^2 = r^2
define k_x = P_x - C_x;
define k_y = P_y - C_y;
and isolate t-terms and solve this 2nd order equation:
(V_x^2 + V_y^2)t^2 + 2(k_x V_x + k_y V_y)t + k_x^2 + k_y^2 - r^2 = 0

References pos, sqr(), vec, UPosition::x, and UPosition::y.

Referenced by UManArc::getMinDistanceXYSigned(), print(), and testCylinderCrossing().

virtual const char* ULine::getDataType ( )

inlinevirtual

Get (end) type string for this structure

Reimplemented from UDataBase.

Reimplemented in ULineSegment.

References clear(), and isValid().

double ULine::getDistance ( UPosition * point )

inline

Get distance from this line to point.

References getDistanceSq().

double ULine::getDistanceSq ( UPosition * point )

Get distance squared from this line to point.

References getPositionOnLine(), pos, sqr(), vec, UPosition::x, UPosition::y, and UPosition::z.

Referenced by UManSeq::getDeviationFromDirect(), getDistance(), URoadLine::getDistanceSD(), getDistanceSq(), and UEdgeLinePair::setPair().

double ULine::getDistanceSq ( UPosition point )

inline

Get distance squared from this line to point.

References getDistanceSq(), and getPositionOnLine().

UPosition ULine::getPlaneLineCrossing	(	ULine	crossingLine,
		bool *	notParallel = `NULL`
	)

Get position on this plane, where the line 'crossingLine' is crossing. This is a plane with a point 'pos' and a normal vector 'vec'. If the line is not too close to parallel 'notParallel' is true and crossing point is returned. Otherwise a very large position in direction of the line vector is returned.

References absd(), getPositionOnLine(), pos, vec, UPosition::x, UPosition::y, and UPosition::z.

Referenced by UImgPolyProj::getPixToRobFloor(), UImgProj::getPixToRobFloor(), UClientFuncLaserGui::getPixToRobFloor(), print(), and testLinePlane().

double ULine::getPositionOnLine ( UPosition * point )

Get get parameter value for nearest position on line to this point. parameter is the 't' in line equation [x,y,z] = vec * t + pos. Returns t value

References pos, vec, UPosition::x, UPosition::y, and UPosition::z.

Referenced by UAvoidPath2::addNoVisLinesAsObstacles(), UAvoidObst::crossingNonVisibilityLine(), UAvoidCellGraph::fillVerticeList(), UResRoadDrive::findExitPose(), UResPassable::findTopOfRoad(), ULaserPi::getCenterPos(), UPolygon::getClosestDistance(), ULineSegment::getDistanceFromSegSq(), getDistanceSq(), ULineSegment::getDistanceXYSigned(), UPolygon::getDistanceXYsigned2(), UPlane::getLineCrossing(), UManLine::getMinDistanceXYSigned(), UManArc::getMinDistanceXYSigned(), UPolygon::getMostDistantXY(), UAvoidNoVis::getNoVisSegment(), ULineSegment::getOtherEnd(), getPlaneLineCrossing(), getPositionOnLine(), ULaserPi::getTop(), UPolygon::isOverlappingXYconvex2(), UNavPaint::paintFeatures(), UAvoidPath2::setClosestPoint(), UAvoidNoVis::setNoVisSegment(), testCylinderCrossing(), UReactivePath::testForObstacleNearExit(), URoadLine::update(), and UResRoadLine::updateRoadVariables().

double ULine::getPositionOnLine ( UPosition point )

inline

Get get parameter value for nearest position on line to this point. parameter is the 't' in line equation [x,y,z] = vec * t + pos. Returns t value

References getPositionOnLine(), and getPositionOnLineXY().

UPosition ULine::getPositionOnLine ( const double t )

Get position with this parameter value. Given a 't' value for a parametized line [x,y,z] = vec * t + pos, The [x,y,z] position is returned.

References pos, vec, UPosition::x, UPosition::y, and UPosition::z.

double ULine::getPositionOnLineXY ( UPosition * point )

Get get parameter value for nearest position on line to this point. parameter is the 't' in line equation [x,y,z] = vec * t + pos. Returns t value

References pos, vec, UPosition::x, and UPosition::y.

Referenced by getPositionOnLine(), getPositionOnLineXY(), and ULineSegment::getSegmentCrossingXY().

double ULine::getPositionOnLineXY ( UPosition point )

inline

Get get parameter value for nearest position on line to this point. parameter is the 't' in line equation [x,y,z] = vec * t + pos. Returns t value

References getPositionOnLineXY(), set2D(), setFromPoints(), and show().

UPosition ULine::getPositionOnLineXY ( const double t )

Get position with this parameter value. Given a 't' value for a parametized line [x,y,z] = vec * t + pos, The [x,y,z] position is returned.

References pos, vec, UPosition::x, UPosition::y, and UPosition::z.

int ULine::getSphereCrossings	(	UPosition	center,
		double	r,
		double *	t1,
		double *	t2
	)

Get 3D crosings of this line and a sphere of radius r and center (x,y,z) at 'center'. The results are in t1, t2 (vector distance on line) and if solutions are found). Returns the number of solutions - either 0 or 2. If only one solution is found, then this solution in in both t1 and t2. Solved from the equations: Line = P + V*t (or P(t) = [P_x + V_x * t, P_y + V_y * t, P_z * V_z * t])
Circle (x - C_x)^2 + (y - C_y)^2 + (z - C_z)^2 = r^2
so by inserting line into circle:
((P_x + V_x * t) - C_x)^2 + ((P_x + V_x * t) - C_x)^2 + ((P_z + V_z * t) - C_z)^2= r^2
define k_x = P_x - C_x;
define k_y = P_y - C_y;
define k_z = P_z - C_z;
and isolate t-terms and solve this 2nd order equation:
(V_x^2 + V_y^2 + V_z^2)t^2 + 2(k_x V_x + k_y V_y + k_z V_z)t + k_x^2 + k_y^2 + k_z^2 - r^2 = 0

References pos, sqr(), vec, UPosition::x, UPosition::y, and UPosition::z.

Referenced by print(), and testCylinderCrossing().

bool ULine::getXYCrossing	(	ULine	other,
		UPosition *	crossingXY
	)

This functio finds the crossing of the two lines projected to the Z plane, i.e. the x and y position is used only, and the z position returned is always zero. Returns true if crossing is found.

References U2Dlined::getCrossing(), pos, UPosition::set(), U2Dlined::setPV(), vec, UPosition::x, and UPosition::y.

Referenced by UAvoidObst::crossingNonVisibilityLine(), ULineSegment::getSegmentCrossingXY(), and print().

double ULine::getXYHeading ( )

inline

Get vector XY heading in radians

References turn90degXY(), vec, UPosition::x, and UPosition::y.

Referenced by UAvoidPath2::addLnksToOpenSet(), UResRoadDrive::findExitPose(), UAvoidPath2::findRoutes(), UAvoidPath2::findRoutesA(), UAvoidPath2::findRouteToDest(), and UReactivePath::getMidPoseHere().

double ULine::getXYsignedDistance ( UPosition pos )

Get signed distance from the x,y part of this position to this line (projected to the z-plane). Returnes the distance, with sign indicating the side of the line.

References U2Dlined::distanceSigned(), pos, U2Dlined::setPV(), vec, UPosition::x, and UPosition::y.

Referenced by UResRoadLine::findCurrentRoad(), UResRoadLine::findCurrentRoad2(), UResPassable::findTopOfRoad(), print(), and UResRoadLine::updateRoadVariables().

bool ULine::isValid ( )

Test to if line is valid. Line is valid if length of vector is 1.0.

References absd(), UPosition::dist(), and vec.

Referenced by getDataType().

void ULine::normalize ( )

inline

Normalize the line, to make a valid line (vector needs to be a unit vector)

Referenced by UPlane::getPlaneCrossing().

void ULine::print ( const char * prestring = NULL )

inline

References getCylinderCrossings(), getPlaneLineCrossing(), getSphereCrossings(), getXYCrossing(), getXYsignedDistance(), pos, show(), and snprint().

void ULine::set	(	UPosition	iPos,
		UPosition	iVec
	)

Set from a position and a vector

References pos, UPosition::toUnitVector(), and vec.

Referenced by testLinePlane().

void ULine::set2D	(	double	x,
		double	y,
		double	heading
	)

Set line from 2D (x,y,th) information. Other values are set to zero.

References pos, vec, UPosition::x, UPosition::y, and UPosition::z.

Referenced by getPositionOnLineXY(), UClientFuncSF::handleSF(), UResLaserIfSf::handleSF(), and URoadLine::setNew().

void ULine::setFromPoints	(	const UPosition *	pos1,
		const UPosition *	pos2
	)

virtual

Set line from two points. Position pos1 is used as base point and vector is set pointing towards pos2.

Reimplemented in ULineSegment.

References pos, UPosition::toUnitVector(), vec, UPosition::x, UPosition::y, and UPosition::z.

Referenced by ULineSegment::getDataType(), UImgProj::getPixToRobFloor(), UImgPolyProj::getPixToRobFloor(), UClientFuncLaserGui::getPixToRobFloor(), getPositionOnLineXY(), ULineSegment::setFromPoints(), and testCylinderCrossing().

void ULine::setFromPoints	(	UPosition	pos1,
		UPosition	pos2
	)

virtual

Set line from two points. Position pos1 is used as base point and vector is set pointing towards pos2.

Reimplemented in ULineSegment.

References pos, UPosition::toUnitVector(), vec, UPosition::x, UPosition::y, and UPosition::z.

void ULine::setFromPoints	(	double	x1,
		double	y1,
		double	z1,
		double	x2,
		double	y2,
		double	z2
	)

Set line from points. a line is a start point and a direction vector.

Parameters

x1,y1,z1	defines the first (start) point
x2,y2,z2	defines the end point

References pos, UPosition::set(), UPosition::toUnitVector(), vec, UPosition::x, UPosition::y, and UPosition::z.

void ULine::show ( const char * prestring = NULL )

Show values of line to console

References pos, UPosition::show(), and vec.

Referenced by ULineSegment::getDataType(), getPositionOnLineXY(), print(), ULineSegment::print(), ULineSegment::show(), and testLinePlane().

void ULine::snprint	(	const char *	prestring,
		char *	buff,
		const int	buffCnt
	)

virtual

Print values of line to string

Reimplemented from UDataBase.

Reimplemented in ULineSegment.

References pos, vec, UPosition::x, UPosition::y, and UPosition::z.

Referenced by ULineSegment::getDataType(), print(), and ULineSegment::snprint().

void ULine::turn90degXY ( bool left )

Turn the vector in this line to its normal vector in the (XY plane only). This is done by swapping the x and y values and xhanging sign on the x value if 'left' is true, otherwise change sign on y-value.

References a, vec, UPosition::x, and UPosition::y.

Referenced by getXYHeading().

Member Data Documentation

UPosition ULine::pos

Position on line, if a line segment then this is the (or one of) end point.

Referenced by UAvoidPath2::addNoVisLinesAsObstacles(), clear(), ULaserMeasurement::clear(), USmlTag::codeLineSegment(), UFakeMap::copyToPoly(), UAvoidObst::crossingNonVisibilityLine(), UClientFuncLaserGui::doImage(), UAvoidCellGraph::fillVerticeList(), ULaserPi::get2DLine(), getCylinderCrossings(), ULineSegment::getDistanceFromSegSq(), getDistanceSq(), ULineSegment::getDistanceXYSigned(), UPlane::getLineCrossing(), USmlTag::getLineSegment(), UManLine::getMinDistanceXYSigned(), UManArc::getMinDistanceXYSigned(), UManPPSeq::getMinDistanceXYSigned(), UManSeq::getMinDistanceXYSigned(), UPolygon::getMostDistantXY(), UPlane::getPlaneCrossing(), getPlaneLineCrossing(), getPositionOnLine(), getPositionOnLineXY(), getSphereCrossings(), getXYCrossing(), getXYsignedDistance(), UAvoidPath2::logObstacleGroups(), USFData::moveLocalToMap(), UFeatureData::moveLocalToOdo(), ULaserObst::moveToMap(), ULaserPi::moveToMap(), UNavPaint::paintFeatures(), UNavView::paintFeatures(), UClientFuncLaserGui::paintLineSegments(), UNavPaint::paintManData(), UClientFuncLaserGui::paintPathData(), UNavPaint::paintRoadLine(), print(), UFunctionAvoid::sendCurrentAvoidPath(), set(), set2D(), setFromPoints(), ULaserPi::setInterval(), show(), snprint(), testPlane(), ULine(), URoadLineData::update(), and URoadLine::update().

UPosition ULine::vec

Direction of line. This is a unit vector

Referenced by UAvoidPath2::addNoVisLinesAsObstacles(), clear(), USmlTag::codeLineSegment(), UResPassable::combineNearIntervals(), UAvoidObst::crossingNonVisibilityLine(), UAvoidCellGraph::fillVerticeList(), ULaserPi::get2DLine(), getCylinderCrossings(), UAvoidLnkSeq::getDistance(), getDistanceSq(), UPlane::getLineCrossing(), USmlTag::getLineSegment(), UManPPSeq::getMinDistanceXYSigned(), UPolygon::getMostDistantXY(), UAvoidVertexIdx::getPktType2(), UPlane::getPlaneCrossing(), getPlaneLineCrossing(), getPositionOnLine(), getPositionOnLineXY(), getSphereCrossings(), UAvoidVertexIdx::getUpper(), getXYCrossing(), getXYHeading(), getXYsignedDistance(), UPolygon::isOverlappingXYtype(), isValid(), set(), set2D(), setFromPoints(), URoad::setRoadQ(), show(), snprint(), testPlane(), turn90degXY(), and ULine().

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

Public Member Functions

Public Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation