inline DistEstimate RefEstimator::estimate(const ReadPair & p) const {
DistEstimate d;
if(p.strand() == BOTH){
DistEstimate d1 = estimate_(p, pblocks_);
DistEstimate d2 = estimate_(p, mblocks_);
if(!d1.fail && !d2.fail) d.fail = true;
else if(!d1.fail) d = d1;
else d = d2;
}else if(p.strand() == MINUS){
d = estimate_(p, mblocks_);
}else if(p.strand() == PLUS){
d = estimate_(p, pblocks_);
}
return d;
}
void QUEST::do_compute(const datatype::Time& t) {
if(t <= this->last_update_) return; //既に別のブロック経由で更新済みなら再計算しない
util::Trace trace(util::Trace::kControlBlock, name_);
//センサから取得した衛星基準座標系における地球，太陽方向
datatype::StaticVector<2> w_sun = this->source<0, datatype::StaticVector<2>>().get_value(t);
datatype::StaticVector<2> w_earth = this->source<1, datatype::StaticVector<2>>().get_value(t);
//軌道情報をもとに計算された衛星位置における地球，太陽方向
datatype::StaticVector<3> v1 = datatype::OrbitCalc::getSunDirection3D(this->source<3, datatype::DateTime>().get_value(t));
datatype::StaticVector<3> v2 = datatype::OrbitCalc::getEarthDirection3D(this->source<2, datatype::PositionInfo>().get_value(t));
datatype::StaticVector<3> w1 = datatype::TypeConverter::toRectangular(w_sun);
datatype::StaticVector<3> w2 = datatype::TypeConverter::toRectangular(w_earth);
this->value_ = estimate_(v1, v2, w1, w2);
this->last_update_ = t;
}
std::vector<int> Pathfinder::getPathFrom(int start) const
{
if (goal_(start)) return {start};
// Record shortest path costs for every node we examine.
std::unordered_map<int, AstarNodePtr> nodes;
// Maintain a heap of nodes to consider.
std::vector<int> open;
int goalLoc = -1;
AstarNodePtr goalNode;
// The heap functions confusingly use operator< to build a heap with the
// *largest* element on top. We want to get the node with the *least* cost,
// so we have to order nodes in the opposite way.
auto orderByCost = [&] (int lhs, int rhs)
{
return nodes[lhs]->estTotalCost > nodes[rhs]->estTotalCost;
};
nodes.emplace(start, make_astar_node(-1, 0, 0));
open.push_back(start);
// A* algorithm. Decays to Dijkstra's if estimate function is always 0.
while (!open.empty()) {
auto loc = open.front();
pop_heap(std::begin(open), std::end(open), orderByCost);
open.pop_back();
if (goal_(loc)) {
goalLoc = loc;
goalNode = nodes[loc];
break;
}
auto &curNode = nodes[loc];
curNode->visited = true;
for (auto n : neighbors_(loc)) {
auto nIter = nodes.find(n);
auto step = stepCost_(loc, n);
if (nIter != nodes.end()) {
auto &nNode = nIter->second;
if (nNode->visited) {
continue;
}
// Are we on a shorter path to the neighbor node than what
// we've already seen? If so, update the neighbor's node data.
if (curNode->costSoFar + step < nNode->costSoFar) {
nNode->prev = loc;
nNode->costSoFar = curNode->costSoFar + step;
nNode->estTotalCost = nNode->costSoFar + estimate_(n);
make_heap(std::begin(open), std::end(open), orderByCost);
}
}
else {
// We haven't seen this node before. Add it to the open list.
nodes.emplace(n, make_astar_node(loc, curNode->costSoFar + step,
curNode->costSoFar + step + estimate_(n)));
open.push_back(n);
push_heap(std::begin(open), std::end(open), orderByCost);
}
}
}
if (!goalNode) {
return {};
}
// Build the path from the chain of nodes leading to the goal.
std::vector<int> path = {goalLoc};
auto n = goalNode;
while (n->prev != -1) {
path.push_back(n->prev);
n = nodes[n->prev];
}
reverse(std::begin(path), std::end(path));
assert(contains(path, start));
return path;
}
