AURobotServers  4
Public Member Functions | Static Public Member Functions | Public Attributes | List of all members
UPosition Class Reference

#include <u3d.h>

Inheritance diagram for UPosition:
Inheritance graph

Public Member Functions

void add (const UPosition *pos)
 
void add (const UPosition pos)
 
void add (const double ix, const double iy, const double iz)
 
UPosition added (UPosition *pos)
 
UPosition added (UPosition pos)
 
UMatrix4 asCol3 ()
 
UMatrix4 asCol4 ()
 
UMatrix4 asMatrix4x4 ()
 
UMatrix4 asRow3 ()
 
UMatrix4 asRow4 ()
 
UMatrix4 asVector3 (bool column)
 
UMatrix4 asVector4 (bool column)
 
void clear (void)
 
virtual const char * codeXml (char *buf, const int bufCnt, const char *extraAttr)
 
virtual const char * codeXml (const char *name, char *buf, const int bufCnt, const char *extraAttr)
 
int copy (UMatrix4 *pos)
 
int copy (const URotation *rot)
 
UPosition cross (UPosition p2)
 
void cross (UPosition p1, UPosition p2)
 
double dist (const UPosition *pTo)
 
double dist (UPosition To)
 
double dist ()
 
double distSq (const UPosition *pTo)
 
double distSq ()
 
double distXY ()
 
double distXY (UPosition p2)
 
double dot (UPosition p2)
 
double get (int idx)
 
virtual const char * getDataType ()
 
UMatrix4 getPixelPos (UMatrix4 *A, UMatrix4 *b)
 
UPositiongetPos ()
 
UPosition getTangentPointXY (const UPosition centre, const double radius, const bool rightSide, bool *valid, double *ang=NULL)
 
bool load (const char *valueString)
 
int LoadFromReg (Uconfig *ini, const char *subject, const char *key)
 
UPosition operator* (const double scalar)
 
void operator*= (const double scalar)
 
UPosition operator+ (UPosition pos)
 
void operator+= (const UPosition pos)
 
UPosition operator- (UPosition pos)
 
void operator-= (const UPosition pos)
 
UPosition operator= (UMatrix4 vec)
 
UPosition operator= (const URotation rot)
 
UPosition operator= (const U2Dpos val)
 
void print (const char *leadString)
 
void println (const char *leadString)
 
bool save (FILE *fmap, const char *key)
 
int SaveToReg (Uconfig *ini, const char *subject, const char *key)
 
void scale (const double val)
 
UPosition scaled (const double val)
 
void set (const double ix, const double iy, const double iz)
 
void set (const int idx, const double value)
 
void setPos (UPosition fromPos)
 
void show (const char *leadString)
 
virtual void snprint (const char *leadString, char *s, const int bufferLength)
 
void sprint (char *s, const char *leadString)
 
void subtract (const UPosition *pos)
 
UPosition subtracted (UPosition *pos)
 
UPosition subtracted (UPosition pos)
 
int toUnitVector ()
 
void transfer (UMatrix4 *mA)
 
UPosition transferred (UMatrix4 *mA)
 
 UPosition ()
 
 UPosition (double ix, double iy, double iz)
 
 UPosition (double ix, double iy)
 
virtual ~UPosition ()
 
- Public Member Functions inherited from UDataBase
bool isA (const char *typeString)
 
virtual bool isAlsoA (const char *typeString)
 
 UDataBase ()
 
virtual ~UDataBase ()
 

Static Public Member Functions

static UPosition position (double ix, double iy, double iz)
 

Public Attributes

double x
 
double y
 
double z
 

Detailed Description

General class to hold x,y,z position x is left, y is up and z is forward for 3D coordinates unit is meter

Constructor & Destructor Documentation

UPosition::UPosition ( )

Constructor with zero initial value.

UPosition::~UPosition ( )
virtual

Destructor.

UPosition::UPosition ( double  ix,
double  iy,
double  iz 
)

Constructor with initial values.

UPosition::UPosition ( double  ix,
double  iy 
)
inline

Constructor with initial values.

References UPos::x, and UPos::y.

Member Function Documentation

void UPosition::add ( const UPosition pos)

Add 'pos' to this position.

References x, y, and z.

Referenced by ULaserMeasurement::clear(), UFakeMap::getFake2range(), handlePeopleDetection(), operator+=(), and UFuncStraightLine::toRobot().

void UPosition::add ( const UPosition  pos)

Add 'pos' to this position.

References x, y, and z.

void UPosition::add ( const double  ix,
const double  iy,
const double  iz 
)

Add (ix, iy, iz) to this position.

References x, y, and z.

UPosition UPosition::added ( UPosition pos)

Return this position added with 'pos', but leave this position unchanged.

References x, y, and z.

Referenced by operator+().

UPosition UPosition::added ( UPosition  pos)

Return this position added with 'pos', but leave this position unchanged.

References x, y, and z.

UMatrix4 UPosition::asCol3 ( )
inline

position as [x,y,z] column.

References add.

UMatrix4 UPosition::asCol4 ( )
inline

position as homogenous [x,y,z,1] column.

UMatrix4 UPosition::asMatrix4x4 ( )

Return as translation matrix, Returns: (1,0,0,-x; 0,1,0,-y; 0,0,1,-z; 0,0,0,1).

References UMatrix::setRC(), x, y, and z.

Referenced by URotation::asMatrix4x4CtoW(), URotation::asMatrix4x4MtoR(), URotation::asMatrix4x4RtoM(), and URotation::asMatrix4x4WtoC().

UMatrix4 UPosition::asRow3 ( )
inline

position as [x,y,z] row.

UMatrix4 UPosition::asRow4 ( )
inline

position as homogenous [x,y,z,1] row.

UMatrix4 UPosition::asVector3 ( bool  column)

Returns position as [x,y,z] vector. column or row.

References UMatrix::getData(), UMatrix::setSize(), x, y, and z.

Referenced by URotation::asVector3().

UMatrix4 UPosition::asVector4 ( bool  column)

Returns position as homogenous [x,y,z,1] vector. Column or row

