void work()
{
spfa(T);
printf("%d\n",astar());
}
bool PathPlanner::CreatePathToPosition(vec2 TargetPos, std::list<vec2> &path) {
//Flush any current nodes in the path
path.clear();
//Make a note of the target position
vDestPos = TargetPos;
//If the target is unobstructed from the bot's pos, a path does not need to
//be calculated, and the bot can ARRIVE directly at the destination.
//isPathObstructed is a method that takes a start position, a target position,
//and an entity radius and determines if an agent of that size is able
//to move unobstructed between the two positions. It is used here to determine
//if the bot can move directly to the target location without the need
//for planning a path.
//mEnt->fRadius is broken, using hard value of 4.099 (from enemy npc radius)
/*if (!isPathObstructed(pEnt->pos, vDestPos, 4.099)) {
path.push_back(vDestPos);
return true;
}*/
//Find the closest unobstructed node to the bot's position
//GetClosestNodeToPosition is a method that queries the nav graph nodes (NO
//NAV GRAPH ATM) nodes to determine the closest unobstructed node to the given
//position vector. It is used here to find the closest unobstructed node
//to the bot's current location.
int ClosestNodeToBot = GetClosestNodeToPosition(pEnt->pos);
//If no visible node found return failure.
if (ClosestNodeToBot == no_closest_node_found) {
return false;
}
//Find the closest visible unobstructed node to the target position
int ClosestNodeToTarget = GetClosestNodeToPosition(vDestPos);
if (ClosestNodeToTarget == no_closest_node_found) {
return false;
}
//Create an instance of the A* search class to search for a path between
//the closest node to the bot and the closest node to the target position.
//This A* search will utilize the Euclidean straight heuristic
AStar astar(*pWorldInfo, ClosestNodeToBot, ClosestNodeToTarget);
path = astar.GetPathToTarget();
//Convert to indices to vectors
/*if (!pathIndices.empty()){
for (std::list<int>::const_iterator nodeIndex = pathIndices.begin(), end = pathIndices.end(); nodeIndex != end; ++nodeIndex) {
path.push_back(astar.INDEX_NODE_POINTERS[(*nodeIndex)]->vPos);
}
path.push_back(TargetPos);
}
else {
return false;
}*/
path.push_back(vDestPos);
return true;
}
void FloorMapLayer::onTouchEnded(Touch *touch, Event *e)
{
// bugfix: 如果对话框已弹出，就不再执行以下代码，这种情况是在对话框弹出前，touchbegan就已经触发的情况下发生
// 重现方法: 控制勇士经过怪物，然后鼠标按下等待勇士到达怪物处，弹出提示对话框后，松开鼠标继续弹出
if (ModalDialogManager::GetInstance()->isShown())
{
return;
}
// 正在战斗中则不允许重新走动，以免计算混乱
if (_warrior->is_fighting())
{
return;
}
// 获取起始点
auto end_vec2 = _tiled_map->convertTouchToNodeSpace(touch) / 75.0f;
auto end_pt = node_t(end_vec2.x, end_vec2.y);
if (!_paths.empty() && _paths.back() == end_pt)
{
return;
}
auto start_vec2 = _tiled_map->convertToNodeSpace(_warrior->getPosition()) / 75.0f;
auto start_pt = node_t(start_vec2.x, start_vec2.y);
if (start_pt == end_pt)
{
return;
}
// 获取阻碍点
auto wall_layer = _tiled_map->getLayer("wall");
auto wall_layer_size = wall_layer->getLayerSize();
auto pos = touch->getLocation();
auto tiles = wall_layer->getTiles();
std::vector<node_t> blocks;
for (uint32_t i = 0; i < (uint32_t)(wall_layer_size.height); ++i)
{
for (uint32_t j = 0; j < (uint32_t)(wall_layer_size.width); ++j)
{
if (tiles[i * (uint32_t)(wall_layer_size.width) + j] != 0)
{
blocks.push_back(node_t(j, (uint32_t)(wall_layer_size.height) - 1 - i));
}
}
}
if (std::find(blocks.begin(), blocks.end(), end_pt) != blocks.end() || end_pt.x >= 10 || end_pt.y >= 12)
{
return;
}
// npc列表
npc_t stair_up, stair_down;
auto npc_layer = _tiled_map->getLayer("npc");
auto npc_layer_size = npc_layer->getLayerSize();
auto npc_tiles = npc_layer->getTiles();
_npcs.clear();
for (uint32_t i = 0; i < (uint32_t)(npc_layer_size.height); ++i)
{
for (uint32_t j = 0; j < (uint32_t)(npc_layer_size.width); ++j)
{
int32_t gid = (int32_t)(npc_tiles[i * (uint32_t)(npc_layer_size.width) + j]);
if (gid != 0)
{
_npcs.push_back(npc_t(j, npc_layer_size.height - 1 - i, gid));
if (get_tile_prop(gid, "style").asInt() == 6)
{
if (get_tile_prop(gid, "type").asInt() == 1)
stair_up = _npcs.back();
else
stair_down = _npcs.back();
}
}
}
}
// A星路径
AStar astar(10, 12);
astar.set_start_and_end(start_pt, end_pt);
astar.set_blocks(blocks);
_paths = astar.get_path();
auto iter = std::find_if(_paths.begin(), _paths.end(), [&](node_t node) {
// 找到除了首节点的第一个楼梯节点
return (node.x != start_pt.x || node.y != start_pt.y) &&
(node.x == stair_up.x && node.y == stair_up.y || node.x == stair_down.x && node.y == stair_down.y);
});
if (iter != _paths.end())
_paths.erase(++iter, _paths.end());
// 箭头
_arrow_node->setVisible(true);
_arrow_node->setPosition(Vec2((end_pt.x + 0.5f) * 75.0f - 50.0f, (end_pt.y + 0.5f) * 75.0f - 50.0f));
// 路线
uint32_t index = 0;
_road_node->removeAllChildren();
for (auto node : _paths)
{
if (index == _paths.size() - 1)
break;
auto pos = Vec2((node.x + 0.5f) * 75.0f - 50.0f, (node.y + 0.5f) * 75.0f - 50.0f);
auto res = "Images/diban" + String::createWithFormat("%d", index % 22)->_string + ".png";
auto sprite = Sprite::create(res);
sprite->setPosition(pos);
_road_node->addChild(sprite);
++index;
}
// 勇士
_warrior->stopAllActions();
step();
}
void work()
{
dijkstra(T);
printf("%d\n",astar());
}
void online_jps_test()
{
bool check_opt = false;
//bool check_opt = true;
warthog::scenario_manager scenmgr;
scenmgr.load_scenario("orz700d.map.scen");
warthog::gridmap map(scenmgr.get_experiment(0)->map().c_str());
// warthog::gridmap map("CSC2F.map", true);
// map.printdb(std::cerr);
// map.print(std::cerr);
// exit(1);
warthog::jps_expansion_policy expander(&map);
warthog::octile_heuristic heuristic(map.width(), map.height());
warthog::flexible_astar<
warthog::octile_heuristic,
warthog::jps_expansion_policy> astar(&heuristic, &expander);
//astar.set_verbose(true);
for(unsigned int i=0; i < scenmgr.num_experiments(); i++)
{
warthog::experiment* exp = scenmgr.get_experiment(i);
int startid = exp->starty() * exp->mapwidth() + exp->startx();
int goalid = exp->goaly() * exp->mapwidth() + exp->goalx();
double len = astar.get_length(startid, goalid);
if(len == warthog::INF)
{
len = 0;
}
if(!check_opt)
continue;
std::cerr << "exp "<<i<<" ";
exp->print(std::cerr);
std::cerr << " (ne=" << astar.get_nodes_expanded() <<
" ng=" << astar.get_nodes_generated() <<
" nt=" << astar.get_nodes_touched() <<
" st=" << astar.get_search_time() <<")";
std::cerr << std::endl;
std::stringstream stroptlen;
stroptlen << std::fixed << std::setprecision(exp->precision());
stroptlen << exp->distance();
std::stringstream strpathlen;
strpathlen << std::fixed << std::setprecision(exp->precision());
strpathlen << len;
if(stroptlen.str().compare(strpathlen.str()))
{
std::cerr << std::setprecision(6);
std::cerr << "optimality check failed!" << std::endl;
std::cerr << std::endl;
std::cerr << "optimal path length: "<<stroptlen.str()
<<" computed length: ";
std::cerr << strpathlen.str()<<std::endl;
std::cerr << "precision: " << exp->precision()<<std::endl;
exit(1);
}
}
std::cerr << "done. total memory: "<< astar.mem() + scenmgr.mem() << "\n";
}
bool smooth_curve(const BVH *bvh, const VectorXu &E2E, std::vector<CurvePoint> &curve, bool watertight) {
const MatrixXu &F = *bvh->F();
const MatrixXf &V = *bvh->V(), &N = *bvh->N();
cout << endl;
std::vector<CurvePoint> curve_new;
std::vector<Float> weight;
std::vector<uint32_t> path;
cout << "Input: " << curve.size() << " vertices" << endl;
for (int it=0;; ++it) {
if (curve.size() < 2)
return false;
for (uint32_t it2=0; it2<curve.size(); ++it2) {
curve_new.clear();
curve_new.push_back(curve[0]);
for (uint32_t i=1; i<curve.size()-1; ++i) {
Vector3f p_new = 0.5f * (curve[i-1].p + curve[i+1].p);
Vector3f n_new = (curve[i-1].n + curve[i+1].n).normalized();
Float maxlength = (curve[i-1].p - curve[i+1].p).norm()*2;
Ray ray1(p_new, n_new, 0, maxlength);
Ray ray2(p_new, -n_new, 0, maxlength);
uint32_t idx1 = 0, idx2 = 0;
Float t1 = 0, t2 = 0;
Vector2f uv1, uv2;
bool hit1 = bvh->rayIntersect(ray1, idx1, t1, &uv1);
bool hit2 = bvh->rayIntersect(ray2, idx2, t2, &uv2);
if (!hit1 && !hit2)
continue;
CurvePoint pt;
if (t1 < t2) {
pt.p = ray1(t1);
pt.f = idx1;
pt.n = ((1 - uv1.sum()) * N.col(F(0, idx1)) + uv1.x() * N.col(F(1, idx1)) + uv1.y() * N.col(F(2, idx1))).normalized();
} else {
pt.p = ray2(t2);
pt.f = idx2;
pt.n = ((1 - uv2.sum()) * N.col(F(0, idx2)) + uv2.x() * N.col(F(1, idx2)) + uv2.y() * N.col(F(2, idx2))).normalized();
}
curve_new.push_back(pt);
}
curve_new.push_back(curve[curve.size()-1]);
curve.swap(curve_new);
}
if (!watertight && it == 1)
break;
curve_new.clear();
curve_new.push_back(curve[0]);
for (uint32_t i=1; i<curve.size(); ++i) {
if (!astar(F, E2E, V, curve[i-1].f, curve[i].f, path))
return false;
auto closest = [](const Vector3f &p0, const Vector3f &p1, const Vector3f &target) -> Vector3f {
Vector3f d = (p1-p0).normalized();
return p0 + d * std::min(std::max((target-p0).dot(d), 0.0f), (p0-p1).norm());
};
if (path.size() > 2) {
uint32_t base = curve_new.size() - 1;
for (uint32_t j=1; j<path.size()-1; ++j) {
uint32_t f = path[j];
Vector3f p0 = V.col(F(0, f)), p1 = V.col(F(1, f)), p2 = V.col(F(2, f));
CurvePoint pt2;
pt2.f = f;
pt2.n = (p1-p0).cross(p2-p0).normalized();
pt2.p = (p0+p1+p2) * (1.0f / 3.0f);
curve_new.push_back(pt2);
}
curve_new.push_back(curve[i]);
for (uint32_t q=1; q<path.size()-1; ++q) {
for (uint32_t j=1; j<path.size()-1; ++j) {
Float bestDist1 = std::numeric_limits<Float>::infinity();
Float bestDist2 = std::numeric_limits<Float>::infinity();
Vector3f bestPt1 = Vector3f::Zero(), bestPt2 = Vector3f::Zero();
uint32_t f = path[j];
for (uint32_t k=0; k<3; ++k) {
Vector3f closest1 = closest(V.col(F(k, f)), V.col(F((k + 1) % 3, f)), curve_new[base+j-1].p);
Vector3f closest2 = closest(V.col(F(k, f)), V.col(F((k + 1) % 3, f)), curve_new[base+j+1].p);
Float dist1 = (closest1 - curve_new[base+j-1].p).norm();
Float dist2 = (closest2 - curve_new[base+j+1].p).norm();
if (dist1 < bestDist1) { bestDist1 = dist1; bestPt1 = closest1; }
if (dist2 < bestDist2) { bestDist2 = dist2; bestPt2 = closest2; }
}
curve_new[base+j].p = (bestPt1 + bestPt2) * 0.5f;
}
}
} else {
curve_new.push_back(curve[i]);
}
}
curve.swap(curve_new);
curve_new.clear();
curve_new.push_back(curve[0]);
weight.clear();
weight.push_back(1.0f);
for (uint32_t i=0; i<curve.size(); ++i) {
auto &cur = curve_new[curve_new.size()-1];
auto &cur_weight = weight[weight.size()-1];
if (cur.f == curve[i].f) {
cur.p += curve[i].p;
cur.n += curve[i].n;
cur_weight += 1;
} else {
curve_new.push_back(curve[i]);
weight.push_back(1.f);
}
}
for (uint32_t i=0; i<curve_new.size(); ++i) {
curve_new[i].p /= weight[i];
curve_new[i].n.normalize();
}
curve_new[0] = curve[0];
curve_new[curve_new.size()-1] = curve[curve.size()-1];
if (curve_new.size() < 2 || curve_new[0].f == curve_new[curve_new.size()-1].f)
return false;
curve_new.swap(curve);
if (it > 2)
break;
}
cout << "Smoothed curve: " << curve.size() << " vertices" << endl;
return true;
}
std::vector<std::pair<std::vector<uint>, std::vector<SentenceInfo> aStar::Suchalgorithmus(char* eingabe, PTree<PTree <Cost> >* blacktree, Lexicon* eLex, Lexicon* fLex){
igzstream in(eingabe);
aStar::flex=fLex;
elex=eLex;
schwarz=blacktree;
string token,line;
unsigned int lineNumber = 0;
while(getline(in,line)){
istringstream ist(line); //Einlesen des Satzes
vector<unsigned int> sentence_id;
vector<HypothesisNode> Knoten;
Knoten.push_back(HypothesisNode());//initialisiert den ersten Knoten
Cost startkosten(0);
Knoten[0].setBestcost(startkosten);
//cout << "start kosten " << Knoten[0].getBestcost().cost() << endl;
int aktPos=0; //merkt sich, wieviele Wörter schon eingelesen wurden
while ( ist >> token){
Word word_id_french=flex->getWord_or_add(token); // das word zum Wort (mit 2 Bits Sprache)
unsigned int id_french= word_id_french.wordId(); //die id ohne sprachbits
sentence_id.push_back(id_french);
aktPos++;
HypothesisNode knoten_next= HypothesisNode();//initialisiert den nächsten Knoten mit den bisherigen Kosten
for (int laengePhrase=1; laengePhrase<5; laengePhrase++){
int posPhraseStart=aktPos-laengePhrase; //gibt die Pos. für den Knoten, auf dem die Phrase beginnt
if (posPhraseStart < 0) break; //wir befinden uns am Satzanfang und es gibt keine Phrasen
vector<unsigned int> fphrase;
for (int i=posPhraseStart; i<aktPos; i++){
fphrase.push_back(sentence_id[i]);
}
PTree<PTree <Cost> >* schwarzRest=schwarz->traverse(fphrase);
if (!schwarzRest) continue; //wenn es die französische Phrase nicht gibt, nächste überprüfen
PTree <Cost>* blauBaum=&schwarzRest->c;
if (blauBaum){
int counter=0; //nur fürs Programmieren, damit alle Fehler ausgemerzt werden
for (PTree<Cost>::iterator it=blauBaum->begin(); it!=blauBaum->end(); it++){
//if (counter++==10) continue;
vector<unsigned int> ephrase=it->phrase();
Cost relfreq = it->c;
if (relfreq.cost() == 1./0. ) continue;
double cost_till_aktPos=Knoten[posPhraseStart].getBestcost().cost();
if (cost_till_aktPos+prune > knoten_next.getBestcost().cost()) continue; //pruning ergibt, das ist eine schlecht Übersetzung
if(cost_till_aktPos+relfreq.cost() < knoten_next.getBestcost().cost()) knoten_next.setBestcost(Knoten[posPhraseStart].getBestcost()+relfreq.cost());
PartialTranslation* Kante= new PartialTranslation(relfreq,ephrase,&knoten_next,posPhraseStart);
Knoten[posPhraseStart].add_PartialTranslation_to_Inbound(Kante);
knoten_next.add_PartialTranslation_to_Outbound(Kante);
}
}
}
if (knoten_next.getOutbound().size() == 0){
//zuerst in das englische Lexicon einfügen
string word_string = flex->getString(sentence_id[aktPos-1]);
unsigned int id_english=elex->getWord_or_add(word_string);
//dann die Kante anlegen, dabei sollen die Kosten niedrig sein, da sie sowieso genutzt werden muss, kann sie auch direkt exploriert werden
PartialTranslation* Kante= new PartialTranslation(Cost(0),vector<unsigned int>{id_english},&Knoten[aktPos],aktPos-1);
knoten_next.setBestcost(Knoten.back().getBestcost());
knoten_next.add_PartialTranslation_to_Outbound(Kante);
Knoten.back().add_PartialTranslation_to_Inbound(Kante);
}
Knoten.push_back(knoten_next); //letzter Knoten (node_next) hat keine eingehenden Kanten
}
//dotGraph(Knoten, elex);
aStar astar(Knoten);
astar.lineNumber = lineNumber;
std::vector<SentenceInfo> sentenceInfos = astar.print(astar.search(), sentence_id, schwarz);
for(unsigned int i = 0; i < Knoten.size(); i++){
HypothesisNode& hnode = Knoten[i];
for(unsigned int j = 0; j < hnode.getOutbound().size(); j++)
delete hnode.getOutbound()[j];
}
std::pair<std::vector<uint>, std::vector<SentenceInfo> > pair_tmp;
pair_tmp.first = sentence_id;
pair_tmp.second = sentenceInfos;
nBestLists.push_back(pair_tmp);
lineNumber ++;
}
return nBestLists;
}
void FloorMapLayer::onTouchEnded(Touch *touch, Event *e)
{
// bugfix: 如果对话框已弹出，就不再执行以下代码，这种情况是在对话框弹出前，touchbegan就已经触发的情况下发生
// 重现方法: 控制勇士经过怪物，然后鼠标按下等待勇士到达怪物处，弹出提示对话框后，松开鼠标继续弹出
if (ModalDialogManager::GetInstance()->isShown())
{
return;
}
// 正在战斗中或自定义忙碌（如开门等待）中则不允许重新走动，以免计算混乱
if (_warrior->is_fighting() || _warrior->is_lock())
{
return;
}
// 获取起始点
auto end_vec2 = _tiled_map->convertTouchToNodeSpace(touch) / 75.0f;
auto end_pt = Floor::position_t(end_vec2.x, end_vec2.y);
if (!_paths.empty() && _paths.back() == end_pt)
{
return;
}
auto start_vec2 = _tiled_map->convertToNodeSpace(_warrior->getPosition()) / 75.0f;
auto start_pt = Floor::position_t(start_vec2.x, start_vec2.y);
if (start_pt == end_pt)
{
return;
}
// 获取阻碍点
auto wall_layer = _tiled_map->getLayer("wall");
auto wall_layer_size = wall_layer->getLayerSize();
auto pos = touch->getLocation();
auto tiles = wall_layer->getTiles();
std::vector<AStar::node_t> blocks;
for (uint32_t i = 0; i < (uint32_t)(wall_layer_size.height); ++i)
{
for (uint32_t j = 0; j < (uint32_t)(wall_layer_size.width); ++j)
{
if (tiles[i * (uint32_t)(wall_layer_size.width) + j] != 0)
{
blocks.push_back(AStar::node_t(j, (uint32_t)(wall_layer_size.height) - 1 - i));
}
}
}
if (std::find(blocks.begin(), blocks.end(), AStar::node_t(end_pt.x, end_pt.y)) != blocks.end() || end_pt.x >= 10 || end_pt.y >= 12)
{
return;
}
// A星路径
AStar astar(10, 12);
astar.set_start_and_end(AStar::node_t(start_pt.x, start_pt.y), AStar::node_t(end_pt.x, end_pt.y));
astar.set_blocks(blocks);
std::vector<AStar::node_t> paths = astar.get_path();
auto stair_iter = std::find_if(paths.begin(), paths.end(), [&](AStar::node_t node) {
// 找到除了首节点的第一个楼梯节点
Floor::position_t pos(node.x, node.y);
return pos != start_pt && (pos == _stair_up.pos || pos == _stair_down.pos);
});
if (stair_iter != paths.end())
paths.erase(++stair_iter, paths.end());
_paths.clear();
for (const auto& node : paths)
{
_paths.push_back(Floor::position_t(node.x, node.y));
}
// 箭头
_arrow_node->setVisible(true);
_arrow_node->setPosition(Vec2((end_pt.x + 0.5f) * 75.0f - 50.0f, (end_pt.y + 0.5f) * 75.0f - 50.0f));
// 路线
uint32_t index = 0;
_road_node->removeAllChildren();
for (auto node : _paths)
{
if (index == _paths.size() - 1)
break;
auto pos = Vec2((node.x + 0.5f) * 75.0f - 50.0f, (node.y + 0.5f) * 75.0f - 50.0f);
auto res = "Images/diban" + String::createWithFormat("%d", index % 22)->_string + ".png";
auto sprite = Sprite::create(res);
sprite->setPosition(pos);
_road_node->addChild(sprite);
++index;
}
// 勇士
_warrior->stopAllActions();
step();
}
