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