References UMatrix::getData(), UMatrix::setSize(), x, y, and z.

Referenced by URotation::asVector4(), UCamMounted::getMtoPix(), getPixelPos(), transfer(), and transferred().

void UPosition::clear ( void  )
const char * UPosition::codeXml ( char *  buf,
const int  bufCnt,
const char *  extraAttr 
)
virtual

Code this structure in XML format. The open tag includes any additional XML attributes from the codeXmlAttributes function call

Parameters
bufis the character buffer where the attributes are stored
bufCntis the amount of buffer space allocated
extraAttris an optional extra set of attributes, pre-coded with name and value
Returns
a pointer to the buffer

References buf, x, y, and z.

Referenced by USmlTag::codePosition(), codeXml(), UPolygon::codeXml(), and UFunctionLaser::handleSetCommand().

const char * UPosition::codeXml ( const char *  name,
char *  buf,
const int  bufCnt,
const char *  extraAttr 
)
virtual

Code this structure in XML format. The open tag includes any additional XML attributes from the codeXmlAttributes function call

Parameters
nameoptional extra name value for a name attribute.
bufis the character buffer where the attributes are stored
bufCntis the amount of buffer space allocated
extraAttris an optional extra set of attributes, pre-coded with name and value
Returns
a pointer to the buffer

References codeXml().

int UPosition::copy ( UMatrix4 pos)

Set position from vector. If vector has length 3, then just copy x = pos.m[0], ... if vector has length 4, then scale with pos.m[3].

References UMatrix::getData(), UMatrix::size(), x, y, and z.

Referenced by URotation::asVector3(), URotation::asVector4(), URotation::copy(), UCalibrationMarkSet::evaluateChartPosRot(), UCalibrationMarkSet::evaluatePosRot(), UCamMounted::getMtoPix(), operator=(), URotation::SaveToReg(), transfer(), and transferred().

int UPosition::copy ( const URotation rot)

Set position from vector. If vector has length 3, then just copy x = pos.m[0], ... if vector has length 4, then scale with pos.m[3]. Set position as numeric copy of three rotation values. x = rot.Omega, y = rot.Phi, z = rot.Kappa.

References URotation::Kappa, URotation::Omega, URotation::Phi, x, y, and z.

UPosition UPosition::cross ( UPosition  p2)

Calculate cross-product with this position (vector)

Parameters
p2is a 3d vector
Returns
(this x p2) = (y*v.z-z*v.y, z*v.x-x*v.z, x*v.y-y*v.x)

References x, y, and z.

Referenced by cross(), UPlane::getPlaneCrossing(), UOriPlane::print(), UPlane::set(), and URotation::setFromYZ().

void UPosition::cross ( UPosition  p1,
UPosition  p2 
)
inline

Sets this vector as a cross product of 2 vectors

References buf, and cross().

double UPosition::dist ( const UPosition pTo)
double UPosition::dist ( UPosition  To)
inline

Returns absolute distance to another position.

References dist.

double UPosition::dist ( )
inline

Returns absolute length of this vector (distance to (0,0,0)).

Referenced by toUnitVector().

double UPosition::distSq ( const UPosition pTo)

Returns absolute distance to another position squared

References sqr(), x, y, and z.

Referenced by ULaserMeasurement::clear(), UFakeMap::copyToPoly(), and ULineSegment::getDistanceFromSegSq().

double UPosition::distSq ( )
inline

Returns squared length of this vector (distance to (0,0,0)).

References sqr(), UPos::x, and UPos::y.

Referenced by dist().

double UPosition::distXY ( )
inline

Get the xy distance from 0,0.

References UPos::x, and UPos::y.

Referenced by UAvoidPath2::isCrossingManPoly().

double UPosition::distXY ( UPosition  p2)
inline

Get the xy distance from second point to this.

Parameters
p2is the other point.
Returns
XY distance.

References UPos::x, x, UPos::y, and y.

double UPosition::dot ( UPosition  p2)

Calculate dot-product with this position (vector)

Parameters
vis the other 3d vector (v.x, v.y, v.x)
Returns
(this * v) = (x*v.x + y*v.y + z*v.z)

References x, y, and z.

Referenced by UImg3Dpoints::getRansacPlane(), and URoad::setRoadQ().

double UPosition::get ( int  idx)
inline

Get value from an index (0=x,1=y, 2=z, 3=1.0)

Parameters
idxindex to value
Returns
value x,y or z from index, if not in range, then 1.0 is returned

References UPos::x, and UPos::y.

Referenced by UPlane::getPlaneCrossing().

virtual const char* UPosition::getDataType ( )
inlinevirtual

Get (end) type of this structure

Reimplemented from UDataBase.

Reimplemented in UPosRot.

Referenced by UFunctionVarPool::initCallReturnStructType().

UMatrix4 UPosition::getPixelPos ( UMatrix4 A,
UMatrix4 b 
)

