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