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()); }