Convert 3D position to pixel position using two conversion matrices. Normally A matrix translates and rotates to 3D image coordinates (4x4 matrix) and b converts to 2D image (top-left oriented coordinates. The result is a normalized vector [x (width), y (down), 1]

References asVector4(), and UMatrix::normalize().

UPosition* UPosition::getPos ( )
inline

Get pointer to this UPosition (e.g. from decendent classes).

Referenced by UPosRot::getPos(), and UPosRot::pos().

UPosition UPosition::getTangentPointXY ( const UPosition  centre,
const double  radius,
const bool  rightSide,
bool *  valid,
double *  ang = NULL 
)

Return tangent point, where a line from this point touches a circle with centre in c and radius bc. if 'rightSide', then b is on the right side of the circle - seen from this point. If this point is inside circle, then 'valid' will be set false and the center point will be returned. The z value of the result will be the same as the centre. If 'ang' != NULL, then the coordinate system angle to the tangent point will be returned here.

References x, and y.

Referenced by UReactivePath::getTangentMidPose(), and testTangetPoint().

bool UPosition::load ( const char *  valueString)

Load values from comma separated string values in e format. Returns true if all 3 values were read.

References x, y, and z.

int UPosition::LoadFromReg ( Uconfig ini,
const char *  subject,
const char *  key 
)

Load position from 'ini' configuration file under this 'subject' and with this 'key'. Returns 0 if successful, else -1 and value set to (0,0,0)

References Uconfig::strGet(), x, y, and z.

Referenced by URotation::LoadFromReg().

UPosition UPosition::operator* ( const double  scalar)

return a scaled (or multiplied) version of this position with 'val'.

References scaled().

void UPosition::operator*= ( const double  scalar)

Scale this position

References scale().

UPosition UPosition::operator+ ( UPosition  pos)

Return this position added with 'pos'

References added().

void UPosition::operator+= ( const UPosition  pos)

Add 'pos' to this position

References add().

UPosition UPosition::operator- ( UPosition  pos)

Return this position subtracted with 'pos'.

References subtracted().

void UPosition::operator-= ( const UPosition  pos)

Subtract 'pos' from this position

References subtract().

UPosition UPosition::operator= ( UMatrix4  vec)

Assign position from vector

References copy().

UPosition UPosition::operator= ( const URotation  rot)

Assign value from rotation

References copy().

UPosition UPosition::operator= ( const U2Dpos  val)

Assign value from rotation

References U2Dpos::x, x, U2Dpos::y, y, and z.

UPosition UPosition::position ( double  ix,
double  iy,
double  iz 
)
static
void UPosition::print ( const char *  leadString)
inline
void UPosition::println ( const char *  leadString)

Just print this position to console

References x, y, and z.

bool UPosition::save ( FILE *  fmap,
const char *  key 
)

Save position in html-like format. Returns true if saved.

References x, y, and z.

int UPosition::SaveToReg ( Uconfig ini,
const char *  subject,
const char *  key 
)

Save this position to 'ini' configuration file under this 'subject' and with this 'key'. returns -1 if no space for data.

References doubleToKeyStr(), MaxCharPerLine, MaxKeyLength, Uconfig::strPut(), x, y, and z.

Referenced by URotation::SaveToReg().

void UPosition::scale ( const double  val)
UPosition UPosition::scaled ( const double  val)

return a scaled (or multiplied) version of this position with 'val', but leave this position unchanged.

References x, y, and z.

Referenced by UCamParEst::adjustParameters35(), ULaserObst::findEdgeObstacle(), ULaserScan::getLineSegmentFit(), and operator*().

void UPosition::set ( const double  ix,
const double  iy,
const double  iz 
)
inline

Set position to (ix, iy, iz).

References UPos::x, and UPos::y.

Referenced by UImg3Dpoints::add(), ULineMatch::addToPoly(), UCamParEst::adjustParameters35(), UReactivePath::avoidObst(), ULaserMeasurement::clear(), UAvoidPath2::createCellBasedPointList(), UCalibrationMarkSets::doEstimateCameraParameters(), UCalibrationMarkSets::doEstimateCameraParametersBinary(), UCalibrationMarkSets::doEstimateCameraParametersCarpet(), UClientFuncLaserGui::doImage(), UImgPolyProj::doPolygonMap(), UImgProj::doPolygonMap(), UImgPolyProj::doProject(), UImgProj::doProject(), UImgPolyProj::doProjectPolygonToFloorReal(), UImgProj::doProjectPolygonToFloorReal(), UCalibrationMarkSet::evaluateChartPosRot(), UAvoidCellGraph::fillVerticeList(), UCalibrate::findBarcodeChartPosition(), UAvoidCellGraph::findCellPointPath(), UResRoadLine::findCurrentRoad(), UResRoadLine::findCurrentRoad2(), UResRoadDrive::findExitPose(), UCalibrate::findGmk(), UAvoidPath2::findObstAvoidRoute(), ULaserPi::get2DDistToCross(), UVariable::get3D(), UPolygon::getClosestDistance(), UPolygon::getDistance(), UFakeMap::getFake2range(), ULaserScan::getLineSegmentFit(), UManLine::getMinDistanceXYSigned(), UManArc::getMinDistanceXYSigned(), UPlane::getPlaneCrossing(), ULine::getXYCrossing(), UFuncLoca2::handleCommand(), UFuncLocalize::handleLocalize(), UNavView::paint(), UNavPaint::paintGmks(), UNavPaint::paintOdoGrid(), UClientFuncLaserGui::paintOdoGrid(), UNavView::paintOdoGrid(), UNavPaint::paintRobotGuidebot(), UNavPaint::paintRobotIrobot(), UResLocater::rowEndDetect(), UFuncKinect::run(), UPosRot::set(), UPosRot::setCtoR(), ULine::setFromPoints(), UPosRot::setFromPose(), UResPoseHist::setPoseResult(), ULaserPoint::setPosRA(), ULaserPoint::setPosXYZ(), UReactivePath::setRouteStats(), test3DtoPix(), testCylinderCrossing(), testImageHandling(), testImageLoadSave(), testLinePlane(), testLineSegment(), testManLineToSeg(), testManToSeg(), testMatrixVecBig(), testOriPlane(), testPlane(), testPolygonCog(), testSmlEncoding(), testUPosRot(), UAvoidParams::UAvoidParams(), UImgPolyProj::UImgPolyProj(), and UImgProj::UImgProj().

void UPosition::set ( const int  idx,
const double  value 
)
inline

Set position to of element idx to this value (idx: 0=x, 1=y, 2=z).

References UPos::x, and UPos::y.

void UPosition::setPos ( UPosition  fromPos)
inline

Set position from another position

Referenced by UPosRot::set(), and UPosRot::setPos().

void UPosition::show ( const char *  leadString)
void UPosition::snprint ( const char *  leadString,
char *  s,
const int  bufferLength 
)
virtual

As above function, but with limit on buffer length.

Reimplemented from UDataBase.

References x, y, and z.

Referenced by UCamPush::print(), UClientCamData::snprint(), and UPolygon::snprint().

void UPosition::sprint ( char *  s,
const char *  leadString 
)

Print this position to a string with this 'leadString'

References x, y, and z.

Referenced by UCalibrationMarkSet::evaluateChartPosRot(), and UCalibrationMarkSet::evaluatePosRot().

void UPosition::subtract ( const UPosition pos)

Subtract 'pos' from this position.

References x, y, and z.

Referenced by UCalibrationMarkSet::evaluateChartPosRot(), UCalibrationMarkSet::evaluatePosRot(), and operator-=().

UPosition UPosition::subtracted ( UPosition pos)

Return this position subtracted with 'pos', but leave this position unchanged.

References x, y, and z.

Referenced by operator-().

UPosition UPosition::subtracted ( UPosition  pos)

Return this position subtracted with 'pos', but leave this position unchanged.

References x, y, and z.

int UPosition::toUnitVector ( )

Scales the position (assumed to be a vector to a unit vector. i.e. (x^2 + y^2 + z^2) = 1. Returns -1 if zero vector.

References dist(), and scale().

Referenced by UPlane::set(), ULine::set(), ULine::setFromPoints(), URotation::setFromYZ(), and ULine::ULine().

void UPosition::transfer ( UMatrix4 mA)

Transfers this position to new position acording to mA (4x4)

References asVector4(), and copy().

Referenced by UImage::lineInGrid(), UNavPaint::paintCams(), and UNavPaint::paintGmks().

UPosition UPosition::transferred ( UMatrix4 mA)

Returns a transferred position of this position, but do not change this position.

References asVector4(), and copy().

Referenced by UMatrix4::operator*(), and UOriPlane::set().

Member Data Documentation

double UPosition::x

Referenced by add(), UResPcp::add(), UPaintBase::addBox(), added(), UResObj3d::addGroundObjects(), UAvoidPath2::addNoVisLinesAsObstacles(), UObstacleGroupLaser::addObst(), UObstacleGroupLaser::addObstPoly(), UPaintPolygon::addPolyToViewer(), UGridBBox::addPosToProfile(), UGridBBox::addPosToSideView(), UAvoidObst::addtangentLines(), ULineMatch::addToPoly(), asMatrix4x4(), asVector3(), asVector4(), UAvoidPath2::avoidToTheLeft(), UFuncBallKL::calculateBallPosition(), UFuncBall::calculateBallPosition(), UFuncBall_improved::calculateBallPosition(), ULaserMeasurement::clear(), clientCmdLine(), UPcpItem::codeXML(), codeXml(), UResAvoid::codeXmlRobotFront(), UResPassable::combineNearIntervals(), UAvoidPath2::convertToManSeq(), copy(), UPolygon::copy(), URotation::copy(), UFakeMap::copyToPoly(), UAvoidPath2::createCellBasedPointList(), UAvoidPath2::createMidPathPoints(), cross(), UAvoidObst::crossingNonVisibilityLine(), UPolygon::cut(), UPolygon::cutPoints(), UCamPool::decodeReplayLine(), Funcavoid::direkte(), U2Dpos::dist(), UPlane::distSigned(), distSq(), distXY(), UFuncGndAna::do3dGroundPlane(), UResObj3d::do3dGroundPlane(), UResObj3d::do3dVoxels(), UNamedBox::doDetect(), UResObj3d::doGndPlaneMask(), UClientFuncLaserGui::doImage(), UImgPolyProj::doProject(), UImgProj::doProject(), UImgProj::doProjectPolygonToMapPix(), UImgPolyProj::doProjectPolygonToMapPix(), UClientFuncLaserGui::doRepaint(), dot(), UCalibrationMarkSet::evaluateChartPosRot(), UCalibrationMarkSet::evaluatePosRot(), UAvoidPath2::expandPolyToSegment(), UAvoidVertexIdx::extent4thPnt(), UPolygon::extractConvexTo(), UAvoidCellGraph::fillVerticeList(), UCalibrate::findCameraPosition(), ULaserObst::findEdgeObstacle(), UResRoadDrive::findExitPose(), UResObj3d::findPolygonsOnEdge(), UAvoidPath2::findRoutes(), UAvoidPath2::findRouteToDest(), UFuncStraightLine::findWoldPoints(), ULaserPi::get2DDistToCross(), ULaserPi::get2DLine(), UGrid3D::getCellCenter(), UGrid3D::getCellFTL(), UGrid3D::getCellNBR(), UPolygon::getClosestDistance(), UAvoidPath2::getClosestObst2(), UAvoidObst::getClosestPoint(), UPolygon::getCloseVertexCnt(), UPolygon::getCogXY(), UProbPoly::getCrossingsAtX(), UProbPoly::getCrossingsAtY(), UCamPar::getCtoPRob(), ULine::getCylinderCrossings(), UAvoidParams::getDiagonal(), UPose::getDistance(), UAvoidLnkSeq::getDistance(), UPolygon::getDistance(), ULine::getDistanceSq(), UManLine::getDistanceXYSigned(), UManArc::getDistanceXYSigned(), UManSeq::getDistanceXYSigned(), ULineSegment::getDistanceXYSigned(), UAvoidParams::getDistToCorner(), UManPPSeq::getEndDistTo(), UCalibrationMarkSet::geterrorInPixels(), UFakeMap::getFake2range(), UManArc::getFoodprintPolygon(), UPolygon::getLimits(), UPlane::getLineCrossing(), ULaserScan::getLineSegmentFit(), UPolygon::getLowerLeftXY(), UPose::getMapToPose(), UPose::getMapToPosePose(), UReactivePath::getMidPoseHere(), UManLine::getMinDistanceXYSigned(), UManArc::getMinDistanceXYSigned(), UManSeq::getMinDistanceXYSigned(), UPolygon::getMostDistantVertexXY(), UPolygon::getMostDistantXY(), UReactivePath::getNearObstacle(), UAvoidNoVis::getNoVisSegment(), UAvoidVertexIdx::getPktType1(), UAvoidVertexIdx::getPktType2(), UPlane::getPlaneCrossing(), ULine::getPlaneLineCrossing(), UPcpItem::getPoint(), UCamMounted::getPosCC(), UPose::getPoseToMap(), UClientLaserData::getPosition(), ULaserData::getPosition(), USmlTag::getPosition(), ULine::getPositionOnLine(), ULine::getPositionOnLineXY(), UCamPar::getPtoCRob(), UImg3Dpoints::getRansacPlane(), UAvoidParams::getSafeDistance(), ULine::getSphereCrossings(), UManPPSeq::getStartDistTo(), UReactivePath::getTangentMidPose(), getTangentPointXY(), UAvoidObst::getTangentVertexFrom(), UManArc::getTurnCentre(), UAvoidVertexIdx::getUpper(), UAvoidParams::getWallWallRadius(), UPosRot::getX(), UPolygon::getXYarea(), ULine::getXYCrossing(), ULine::getXYHeading(), ULine::getXYsignedDistance(), UAvoidPath2::groupObstacles(), UFuncLocalize::handleAddLine(), UFuncLoca2::handleAddLine(), UClientCamData::handleCamGet(), UFuncBallKL::handleCommand(), UFuncObj3d::handleCommand(), UFunctionCamGmk::handleGmkGetCommand(), UFuncImgPoly::handleImgPolyCommand(), UFuncLocalize::handleLocalize(), UFuncLocalize::handleLocalizeMHF(), UFuncLocalize::handleLocalizeUKF(), UResPcp::handleNewData(), UFunctionPassable::handleObstGet(), UFunctionCamPath::handlePathGetCommand(), UClientFuncPath::handlePathPolygonData(), UFunctionLaser::handleSetCommand(), UClientFuncLaser::handleWpc(), UFuncLinefinder::houghTest(), UAvoidPath2::insertNewPointAfter(), UAvoidPath2::invalidateEmbeddedVertices(), UAvoidPath2::invalidateObstaclesAtStartAndExit(), UAvoidPath2::isAfterDestination(), UAvoidPath2::isCrossingManPoly(), UPolygon::isEmbedded(), UAvoidPath2::isInClosedSet(), UPolygon::isInsideConvex(), UAvoidPath2::isInsidePoly(), UPolygon::isOverlappingXYconvex(), UPolygon::isOverlappingXYconvex2(), UPolygon::isOverlappingXYconvexSeg(), UPolygon::isOverlappingXYtype(), Funcavoid::korrigerHuller(), UImage::lineInGrid(), UResLaserIfObst::listGroup(), load(), LoadFromReg(), UResLocater::locate(), UGrid3D::logBB(), UImageLog::logImage(), UObstacle::logObst(), UAvoidPath2::logObstacleGroups(), UAvoidPath2::logPathSequences(), UPaintBase::makeCircleCloud(), UFuncBall::makeDisplayPolygon(), UFuncImSeg::makeDisplayPolygon(), UResLobst::makeObst(), UResPassable::makePassableIntervals2(), UImg3Dpoints::makePCLFile(), UPaintPolygon::makePolyCloud(), UProbGrid::makeProbGrid(), UPaintLaserScan::makeScanCloud(), UObstacle::mergeableOnCogLimits(), UResPcp::methodCallV(), UResPoly::methodCallV(), UNavView::moveViewPose(), U2Dpos::operator=(), operator=(), UComCamSml::pack(), UNavView::paint(), UNavPaint::paintCams(), UCalibrationComponents::PaintChartInImage(), UNavPaint::paintFeatures(), UNavView::paintFeatures(), UNavPaint::paintFreePoly(), UClientFuncLaserGui::paintFreePoly(), UNavView::paintFreePoly(), UNavPaint::paintGmks(), UResObj3d::paintGndPlane(), UNavPaint::paintHistScan(), UClientFuncLaserGui::paintHistScan(), UClientFuncLaserGui::paintLineSegments(), UNavPaint::paintManData(), UNavPaint::paintObstGrp(), UClientFuncLaserGui::paintObstGrp(), UNavView::paintObstGrp(), UClientFuncLaserGui::paintOdoData(), UNavPaint::paintOdoGrid(), UClientFuncLaserGui::paintOdoGrid(), UNavView::paintOdoGrid(), UClientFuncLaserGui::paintPathData(), UNavPaint::paintPis(), UClientFuncLaserGui::paintPis(), UNavView::paintPis(), UNavPaint::paintPoseHistLine(), UClientFuncLaserGui::paintPostHistLine(), UClientFuncLaserGui::paintRangeRings(), UNavPaint::paintRoadLine(), UProbPoly::paintRobot(), UImgPolyProj::paintRobot(), UImgProj::paintRobot(), UImgPolyProj::paintRobotPath(), UImgProj::paintRobotPath(), UNavPaint::paintScanNewest(), UNavPaint::paintScanStatData(), UClientFuncLaserGui::paintScanStatData(), UGrid3D::paintVoxels(), UClientFuncLaserGui::paintWpListData(), UAvoidObst::passingNoGoEdge2(), UObstaclePool::pointsToPolygon(), UObstacle::print(), UPosRot::print(), ULaserPi::print(), UObstInfo::print(), ULineMatch::print(), println(), UAvoidCellGraph::printObstacleList(), UAvoidPoint::printPoses(), UFuncLocalize::projectToLaser(), UFuncLoca2::projectToLaser(), UAvoidPath2::removeEqualXvertices(), UPolygon::removeNearVertex(), UFuncPpl::robotToOdoCooTransf(), UFuncImu::run(), UFuncView::run(), UFuncKinect::run(), save(), SaveToReg(), scale(), scaled(), UFunctionAvoid::sendCurrentAvoidPath(), UFunctionPassable::sendFullScan(), USmlTag::sendProbPoly(), UPlane::set(), UPose::set(), ULine::set2D(), UVariable::set3D(), UVariable::set6D(), UAvoidPoint::setAngNext(), UAvoidLink::setBadEdgeAndVertex(), UCalibrationMarkSet::setBarcodePosRowSet(), UNamedBox::setBox(), UComCamSml::setCamDevice(), UCalibrationMarkSet::setCameraPosRowSet(), UImgPolyProj::setCamPos(), UImgProj::setCamPos(), UAvoidPath2::setClosestPoint(), UNamedBox::setCornersAsPolygon(), UCamParEst::setEstMatrix(), UCamParEst::setEstMatrix35(), ULine::setFromPoints(), ULineSegment::setFromPoints(), U2Dseg::setFromPoints(), URotation::setFromYZ(), URoadLine::setNew(), UAvoidNoVis::setNoVisSegment(), UCamMounted::setPosOnRobotCC(), UAvoidPath2::setPreStopPoint(), UReactivePath::setRouteStats(), UAvoidPath2::setTurnCentre(), ULaserScan::setVariance(), UGrid3D::setVoxel(), show(), UResCamIfGmk::snprint(), UPolygon::snprint(), ULine::snprint(), snprint(), UPolygon::sortByAngleXYTo(), UPolygon40::sortByAngleXYTo(), UPolygon400::sortByAngleXYTo(), UAvoidPath2::splitObstGroup(), sprint(), subtract(), subtracted(), UAvoidPath2::terminateWorseCandidatesInOpenSet(), test3DtoPix(), testCamPar(), UReactivePath::testForObstacleNearExit(), UAvoidPath2::testNoVisSegVisibility(), testPlane(), testPolyDistance(), testPolygonCog(), UNavPaint::toPixels(), UFuncStraightLine::toRobot(), UFuncStraightLine::toWorld(), UAvoidPoint::tPointVisible(), ULine::turn90degXY(), UAvoidPath2::turnCentreCrossing(), ULine::ULine(), UV360Scan::update(), URoadLine::update(), UResV360::update(), UResRoadLine::updateRoadVariables(), UPose::UPose(), UAvoidPath2::validNewClosePoint(), UAvoidLink::visibilityTest(), and UAvoidParams::withinRobotOutline().

double UPosition::y

Referenced by add(), UResPcp::add(), UPaintBase::addBox(), added(), UResObj3d::addGroundObjects(), UAvoidPath2::addNoVisLinesAsObstacles(), UObstacleGroupLaser::addObstPoly(), UPaintPolygon::addPolyToViewer(), UGridBBox::addPosToProfile(), UGridBBox::addPosToSideView(), UAvoidObst::addtangentLines(), ULineMatch::addToPoly(), asMatrix4x4(), asVector3(), asVector4(), UAvoidPath2::avoidToTheLeft(), UFuncBallKL::calculateBallPosition(), UFuncBall::calculateBallPosition(), UFuncBall_improved::calculateBallPosition(), ULaserMeasurement::clear(), clientCmdLine(), UPcpItem::codeXML(), codeXml(), UResAvoid::codeXmlRobotFront(), UResPassable::combineNearIntervals(), UAvoidPath2::convertToManSeq(), copy(), UPolygon::copy(), URotation::copy(), UFakeMap::copyToPoly(), UAvoidPath2::createCellBasedPointList(), UAvoidPath2::createMidPathPoints(), cross(), UAvoidObst::crossingNonVisibilityLine(), UPolygon::cut(), UPolygon::cutPoints(), UCamPool::decodeReplayLine(), Funcavoid::direkte(), U2Dpos::dist(), UPlane::distSigned(), distSq(), distXY(), UNamedBox::doDetect(), UResObj3d::doGndPlaneMask(), UClientFuncLaserGui::doImage(), UImgProj::doProjectPolygonToMapPix(), UImgPolyProj::doProjectPolygonToMapPix(), UClientFuncLaserGui::doRepaint(), dot(), UCalibrationMarkSet::evaluateChartPosRot(), UCalibrationMarkSet::evaluatePosRot(), UAvoidPath2::expandPolyToSegment(), UPolygon::extractConvexTo(), UAvoidCellGraph::fillVerticeList(), UResRoadDrive::findExitPose(), UResObj3d::findPolygonsOnEdge(), UAvoidPath2::findRoutes(), UAvoidPath2::findRouteToDest(), UFuncStraightLine::findWoldPoints(), ULaserPi::get2DDistToCross(), ULaserPi::get2DLine(), UGrid3D::getCellCenter(), UGrid3D::getCellFTL(), UGrid3D::getCellNBR(), UPolygon::getClosestDistance(), UAvoidPath2::getClosestObst2(), UAvoidObst::getClosestPoint(), UPolygon::getCloseVertexCnt(), UPolygon::getCogXY(), UProbPoly::getCrossingsAtX(), UProbPoly::getCrossingsAtY(), UCamPar::getCtoPRob(), ULine::getCylinderCrossings(), UAvoidParams::getDiagonal(), UPose::getDistance(), UAvoidLnkSeq::getDistance(), UPolygon::getDistance(), ULine::getDistanceSq(), UManLine::getDistanceXYSigned(), UManArc::getDistanceXYSigned(), UManSeq::getDistanceXYSigned(), ULineSegment::getDistanceXYSigned(), UAvoidParams::getDistToCorner(), UManPPSeq::getEndDistTo(), UCalibrationMarkSet::geterrorInPixels(), UFakeMap::getFake2range(), UManArc::getFoodprintPolygon(), UPolygon::getLimits(), UPlane::getLineCrossing(), ULaserScan::getLineSegmentFit(), UPolygon::getLowerLeftXY(), UPose::getMapToPose(), UPose::getMapToPosePose(), UReactivePath::getMidPoseHere(), UManLine::getMinDistanceXYSigned(), UManArc::getMinDistanceXYSigned(), UManSeq::getMinDistanceXYSigned(), UAvoidParams::getMinOpening(), UPolygon::getMostDistantVertexXY(), UPolygon::getMostDistantXY(), UReactivePath::getNearObstacle(), UAvoidNoVis::getNoVisSegment(), UAvoidVertexIdx::getPktType2(), UPlane::getPlaneCrossing(), ULine::getPlaneLineCrossing(), UPcpItem::getPoint(), UCamMounted::getPosCC(), UPose::getPoseToMap(), UClientLaserData::getPosition(), ULaserData::getPosition(), USmlTag::getPosition(), ULine::getPositionOnLine(), ULine::getPositionOnLineXY(), UCamPar::getPtoCRob(), UAvoidParams::getSafeDistance(), ULine::getSphereCrossings(), UManPPSeq::getStartDistTo(), UReactivePath::getTangentMidPose(), getTangentPointXY(), UAvoidObst::getTangentVertexFrom(), UManArc::getTurnCentre(), UAvoidParams::getWallWallRadius(), UPolygon::getXYarea(), ULine::getXYCrossing(), ULine::getXYHeading(), ULine::getXYsignedDistance(), UPosRot::getY(), UResCamIfCam::gotNewData(), UAvoidPath2::groupObstacles(), UFuncLocalize::handleAddLine(), UFuncLoca2::handleAddLine(), UClientCamData::handleCamGet(), UFuncBallKL::handleCommand(), UFuncObj3d::handleCommand(), UFunctionCamGmk::handleGmkGetCommand(), UFuncImgPoly::handleImgPolyCommand(), UFuncLocalize::handleLocalize(), UFuncLocalize::handleLocalizeMHF(), UFuncLocalize::handleLocalizeUKF(), UResPcp::handleNewData(), UFunctionPassable::handleObstGet(), UFunctionCamPath::handlePathGetCommand(), UClientFuncPath::handlePathPolygonData(), UFunctionLaser::handleSetCommand(), UClientFuncLaser::handleWpc(), UFuncLinefinder::houghTest(), UAvoidPath2::insertNewPointAfter(), UAvoidPath2::invalidateEmbeddedVertices(), UAvoidPath2::invalidateObstaclesAtStartAndExit(), UAvoidPath2::isAfterDestination(), UAvoidPath2::isCrossingManPoly(), UPolygon::isEmbedded(), UAvoidPath2::isInClosedSet(), UPolygon::isInsideConvex(), UAvoidPath2::isInsidePoly(), UPolygon::isOverlappingXYconvex(), UPolygon::isOverlappingXYconvex2(), UPolygon::isOverlappingXYconvexSeg(), UPolygon::isOverlappingXYtype(), Funcavoid::korrigerHuller(), UImage::lineInGrid(), UResLaserIfObst::listGroup(), load(), LoadFromReg(), UGrid3D::logBB(), UImageLog::logImage(), UObstacle::logObst(), UAvoidPath2::logObstacleGroups(), UAvoidPath2::logPathSequences(), UPaintBase::makeCircleCloud(), UFuncBall::makeDisplayPolygon(), UFuncImSeg::makeDisplayPolygon(), UResLobst::makeObst(), UResPassable::makePassableIntervals2(), UImg3Dpoints::makePCLFile(), UPaintPolygon::makePolyCloud(), UProbGrid::makeProbGrid(), UPaintLaserScan::makeScanCloud(), UObstacle::mergeableOnCogLimits(), UResPcp::methodCallV(), UResPoly::methodCallV(), UNavView::moveViewPose(), U2Dpos::operator=(), operator=(), UComCamSml::pack(), UNavView::paint(), UNavPaint::paintCams(), UCalibrationComponents::PaintChartInImage(), UNavPaint::paintFeatures(), UNavView::paintFeatures(), UNavPaint::paintFreePoly(), UClientFuncLaserGui::paintFreePoly(), UNavView::paintFreePoly(), UNavPaint::paintGmks(), UResObj3d::paintGndPlane(), UNavPaint::paintHistScan(), UClientFuncLaserGui::paintHistScan(), UClientFuncLaserGui::paintLineSegments(), UNavPaint::paintManData(), UNavPaint::paintObstGrp(), UClientFuncLaserGui::paintObstGrp(), UNavView::paintObstGrp(), UClientFuncLaserGui::paintOdoData(), UNavPaint::paintOdoGrid(), UClientFuncLaserGui::paintOdoGrid(), UNavView::paintOdoGrid(), UClientFuncLaserGui::paintPathData(), UNavPaint::paintPis(), UClientFuncLaserGui::paintPis(), UNavView::paintPis(), UNavPaint::paintPoseHistLine(), UClientFuncLaserGui::paintPostHistLine(), UClientFuncLaserGui::paintRangeRings(), UNavPaint::paintRoadLine(), UProbPoly::paintRobot(), UImgPolyProj::paintRobot(), UImgProj::paintRobot(), UImgPolyProj::paintRobotPath(), UImgProj::paintRobotPath(), UNavPaint::paintScanNewest(), UNavPaint::paintScanStatData(), UClientFuncLaserGui::paintScanStatData(), UGrid3D::paintVoxels(), UClientFuncLaserGui::paintWpListData(), UAvoidObst::passingNoGoEdge2(), UObstacle::print(), UPosRot::print(), ULaserPi::print(), UObstInfo::print(), ULineMatch::print(), println(), UAvoidCellGraph::printObstacleList(), UAvoidPoint::printPoses(), UFuncLocalize::projectToLaser(), UFuncLoca2::projectToLaser(), UPolygon::removeNearVertex(), UFuncPpl::robotToOdoCooTransf(), UFuncImu::run(), UFuncView::run(), UFuncKinect::run(), save(), SaveToReg(), scale(), scaled(), UFunctionAvoid::sendCurrentAvoidPath(), UFunctionPassable::sendFullScan(), USmlTag::sendProbPoly(), UPlane::set(), UPose::set(), ULine::set2D(), UVariable::set3D(), UVariable::set6D(), UAvoidPoint::setAngNext(), UAvoidLink::setBadEdgeAndVertex(), UCalibrationMarkSet::setBarcodePosRowSet(), UNamedBox::setBox(), UComCamSml::setCamDevice(), UCalibrationMarkSet::setCameraPosRowSet(), UImgProj::setCamPos(), UImgPolyProj::setCamPos(), UAvoidPath2::setClosestPoint(), UNamedBox::setCornersAsPolygon(), UCamParEst::setEstMatrix(), UCamParEst::setEstMatrix35(), ULine::setFromPoints(), ULineSegment::setFromPoints(), U2Dseg::setFromPoints(), URotation::setFromYZ(), ULaserPi::setInterval(), URoadLine::setNew(), UAvoidNoVis::setNoVisSegment(), UCamMounted::setPosOnRobotCC(), UAvoidPath2::setPreStopPoint(), UReactivePath::setRouteStats(), UAvoidPath2::setTurnCentre(), ULaserScan::setVariance(), UGrid3D::setVoxel(), show(), UResCamIfGmk::snprint(), UPolygon::snprint(), ULine::snprint(), snprint(), UPolygon::sortByAngleXYTo(), UPolygon40::sortByAngleXYTo(), UPolygon400::sortByAngleXYTo(), UAvoidPath2::splitObstGroup(), sprint(), subtract(), subtracted(), UAvoidPath2::terminateWorseCandidatesInOpenSet(), test3DtoPix(), UReactivePath::testForObstacleNearExit(), UAvoidPath2::testNoVisSegVisibility(), testPlane(), testPolyDistance(), testPolygonCog(), UNavPaint::toPixels(), UFuncStraightLine::toRobot(), UFuncStraightLine::toWorld(), UAvoidPoint::tPointVisible(), ULine::turn90degXY(), UAvoidPath2::turnCentreCrossing(), ULine::ULine(), UV360Scan::update(), URoadLine::update(), UResV360::update(), UResRoadLine::updateRoadVariables(), UPose::UPose(), UAvoidPath2::validNewClosePoint(), UAvoidLink::visibilityTest(), and UAvoidParams::withinRobotOutline().

double UPosition::z

Referenced by add(), UResPcp::add(), UPaintBase::addBox(), UPaintBase::addCircleCloud(), added(), UResObj3d::addGroundObjects(), UPaintPolygon::addPolyToViewer(), UGridBBox::addPosToProfile(), UGridBBox::addPosToSideView(), asMatrix4x4(), asVector3(), asVector4(), UFuncBallKL::calculateBallPosition(), UFuncBall::calculateBallPosition(), UFuncBall_improved::calculateBallPosition(), clientCmdLine(), UPcpItem::codeXML(), codeXml(), copy(), URotation::copy(), cross(), UPolygon::cut(), UPolygon::cutPoints(), UCamPool::decodeReplayLine(), UPlane::distSigned(), distSq(), UFuncGndAna::do3dGroundPlane(), UResObj3d::do3dGroundPlane(), UClientFuncLaserGui::doImage(), dot(), UCalibrationMarkSet::evaluateChartPosRot(), UCalibrationMarkSet::evaluatePosRot(), UCalibrate::findCameraPosition(), UFuncCropRow::findPlants(), UResObj3d::findPolygonsOnEdge(), UFuncStraightLine::findWoldPoints(), UGrid3D::getCellCenter(), UGrid3D::getCellFTL(), UGrid3D::getCellNBR(), UCamPar::getCtoPRob(), UPolygon::getDistance(), ULine::getDistanceSq(), UManLine::getDistanceXYSigned(), ULineSegment::getDistanceXYSigned(), UCalibrationMarkSet::geterrorInPixels(), UPlane::getLineCrossing(), UPose::getMapToPose(), UManPPSeq::getMinDistanceXYSigned(), UAvoidNoVis::getNoVisSegment(), UImgPolyProj::getPixToRobFloor(), UImgProj::getPixToRobFloor(), UPlane::getPlaneCrossing(), ULine::getPlaneLineCrossing(), UPcpItem::getPoint(), UCamMounted::getPosCC(), UPose::getPoseToMap(), UClientLaserData::getPosition(), ULaserData::getPosition(), USmlTag::getPosition(), ULine::getPositionOnLine(), ULine::getPositionOnLineXY(), UCamPar::getPtoCRob(), ULine::getSphereCrossings(), UAvoidLink::getTangentLine(), UPosRot::getZ(), UResCamIfCam::gotNewData(), UClientCamData::handleCamGet(), UFuncBallKL::handleCommand(), UFuncObj3d::handleCommand(), UFuncCropRow::handleCommand(), UFuncPpl::handleCommand(), UFunctionCamGmk::handleGmkGetCommand(), UFuncImgPoly::handleImgPolyCommand(), UResPcp::handleNewData(), UFunctionCamPath::handlePathGetCommand(), UClientFuncPath::handlePathPolygonData(), UFunctionLaser::handleSetCommand(), UGroundPatches::heightAnalysis(), UImage::lineInGrid(), load(), LoadFromReg(), UGrid3D::logBB(), UImageLog::logImage(), UPaintBase::makeCircleCloud(), UFuncBall::makeDisplayPolygon(), UFuncImSeg::makeDisplayPolygon(), UResPassable::makePassableIntervals2(), UImg3Dpoints::makePCLFile(), UPaintPolygon::makePolyCloud(), UPaintLaserScan::makeScanCloud(), UResPcp::methodCallV(), UResPoly::methodCallV(), UNavView::moveViewPose(), operator=(), UComCamSml::pack(), UNavView::paint(), UCalibrationComponents::PaintChartInImage(), UNavView::paintFeatures(), UNavView::paintFreePoly(), UResObj3d::paintGndPlane(), UNavView::paintObstGrp(), UClientFuncLaserGui::paintPathData(), UNavView::paintPis(), UGrid3D::paintVoxels(), UPosRot::print(), println(), UFuncImu::run(), UFuncView::run(), UFuncKinect::run(), save(), SaveToReg(), scale(), scaled(), UPlane::set(), ULine::set2D(), UVariable::set3D(), UVariable::set6D(), UCalibrationMarkSet::setBarcodePosRowSet(), UComCamSml::setCamDevice(), UCalibrationMarkSet::setCameraPosRowSet(), UImgPolyProj::setCamPos(), UImgProj::setCamPos(), UCamParEst::setEstMatrix(), UCamParEst::setEstMatrix35(), ULine::setFromPoints(), ULineSegment::setFromPoints(), URotation::setFromYZ(), ULaserPi::setInterval(), UCamMounted::setPosOnRobotCC(), show(), ULine::snprint(), snprint(), sprint(), subtract(), subtracted(), test3DtoPix(), UAvoidPath2::testNoVisSegVisibility(), testPlane(), and ULine::ULine().


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