FloatShapeInterval OffsetPolygonEdge::clippedEdgeXRange(float y1,
float y2) const {
if (!overlapsYRange(y1, y2) || (y1 == maxY() && minY() <= y1) ||
(y2 == minY() && maxY() >= y2))
return FloatShapeInterval();
if (isWithinYRange(y1, y2))
return FloatShapeInterval(minX(), maxX());
// Clip the edge line segment to the vertical range y1,y2 and then return
// the clipped line segment's horizontal range.
FloatPoint minYVertex;
FloatPoint maxYVertex;
if (vertex1().y() < vertex2().y()) {
minYVertex = vertex1();
maxYVertex = vertex2();
} else {
minYVertex = vertex2();
maxYVertex = vertex1();
}
float xForY1 = (minYVertex.y() < y1) ? xIntercept(y1) : minYVertex.x();
float xForY2 = (maxYVertex.y() > y2) ? xIntercept(y2) : maxYVertex.x();
return FloatShapeInterval(std::min(xForY1, xForY2), std::max(xForY1, xForY2));
}
void tScetch::setTrs()
{
scaleX=(width()-2*ofs)/(double)(maxX()-minX());
scaleY=(height()-2*ofs)/(double)maxY();
if(scaleX>scaleY) scaleX=scaleY;
else scaleY=scaleX;
}
bool VertexPair::overlapsRect(const FloatRect& rect) const
{
bool boundsOverlap = (minX() < rect.maxX()) && (maxX() > rect.x()) && (minY() < rect.maxY()) && (maxY() > rect.y());
if (!boundsOverlap)
return false;
float leftSideValues[4] = {
leftSide(vertex1(), vertex2(), rect.minXMinYCorner()),
leftSide(vertex1(), vertex2(), rect.maxXMinYCorner()),
leftSide(vertex1(), vertex2(), rect.minXMaxYCorner()),
leftSide(vertex1(), vertex2(), rect.maxXMaxYCorner())
};
int currentLeftSideSign = 0;
for (unsigned i = 0; i < 4; ++i) {
if (!leftSideValues[i])
continue;
int leftSideSign = leftSideValues[i] > 0 ? 1 : -1;
if (!currentLeftSideSign)
currentLeftSideSign = leftSideSign;
else if (currentLeftSideSign != leftSideSign)
return true;
}
return false;
}
// True if this structure has valid values
bool isValid()
{
return
minX() <= maxX() &&
minY() <= maxY() &&
minZ() <= maxZ();
}
void RRTstar_planning(){
ROS_INFO("RRT star planning");
ob::StateSpacePtr space(new ob::RealVectorStateSpace(2));
space->as<ob::RealVectorStateSpace>()->setBounds(minX(local_map->info), maxX(local_map->info));
space->setLongestValidSegmentFraction(0.01/(maxX(local_map->info)-minX(local_map->info)));
ob::SpaceInformationPtr si(new ob::SpaceInformation(space));
si->setStateValidityChecker(ob::StateValidityCheckerPtr(new ValidityChecker(si)));
si->setup();
ob::ScopedState<> start(space);
start->as<ob::RealVectorStateSpace::StateType>()->values[0] = startState[0];
start->as<ob::RealVectorStateSpace::StateType>()->values[1] = startState[1];
ob::ScopedState<> goal(space);
goal->as<ob::RealVectorStateSpace::StateType>()->values[0] = goalState[0];
goal->as<ob::RealVectorStateSpace::StateType>()->values[1] = goalState[1];
ob::ProblemDefinitionPtr pdef(new ob::ProblemDefinition(si));
pdef->setStartAndGoalStates(start, goal);
pdef->setOptimizationObjective(ob::OptimizationObjectivePtr(new ClearanceObjective(si)));
og::RRTstar *plan_pt=new og::RRTstar(si);
plan_pt->setGoalBias(0.05);
plan_pt->setRange(1.0);
ob::PlannerPtr optimizingPlanner(plan_pt);
optimizingPlanner->setProblemDefinition(pdef);
optimizingPlanner->setup();
ob::PlannerStatus solved;
int it=0;
while(solved!=ompl::base::PlannerStatus::StatusType::EXACT_SOLUTION && it<100){
it++;
solved = optimizingPlanner->solve(1.0);
}
if(solved==ompl::base::PlannerStatus::StatusType::EXACT_SOLUTION){
std::vector< ob::State * > sol = boost::static_pointer_cast<og::PathGeometric>(pdef->getSolutionPath())->getStates();
current_path_raw.resize(sol.size());
std::transform(sol.begin(), sol.end(), current_path_raw.begin(), &obstateState);
/*std::cout << "Path length: " << pdef->getSolutionPath()->length() << std::endl;
std::cout << "Path cost: " << pdef->getSolutionPath()->cost(pdef->getOptimizationObjective()) << std::endl;
std::cout << "Path :" << std::endl;
for(std::deque<State<2>>::iterator it=current_path_raw.begin(),end=current_path_raw.end();it!=end;++it){
std::cout << (*it) << " -- ";
}
std::cout << std::endl;*/
planned=true;
}else{
ROS_INFO("Planning failed");
planned=false;
}
}
bool AABB::overlapping(AABB& other) {
if (maxX() < other.minX() ||
minX() > other.maxX() ||
maxY() < other.minY() ||
minY() > other.maxY()) {
return false;
}
return true;
}
bool Boundary::isValid() const
{
bool valid = std::isfinite (minX ()) &&
std::isfinite (maxX ()) &&
std::isfinite (minY ()) &&
std::isfinite (maxY ()) &&
std::isfinite (minZ ()) &&
std::isfinite (maxZ ());
return valid;
}
/**
* Print some informations concerning the object in a std::string and does a cleanup of the
* memory.
*
* @return A std::string.
**/
std::string stats()
{
std::string s;
s += "*****************************************************\n";
s += "BitGraphZ2 object statistics\n\n";
s += "- memory used : " + toString(memory()) + "Mb\n";
s += "- lattice represented : [ " + toString(minV()) + " , " + toString(maxV()) + " ]^2\n";
s += "- Main grid size : [ " + toString(-LL) + " , " + toString(LL-1) + " ]^2 (" + toString((4*sizeof(int32)*LL*LL)/(1024*1024)) + "Mb)\n";
s += "- Size of a subsquare : " + toString(N*8) + " x " + toString(N*8) + " (" + toString(sizeof(_MSQ)) + "b each)\n";
s += "- Number of subsquare : " + toString(VV) + " (" + toString(((int64)VV)*sizeof(_MSQ) /(1024*1024)) + "Mb)\n\n";
s += "Number of point set : " + toString(nbSet()) + "\n";
s += "Surrounding square : ";
if (minX() != LLONG_MAX) {
s += "[ " + toString(minX()) + " , " + toString(maxX()) + " ] x [ " + toString(minY()) + " , " + toString(maxY()) + " ]\n";
} else {s += "No point set yet !\n";}
s += "Memory used before cleanup\t" + toString(v) + "/" + toString(VV) + " (" + toString((int)(100*(((double)v)/((double)VV))))+ "%)\n";
cleanup();
s += "Memory used after cleanup\t" + toString(v) + "/" + toString(VV) + " (" + toString((int)(100*(((double)v)/((double)VV))))+ "%)\n";
s += "*****************************************************\n";
return s;
}
// initializes the map (clearing any previous obstacles)
// the heading is required because the specific grid cells that obstacles
// occupy can vary
void FovObstacleMap::initialize(Heading robotHeading)
{
this->robotHeading = robotHeading;
// set all points to unknown
for(int x = minX(); x < maxX(); x++) {
for(int y = minY(); y < maxY(); y++) {
CellData& cellData = map.cell(x, y);
cellData.isKnownObstacle = false;
}
}
}
float OffsetPolygonEdge::xIntercept(float y) const
{
ASSERT(y >= minY() && y <= maxY());
if (vertex1().y() == vertex2().y() || vertex1().x() == vertex2().x())
return minX();
if (y == minY())
return vertex1().y() < vertex2().y() ? vertex1().x() : vertex2().x();
if (y == maxY())
return vertex1().y() > vertex2().y() ? vertex1().x() : vertex2().x();
return vertex1().x() + ((y - vertex1().y()) * (vertex2().x() - vertex1().x()) / (vertex2().y() - vertex1().y()));
}
void GeneratedMatrix::save(QXmlStreamWriter &xml) {
xml.writeStartElement("generatedmatrix");
xml.writeAttribute("tag", tag().tagString());
xml.writeAttribute("xmin", QString::number(minX()));
xml.writeAttribute("ymin", QString::number(minY()));
xml.writeAttribute("nx", QString::number(xNumSteps()));
xml.writeAttribute("ny", QString::number(yNumSteps()));
xml.writeAttribute("xstep", QString::number(xStepSize()));
xml.writeAttribute("ystep", QString::number(yStepSize()));
xml.writeAttribute("gradzmin", QString::number(_gradZMin));
xml.writeAttribute("gradzmax", QString::number(_gradZMax));
xml.writeAttribute("xdirection", QVariant(_xDirection).toString());
xml.writeEndElement();
}
QPointF SingleCellViewGraphPanelPlotWidget::canvasPoint(const QPoint &pPoint,
const bool pNeedOffset) const
{
// Return the mouse position using canvas coordinates, making sure that they
// are within our ranges
QPointF realPoint = pPoint;
if (pNeedOffset)
realPoint -= plotLayout()->canvasRect().topLeft();
return QPointF(qMin(maxX(), qMax(minX(), canvasMap(QwtPlot::xBottom).invTransform(realPoint.x()))),
qMin(maxY(), qMax(minY(), canvasMap(QwtPlot::yLeft).invTransform(realPoint.y()))));
}
void QStackedBarSeriesPrivate::initializeDomain()
{
qreal minX(domain()->minX());
qreal minY(domain()->minY());
qreal maxX(domain()->maxX());
qreal maxY(domain()->maxY());
qreal x = categoryCount();
minX = qMin(minX, - (qreal)0.5);
minY = qMin(minY, bottom());
maxX = qMax(maxX, x - (qreal)0.5);
maxY = qMax(maxY, top());
domain()->setRange(minX, maxX, minY, maxY);
}
void GeneratedMatrix::save(QXmlStreamWriter &xml) {
xml.writeStartElement(staticTypeTag);
xml.writeAttribute("xmin", QString::number(minX()));
xml.writeAttribute("ymin", QString::number(minY()));
xml.writeAttribute("nx", QString::number(xNumSteps()));
xml.writeAttribute("ny", QString::number(yNumSteps()));
xml.writeAttribute("xstep", QString::number(xStepSize()));
xml.writeAttribute("ystep", QString::number(yStepSize()));
xml.writeAttribute("gradzmin", QString::number(_gradZMin));
xml.writeAttribute("gradzmax", QString::number(_gradZMax));
xml.writeAttribute("xdirection", QVariant(_xDirection).toString());
saveNameInfo(xml, VECTORNUM|MATRIXNUM|SCALARNUM);
xml.writeEndElement();
}
void Boundary::refer(const Boundary &boundary)
{
if (boundary.minX () < minX ())
setMinX (boundary.minX ());
if (boundary.maxX () > maxX ())
setMaxX (boundary.maxX ());
if (boundary.minY () < minY ())
setMinY (boundary.minY ());
if (boundary.maxY () > maxY ())
setMaxY (boundary.maxY ());
if (boundary.minZ () < minZ ())
setMinZ (boundary.minZ ());
if (boundary.maxZ () > maxZ ())
setMaxZ (boundary.maxZ ());
}
void Boundary::refer(const xjPoint &point)
{
if (point.x () < minX ())
setMinX (point.x ());
if (point.x () > maxX ())
setMaxX (point.x ());
if (point.y () < minY ())
setMinY (point.y ());
if (point.y () > maxY ())
setMaxY (point.y ());
if (point.z () < minZ ())
setMinZ (point.z ());
if (point.z () > maxZ ())
setMaxZ (point.z ());
}
bool SingleCellViewGraphPanelPlotWidget::setAxes(double pMinX, double pMaxX,
double pMinY,double pMaxY,
const bool &pCanReplot)
{
// Keep track of our axes' old values
double oldMinX = minX();
double oldMaxX = maxX();
double oldMinY = minY();
double oldMaxY = maxY();
// Make sure that the given axes' values are fine
checkAxesValues(pMinX, pMaxX, pMinY, pMaxY);
// Update our axes' values, if needed
bool axesValuesChanged = false;
if ((pMinX != oldMinX) || (pMaxX != oldMaxX)) {
setAxis(QwtPlot::xBottom, pMinX, pMaxX);
axesValuesChanged = true;
}
if ((pMinY != oldMinY) || (pMaxY != oldMaxY)) {
setAxis(QwtPlot::yLeft, pMinY, pMaxY);
axesValuesChanged = true;
}
// Update our actions in case the axes' values have changed
if (axesValuesChanged)
updateActions();
// Replot ourselves, if needed and allowed
if (axesValuesChanged && pCanReplot) {
replotNow();
return true;
} else {
return false;
}
}
void EditableMatrix::save(QXmlStreamWriter &xml) {
QByteArray qba(_zSize*sizeof(double), '\0');
QDataStream qds(&qba, QIODevice::WriteOnly);
for (int i = 0; i < _zSize; i++) {
qds << _z[i];
}
xml.writeStartElement(staticTypeTag);
xml.writeAttribute("xmin", QString::number(minX()));
xml.writeAttribute("ymin", QString::number(minY()));
xml.writeAttribute("nx", QString::number(xNumSteps()));
xml.writeAttribute("ny", QString::number(yNumSteps()));
xml.writeAttribute("xstep", QString::number(xStepSize()));
xml.writeAttribute("ystep", QString::number(yStepSize()));
xml.writeTextElement("data", qCompress(qba).toBase64());
saveNameInfo(xml, VNUM|MNUM|XNUM);
xml.writeEndElement();
}
void SingleCellViewGraphPanelPlotWidget::scaleAxes(const QPoint &pPoint,
const double &pScalingFactorX,
const double &pScalingFactorY)
{
// Rescale our X axis, but only if zooming in/out is possible on that axis
QPointF originPoint = canvasPoint(pPoint);
double newMinX = minX();
double newMaxX = maxX();
double newMinY = minY();
double newMaxY = maxY();
bool scaledAxisX = scaleAxis(pScalingFactorX, mCanZoomInX, mCanZoomOutX,
originPoint.x(), newMinX, newMaxX);
bool scaledAxisY = scaleAxis(pScalingFactorY, mCanZoomInY, mCanZoomOutY,
originPoint.y(), newMinY, newMaxY);
// Note: we want to make both calls to scaleAxis(), hence they are not part
// of the if() statement below...
if (scaledAxisX || scaledAxisY)
setAxes(newMinX, newMaxX, newMinY, newMaxY);
}
void GraphPanelWidgetCustomAxesWindow::on_buttonBox_accepted()
{
// Check that the values make sense
bool xProblem = minX() >= maxX();
bool yProblem = minY() >= maxY();
if (xProblem && yProblem) {
QMessageBox::warning(this, tr("Custom Axes"),
tr("X-min and Y-min must be lower than X-max and Y-max, respectively."));
} else if (xProblem) {
QMessageBox::warning(this, tr("Custom Axes"),
tr("X-min must be lower than X-max."));
} else if (yProblem) {
QMessageBox::warning(this, tr("Custom Axes"),
tr("Y-min must be lower than Y-max."));
} else {
// Confirm that we accepted the changes
accept();
}
}
void DataMatrix::save(QXmlStreamWriter &xml) {
if (file()) {
xml.writeStartElement(staticTypeTag);
saveFilename(xml);
xml.writeAttribute("field", _field);
xml.writeAttribute("reqxstart", QString::number(_reqXStart));
xml.writeAttribute("reqystart", QString::number(_reqYStart));
xml.writeAttribute("reqnx", QString::number(_reqNX));
xml.writeAttribute("reqny", QString::number(_reqNY));
xml.writeAttribute("doave", QVariant(_doAve).toString());
xml.writeAttribute("doskip", QVariant(_doSkip).toString());
xml.writeAttribute("skip", QString::number(_skip));
xml.writeAttribute("xmin", QString::number(minX()));
xml.writeAttribute("ymin", QString::number(minY()));
xml.writeAttribute("xstep", QString::number(xStepSize()));
xml.writeAttribute("ystep", QString::number(yStepSize()));
saveNameInfo(xml, VNUM|MNUM|XNUM);
xml.writeEndElement();
}
}
void SingleCellViewGraphPanelPlotWidget::updateActions()
{
// Update our actions
double crtMinX = minX();
double crtMaxX = maxX();
double crtMinY = minY();
double crtMaxY = maxY();
double crtRangeX = crtMaxX-crtMinX;
double crtRangeY = crtMaxY-crtMinY;
mCanZoomInX = crtRangeX > MinAxisRange;
mCanZoomOutX = crtRangeX < MaxAxisRange;
mCanZoomInY = crtRangeY > MinAxisRange;
mCanZoomOutY = crtRangeY < MaxAxisRange;
// Update the enabled status of our actions
mGui->actionZoomIn->setEnabled(mCanZoomInX || mCanZoomInY);
mGui->actionZoomOut->setEnabled(mCanZoomOutX || mCanZoomOutY);
QRectF dRect = dataRect();
if (dRect == QRectF())
dRect = QRectF(DefMinAxis, DefMinAxis,
DefMaxAxis-DefMinAxis, DefMaxAxis-DefMinAxis);
else
dRect = optimisedRect(dRect);
mGui->actionResetZoom->setEnabled( (crtMinX != dRect.left())
|| (crtMaxX != dRect.left()+dRect.width())
|| (crtMinY != dRect.top())
|| (crtMaxY != dRect.top()+dRect.height()));
}
// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
cPoint
cRect::bottomLeft() const
{
return cPoint(minX(), maxY());
}
void SingleCellViewGraphPanelPlotWidget::mouseMoveEvent(QMouseEvent *pEvent)
{
// Default handling of the event
QwtPlot::mouseMoveEvent(pEvent);
// Carry out the action
switch (mAction) {
case Pan: {
// Determine the X/Y shifts for our panning
QPointF origPoint = canvasPoint(mPoint);
QPointF crtPoint = canvasPoint(pEvent->pos());
double shiftX = crtPoint.x()-origPoint.x();
double shiftY = crtPoint.y()-origPoint.y();
mPoint = pEvent->pos();
// Set our axes' new values
setAxes(minX()-shiftX, maxX()-shiftX, minY()-shiftY, maxY()-shiftY);
break;
}
case ShowCoordinates:
// Update the point of our overlay widget
mOverlayWidget->setPoint(pEvent->pos());
break;
case Zoom: {
// Determine our X/Y delta values
QPointF origPoint = canvasPoint(mPoint);
QPointF crtPoint = canvasPoint(pEvent->pos());
double deltaX = crtPoint.x()-origPoint.x();
double deltaY = crtPoint.y()-origPoint.y();
mPoint = pEvent->pos();
// Rescale ourselves
// Note: this will automatically replot ourselves...
scaleAxes(mOriginPoint,
deltaX?
(deltaX > 0)?
ScalingInFactor:
ScalingOutFactor:
NoScalingFactor,
deltaY?
(deltaY < 0)?
ScalingInFactor:
ScalingOutFactor:
NoScalingFactor);
break;
}
case ZoomRegion:
// Update our zoom region by updating the point of our overlay widget
mOverlayWidget->setPoint(pEvent->pos());
break;
default:
// None
;
}
// The mouse has moved, so we definitely won't need to show our context menu
mNeedContextMenu = false;
}
void InteriorPointsConstructor::ForceDirectedAlgorithm2()
{
//Clone the data structure, and initialize it to 0;
int n = _contourArray->length()/2;
computeNumberVertices();
_interiorDisplacementArray = new AcArray<AcGePoint3d>[n+1];
int i,j, vIter;
//int i,j,vIter;
AcGePoint3d delta;
//int vIter;
double Volume = (maxX() - minX())*(maxY() - minY())*(maxZ() - minZ());
double surface = (maxX() - minX())*(maxY() - minY());
double kappa;
if (Volume)
kappa = sqrt(Volume/_numVertices);
else kappa = sqrt(surface/_numVertices);
setKappa(kappa);
//acutPrintf(_T("Proceeding...\n"));
vector<pair<int, int>> v;
vector<pair<int, int>>::iterator it;
for (vIter = 0; vIter<2; vIter++)
{
//Calculate repulsive forces
for (i = 0; i < _contourArray->length()/2+1; i++)
{
for (j = 0; j < _interiorArray[i].length(); j++)
{
if (vIter == 0)
{
AcGePoint3d * newPoint = new AcGePoint3d;
newPoint->x = 0.0;
newPoint->y = 0.0;
newPoint->z = 0.0;
_interiorDisplacementArray[i].append(*newPoint);
}
else
{
_interiorDisplacementArray[i][j].x = 0.0;
_interiorDisplacementArray[i][j].y = 0.0;
_interiorDisplacementArray[i][j].z = 0.0;
}
}
}
//acutPrintf(_T("Proceeding...attractive ...\n"));
//Calculate attractive forces;
for (i = 0; i < _contourArray->length()/2; i++)
{
for (j = 0; j < _interiorArray[i].length(); j++)
{
if ((i != 0) && (j!=_interiorArray[i].length() - 1) && (j!=0) && (i!=_contourArray->length()/2))
{
arrayNeighbour(v, i, j);
for (it = v.begin(); it!=v.end(); it++)
{
delta.x = _interiorArray[(*it).first][(*it).second].x - _interiorArray[i][j].x;
delta.y = _interiorArray[(*it).first][(*it).second].y - _interiorArray[i][j].y;
delta.z = _interiorArray[(*it).first][(*it).second].z - _interiorArray[i][j].z;
_interiorDisplacementArray[i][j].x = _interiorDisplacementArray[i][j].x + F_attractive(delta.x);
_interiorDisplacementArray[i][j].y = _interiorDisplacementArray[i][j].y + F_attractive(delta.y);
_interiorDisplacementArray[i][j].z = _interiorDisplacementArray[i][j].z + F_attractive(delta.z);
}
}
}
}
//Limit the fluctuations first step:
for (i = 0; i < _contourArray->length()/2+1; i++)
{
for (j = 0; j < _interiorArray[i].length(); j++)
{
_interiorArray[i][j].x = _interiorArray[i][j].x + _interiorDisplacementArray[i][j].x;
_interiorArray[i][j].y = _interiorArray[i][j].y + _interiorDisplacementArray[i][j].y;
_interiorArray[i][j].z = _interiorArray[i][j].z + _interiorDisplacementArray[i][j].z;
}
}
}
acutPrintf(_T("\nVolume: %f, numPoints: %d, kappa: %f\n"), Volume, _numVertices, _kappa);
}
void SingleCellViewGraphPanelPlotWidget::drawCurveSegment
(
QSharedPointer<SingleCellViewGraphPanelPlotCurve> pCurve,
const qulonglong &pFrom,
const qulonglong &pTo)
{
// Make sure that we have a curve segment to draw
if (pFrom == pTo)
return;
// Reset our local axes and replot ourselves, if it is our first curve
// segment, or carry on as normal
if (!pFrom) {
// It is our first curve segment, so check our local axes
// Note: we always want to replot, hence our passing false as an
// argument to resetLocalAxes()...
resetLocalAxes(false);
replotNow();
} else {
// It's not our first curve segment, so determine the minimum/maximum
// X/Y values of our new data
double xMin = 0.0;
double xMax = 0.0;
double yMin = 0.0;
double yMax = 0.0;
for (qulonglong i = pFrom; i <= pTo; ++i)
if (i == pFrom) {
xMin = xMax = pCurve->data()->sample(i).x();
yMin = yMax = pCurve->data()->sample(i).y();
} else {
double xVal = pCurve->data()->sample(i).x();
double yVal = pCurve->data()->sample(i).y();
xMin = qMin(xMin, xVal);
xMax = qMax(xMax, xVal);
yMin = qMin(yMin, yVal);
yMax = qMax(yMax, yVal);
}
// Check whether our X/Y axis can handle the minimum/maximum X/Y values
// of our new data
if ( (xMin < minX()) || (xMax > maxX())
|| (yMin < minY()) || (yMax > maxY()))
// Our X/Y axis cannot handle the minimum/maximum X/Y values of our
// new data, so check our local axes
checkLocalAxes(true, true, true);
else
// Our X/Y axis can handle the X/Y min/max of our new data, so just
// draw our new curve segment
mDirectPainter->drawSeries(pCurve.data(), pFrom, pTo);
}
}
void MSPP_planning(){
ROS_INFO("MSPP planning");
if(mapChanged){
//Set Search Space Bounds
State<2> minState;
minState[0]=minX(local_map->info);
minState[1]=minY(local_map->info);
State<2> maxState;
maxState[0]=maxX(local_map->info);
maxState[1]=maxY(local_map->info);
t->setStateBounds(minState,maxState);
//std::cout << "min bound : " << minState <<std::endl;
//std::cout << "max bound : " << maxState <<std::endl;
// Set Tree Max Depth
t->setMaxDepth(depth);
//Depth First Obstacle Creation
//*
std::cout << "Obstacle creation " << std::setw(10) << 0.0 << "\% done.";
Ocount=0;
timerStart=time(NULL);
addObstacles(t->getRootKey(),0,t->getRootKey()[0],t);
std::cout << std::endl;
time_t timerNow=time(NULL);
int seconds = (int)difftime(timerNow,timerStart);
int hours = seconds/3600;
int minutes= (seconds/60)%60;
seconds=seconds%60;
std::cout << "Obstacles created in " << std::setw(4) << hours << ":"
<< std::setw(2) << minutes << ":"
<< std::setw(2) << seconds
<< std::endl;
//*/
mapChanged=false;
}
MSP<2> algo(t);
//Set algo parameters
algo.setNewNeighboorCheck(true);
//algo.setMapLearning(true,nb_obstacle_check,isObstacle);
algo.setSpeedUp(true);
algo.setAlpha(2*sqrt(2));
algo.setEpsilon(epsilon);
algo.setLambda1(lambda1);
//algo.setMinRGcalc(true);
bool initAlgo=algo.init(startState,goalState);
std::cout << "start : " << startState <<std::endl;
std::cout << "goal : " << goalState <<std::endl;
std::cout << "init : " << initAlgo <<std::endl;
//Run algo
if(initAlgo && algo.run()){
ROS_INFO_STREAM("Algo init "<<initAlgo<<", planning in progress ...");
current_path_raw=algo.getPath();
/*std::cout << "Path length: " << current_path_raw.size() << std::endl;
std::cout << "Path cost: " << algo.getPathCost() << std::endl;
std::cout << "Path :" << std::endl;
for(std::deque<State<2>>::iterator it=current_path_raw.begin(),end=current_path_raw.end();it!=end;++it){
std::cout << (*it) << " -- ";
}
std::cout << std::endl;*/
current_path_raw.push_back(goalState);
planned=true;
}else{
ROS_INFO("Planning failed");
planned=false;
}
algo.clear();
}
void SingleCellViewGraphPanelPlotWidget::mouseMoveEvent(QMouseEvent *pEvent)
{
// Default handling of the event
QwtPlot::mouseMoveEvent(pEvent);
// Check that interaction is allowed
if (!mInteractive)
return;
// Retrieve the current point
QPointF currentPoint = mousePositionWithinCanvas(pEvent->pos());
// Carry out the action
switch (mAction) {
case Pan: {
// Determine the X/Y shifts for the panning
double shiftX = currentPoint.x()-mOriginPoint.x();
double shiftY = currentPoint.y()-mOriginPoint.y();
// Determine our new local minimum/maximum values for our axes
double newLocalMinX = localMinX()-shiftX;
double newLocalMaxX = localMaxX()-shiftX;
double newLocalMinY = localMinY()-shiftY;
double newLocalMaxY = localMaxY()-shiftY;
// Make sure that our new local minimum/maximum values for our axes
// are within our local minimum/maximum values
if (newLocalMinX < minX()) {
newLocalMinX = minX();
newLocalMaxX = newLocalMinX+localMaxX()-localMinX();
} else if (newLocalMaxX > maxX()) {
newLocalMaxX = maxX();
newLocalMinX = newLocalMaxX-localMaxX()+localMinX();
}
if (newLocalMinY < minY()) {
newLocalMinY = minY();
newLocalMaxY = newLocalMinY+localMaxY()-localMinY();
} else if (newLocalMaxY > maxY()) {
newLocalMaxY = maxY();
newLocalMinY = newLocalMaxY-localMaxY()+localMinY();
}
// Set our new local minimum/maximum values for our local axes which
// will replot ourselves as a result
setLocalAxes(newLocalMinX, newLocalMaxX, newLocalMinY, newLocalMaxY);
break;
}
case ShowCoordinates:
// Show the coordinates by simply replotting ourselves
replotNow();
break;
case Zoom: {
// Rescale ourselves (which will replot ourselves as a result)
double deltaX = currentPoint.x()-mOriginPoint.x();
double deltaY = currentPoint.y()-mOriginPoint.y();
scaleLocalAxes(deltaX?
(deltaX > 0)?
ScalingInFactor:
ScalingOutFactor:
NoScalingFactor,
deltaY?
(deltaY < 0)?
ScalingInFactor:
ScalingOutFactor:
NoScalingFactor);
break;
}
case ZoomRegion:
// Draw our zoom region by updating our end point and then replotting
// ourselves
mEndPoint = mousePositionWithinCanvas(pEvent->pos());
replotNow();
break;
default:
// None
;
}
// Reset our point of origin, but only if we are doing something and it's
// not zooming a region
if ((mAction != None) && (mAction != ZoomRegion))
mOriginPoint = mousePositionWithinCanvas(pEvent->pos());
}
// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
Vector2
Rectangle::bottomLeft() const
{
return Vector2(minX(), maxY());
}
