本文整理汇总了C++中ArPose::findAngleTo方法的典型用法代码示例。如果您正苦于以下问题：C++ ArPose::findAngleTo方法的具体用法？C++ ArPose::findAngleTo怎么用？C++ ArPose::findAngleTo使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类ArPose的用法示例。
在下文中一共展示了ArPose::findAngleTo方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: processReadings
AREXPORT void ArForbiddenRangeDevice::processReadings(void)
{
ArPose intersection;
std::list<ArLineSegment *>::iterator it;
lockDevice();
myDataMutex.lock();
myCurrentBuffer.beginRedoBuffer();
if (!myIsEnabled)
{
myCurrentBuffer.endRedoBuffer();
myDataMutex.unlock();
unlockDevice();
return;
}
ArLineSegment *segment;
ArPose start;
double startX;
double startY;
ArPose end;
double angle;
double length;
double gone;
double sin;
double cos;
double atX;
double atY;
double robotX = myRobot->getX();
double robotY = myRobot->getY();
double max = (double) myMaxRange;
double maxSquared = (double) myMaxRange * (double) myMaxRange;
ArTime startingTime;
//startingTime.setToNow();
// now see if the end points of the segments are too close to us
for (it = mySegments.begin(); it != mySegments.end(); it++)
{
segment = (*it);
// if either end point or some perpindicular point is close to us
// add the line's data
if (ArMath::squaredDistanceBetween(
segment->getX1(), segment->getY1(),
myRobot->getX(), myRobot->getY()) < maxSquared ||
ArMath::squaredDistanceBetween(
segment->getX2(), segment->getY2(),
myRobot->getX(), myRobot->getY()) < maxSquared ||
segment->getPerpDist(myRobot->getPose()) < max)
{
start.setPose(segment->getX1(), segment->getY1());
end.setPose(segment->getX2(), segment->getY2());
angle = start.findAngleTo(end);
cos = ArMath::cos(angle);
sin = ArMath::sin(angle);
startX = start.getX();
startY = start.getY();
length = start.findDistanceTo(end);
// first put in the start point if we should
if (ArMath::squaredDistanceBetween(
startX, startY, robotX, robotY) < maxSquared)
myCurrentBuffer.redoReading(start.getX(), start.getY());
// now walk the length of the line and see if we should put the points in
for (gone = 0; gone < length; gone += myDistanceIncrement)
{
atX = startX + gone * cos;
atY = startY + gone * sin;
if (ArMath::squaredDistanceBetween(
atX, atY, robotX, robotY) < maxSquared)
myCurrentBuffer.redoReading(atX, atY);
}
// now check the end point
if (end.squaredFindDistanceTo(myRobot->getPose()) < maxSquared)
myCurrentBuffer.redoReading(end.getX(), end.getY());
}
}
myDataMutex.unlock();
// and we're done
myCurrentBuffer.endRedoBuffer();
unlockDevice();
//printf("%d\n", startingTime.mSecSince());
}开发者ID:eilo，项目名称:Evolucion-Artificial-y-Robotica-Autonoma-en-Robots-Pioneer-P3-DX，代码行数:82，代码来源:ArForbiddenRangeDevice.cpp