int main(){ const long treeRoot = 1; long n; scanf("%ld\n", &n); std::vector<std::vector<long> > edges(n + 1); for(long p = 1; p < n; p++){ long u, v; scanf("%ld %ld\n", &u, &v); edges[u].push_back(v); edges[v].push_back(u); } std::map<long, std::vector<long> > tree = makeTreeFromEdges(edges, 1); std::vector<int> initialState(n + 1, 0); for(long p = 1; p <= n; p++){scanf("%d", &initialState[p]);} std::vector<int> targetState(n + 1, 0); for(long p = 1; p <= n; p++){scanf("%d", &targetState[p]);} //for(long p = 1; p <= n; p++){printf("*%d\t", targetState[p]);}; puts(""); long total = countFlips(treeRoot, 0, 0, 0, tree, initialState, targetState); /* for(std::map<long, std::vector<long> >::iterator mapIter = tree.begin(); mapIter != tree.end(); mapIter++){ long parent = mapIter->first; std::vector<long> sons = mapIter->second; for(int p = 0; p < sons.size(); p++){printf("%ld --> %ld\n", parent, sons[p]);} } */ printf("%ld\n", flippedNodes.size()); for(long p = 0; p < flippedNodes.size(); p++){printf("%ld\n", flippedNodes[p]);} return 0; }
GTEST_TEST(DownhillSimplex, bench4) { std::array<Rangef, 4> ranges; ranges[0] = Rangef(-50.f, 50); ranges[1] = Rangef(-50.f, 50); ranges[2] = Rangef(0, 1); ranges[3] = Rangef(0, 1); float timeToStepTarget = 0.3f; float timeAfterStepTarget = 0.3f; auto errorFunction = [timeToStepTarget, timeAfterStepTarget] (const Vector4f& vals) { LIP currentState(250.f); LIP targetState(250.f); float footTrans = -50.f; float posWeight = 1; float velWeight = 50; float timeWeight = 500; const float currentZmp = vals(0); const float nextZmp = vals(1); const float timeToStep = vals(2); const float timeAfterStep = vals(3); LIP forwardedState = currentState.predict(timeToStep, currentZmp); forwardedState.position += footTrans; forwardedState.update(timeAfterStep, nextZmp); return posWeight * sqr(targetState.position - forwardedState.position) + velWeight * sqr(targetState.velocity - forwardedState.velocity) + timeWeight * sqr(timeToStepTarget - timeToStep) + timeWeight * sqr(timeAfterStepTarget - timeAfterStep); }; auto optimizer = Optimizer::makeDownhillSimplexOptimizer(errorFunction, ranges); Eigen::BenchTimer optTimer; for(int i = 0; i < TRIES; ++i) { optimizer.initDelta(Vector4f(-1.f, 0.f, timeToStepTarget, timeAfterStepTarget), Vector4f(10.f, 10.f, 0.1f, 0.1f)); // waste some cycles to get the return type automatically auto res = optimizer.optimize(0.0001f, 1); optTimer.start(); for(int j = 0; j < RUNS; ++j) { res = optimizer.optimize(0.0001f, 1000); } optTimer.stop(); } PRINTF("overall - best: %.3fs, avg: %.3fs, worst: %.3fs \n", optTimer.best(), optTimer.total() / TRIES, optTimer.worst()); PRINTF("per run - best: %.6fs, avg: %.6fs, worst: %.6fs \n", optTimer.best() / RUNS, optTimer.total() / (TRIES * RUNS), optTimer.worst() / RUNS); }
int QAbstractTransition::qt_metacall(QMetaObject::Call _c, int _id, void **_a) { _id = QObject::qt_metacall(_c, _id, _a); if (_id < 0) return _id; if (_c == QMetaObject::InvokeMetaMethod) { if (_id < 1) qt_static_metacall(this, _c, _id, _a); _id -= 1; } #ifndef QT_NO_PROPERTIES else if (_c == QMetaObject::ReadProperty) { void *_v = _a[0]; switch (_id) { case 0: *reinterpret_cast< QState**>(_v) = sourceState(); break; case 1: *reinterpret_cast< QAbstractState**>(_v) = targetState(); break; case 2: *reinterpret_cast< QList<QAbstractState*>*>(_v) = targetStates(); break; } _id -= 3; } else if (_c == QMetaObject::WriteProperty) { void *_v = _a[0]; switch (_id) { case 1: setTargetState(*reinterpret_cast< QAbstractState**>(_v)); break; case 2: setTargetStates(*reinterpret_cast< QList<QAbstractState*>*>(_v)); break; } _id -= 3; } else if (_c == QMetaObject::ResetProperty) { _id -= 3; } else if (_c == QMetaObject::QueryPropertyDesignable) { _id -= 3; } else if (_c == QMetaObject::QueryPropertyScriptable) { _id -= 3; } else if (_c == QMetaObject::QueryPropertyStored) { _id -= 3; } else if (_c == QMetaObject::QueryPropertyEditable) { _id -= 3; } else if (_c == QMetaObject::QueryPropertyUser) { _id -= 3; } #endif // QT_NO_PROPERTIES return _id; }
std::pair<std::vector<StateOfCar>, Seed> AStarSeed::findPathToTargetWithAstar(const cv::Mat& img,const State& start,const State& goal) { // USE : for garanteed termination of planner int no_of_iterations = 0; fusionMap = img; MAP_MAX_ROWS = img.rows; MAP_MAX_COLS = img.cols; if(DT==1) distanceTransform(); if (DEBUG) { image = fusionMap.clone(); } StateOfCar startState(start), targetState(goal); std::map<StateOfCar, open_map_element> openMap; std::map<StateOfCar,StateOfCar, comparatorMapState> came_from; SS::PriorityQueue<StateOfCar> openSet; openSet.push(startState); if (startState.isCloseTo(targetState)) { std::cout<<"Bot is On Target"<<std::endl; return std::make_pair(std::vector<StateOfCar>(), Seed()); } else if(isOnTheObstacle(startState)){ std::cout<<"Bot is on the Obstacle Map \n"; return std::make_pair(std::vector<StateOfCar>(), Seed()); } else if(isOnTheObstacle(targetState)){ std::cout<<"Target is on the Obstacle Map \n"; return std::make_pair(std::vector<StateOfCar>(), Seed()); } while (!openSet.empty() && ros::ok()) { // std::cout<<"openSet size : "<<openSet.size()<<"\n"; if(no_of_iterations > MAX_ITERATIONS){ std::cerr<<"Overflow : openlist size : "<<openSet.size()<<"\n"; return std::make_pair(std::vector<StateOfCar>(), Seed()); } StateOfCar currentState=openSet.top(); if (openMap.find(currentState)!=openMap.end() && openMap[currentState].membership == CLOSED) { openSet.pop(); } currentState=openSet.top(); if (DEBUG) { std::cout<<"current x : "<<currentState.x()<<" current y : "<<currentState.y()<<std::endl; plotPointInMap(currentState); cv::imshow("[PLANNER] Map", image); cvWaitKey(0); } // TODO : use closeTo instead of onTarget if (currentState.isCloseTo(targetState)) { std::cout<<"openSet size : "<<openSet.size()<<"\n"; // std::cout<<"Target Reached"<<std::endl; return reconstructPath(currentState, came_from); } openSet.pop(); openMap[currentState].membership = UNASSIGNED; openMap[currentState].cost=-currentState.gCost(); openMap[currentState].membership=CLOSED; std::vector<StateOfCar> neighborsOfCurrentState = neighborNodesWithSeeds(currentState); for (unsigned int iterator=0; iterator < neighborsOfCurrentState.size(); iterator++) { StateOfCar neighbor = neighborsOfCurrentState[iterator]; double tentativeGCostAlongFollowedPath = neighbor.gCost() + currentState.gCost(); double admissible = neighbor.distanceTo(targetState); double consistent = admissible; double intensity = fusionMap.at<uchar>(neighbor.y(), neighbor.x()); double consistentAndIntensity = (neighbor.hCost()*neighbor.hCost()+2 + intensity*intensity)/(neighbor.hCost()+intensity+2); if (!((openMap.find(neighbor) != openMap.end()) && (openMap[neighbor].membership == OPEN))) { came_from[neighbor] = currentState; neighbor.gCost( tentativeGCostAlongFollowedPath) ; if(DT==1) neighbor.hCost(consistentAndIntensity); else neighbor.hCost( consistent) ; neighbor.updateTotalCost(); openSet.push(neighbor); openMap[neighbor].membership = OPEN; openMap[neighbor].cost = neighbor.gCost(); } } no_of_iterations++; } std::cerr<<"NO PATH FOUND"<<std::endl; return std::make_pair(std::vector<StateOfCar>(), Seed()); }