LocalInfo* get_local(int which) {
std::map<int, LocalInfo>::iterator i = local_info_.find(which);
if(i == local_info_.end()) {
LocalInfo li(which);
local_info_[which] = li;
return &local_info_[which];
}
return &i->second;
}
std::list<Node*> Dlite::GetSurroundings(Node *current, LocalMap &map) {
std::list<Node> result1;
int x = current->point.x;
int y = current->point.y;
if(opt.allowdiagonal) {
for (int k = y - 1; k <= y + 1; ++k) {
for (int l = x - 1; l <= x + 1; ++l) {
if (!(k == y && l == x) && map.CellOnGrid(Cell(l, k)) && map.CellIsTraversable(Cell(l, k))) {
result1.push_front(Node(Cell(l, k)));
}
}
}
} else {
for (int k = x - 1; k <= x + 1; ++k)
if (k != x && map.CellOnGrid(Cell(k, y)) && map.CellIsTraversable(Cell(k, y)))
result1.push_front(Node(Cell(k, y)));
for (int l = y - 1; l <= y + 1; ++l)
if (l != y && map.CellOnGrid(Cell(x, l)) && map.CellIsTraversable(Cell(x, l)))
result1.push_front(Node(Cell(x, l)));
}
std::list<Node*> result;
for(auto elem : result1) {
if(!NODES.count(vertex(elem.point, map.height))) { //if vertex wasn't previously examined
continue;
} else {
result.push_back(&(NODES.find(vertex(elem.point, map.height))->second));
}
}
return result;
}
std::list<Node> Dlite::FindNeighbors(Node* n, LocalMap &map) const {
Node newNode;
Cell curNode = n->point;
std::list<Node> successors;
for (int i = -1; i <= +1; i++)
for (int j = -1; j <= +1; j++)
if ((i != 0 || j != 0) && map.CellOnGrid(Cell(curNode.x + j, curNode.y + i)) &&
(map.CellIsTraversable(Cell(curNode.x + j, curNode.y + i)))) {
if (i != 0 && j != 0) {
if (!opt.allowdiagonal)
continue;
else if (!opt.cutcorners) {
if (map.CellIsObstacle(Cell(curNode.x + j, curNode.y)) ||
map.CellIsObstacle(Cell(curNode.x, curNode.y + i)))
continue;
}
else if (!opt.allowsqueeze) {
if (map.CellIsObstacle(Cell( curNode.x + j, curNode.y)) &&
map.CellIsObstacle(Cell( curNode.x, curNode.y + i)))
continue;
}
}
newNode = Node(Cell(curNode.x + j, curNode.y + i), n);
successors.push_front(newNode);
}
return successors;
}
void FollowIdMovement::setDir(LocalMap& localmap, GameObject& my_obj)
{
// set base direction
sf::Vector2f dir = localmap.getPosById(_id_to_follow) - my_obj.getPos();
dir /= float(SCRN_TILE_SIZE); // normallize to squares
// check if following_id is within range
if (my_obj.getRadius()*1.8f/SCRN_TILE_SIZE < distance(dir, sf::Vector2f(0,0)) && distance(dir, sf::Vector2f(0,0)) < _radius)
{
// if it is, set the direction
if (EPSILON < abs(dir.x))
dir.x = abs(dir.x)/dir.x;
if (EPSILON < abs(dir.y))
dir.y = abs(dir.y)/dir.y;
_direction = dir;
}
// if not, don't move.
else _direction = sf::Vector2f(0,0);
}
// check if we have enough exp to raise to the next level (will check when Loot is called)
void HeroData::checkLevelRaise(LocalMap& map, HeroCharacter& hero_char)
{
// check raise
for (;_level < _exp/pow(2.f,int(_level)); ++_level)
{
Stats raise = _stats.Raise(); // raise stats
stringstream ss; // write message on raise
ss << "You have been raised to level " << _level+1 << "!\nCongratulations!";
ss << std::endl << "ATK + " << raise.ATK() << " | DEF + " << raise.DEF() << " | LUCK + " << raise.LUCK();
if (_level+1 == LEARN_BULLET_ATTACK_LEVEL) // add a new attack when reach to a certain level
{
hero_char.addNewAttack(shared_ptr<AttackFactory>(new AttackFactoryT<BulletAttack>));
ss << "\nYou have learned a new Attack: Bullet!";
}
string str(ss.str());
map.addScript(shared_ptr<Script>(new Dialog(str))); // write the message
_HP = BASE_HP; // regenerate health
}
}
LocalInfo* get_local(int which) {
LocalMap::iterator i = local_info_.find(which);
if(i == local_info_.end()) return 0;
return &i->second;
}
static void insertAndConnectBlocks(BasicBlockMap& newBlocks,
ir::ControlFlowGraph::iterator& functionEntry,
ir::ControlFlowGraph::iterator& functionExit,
ir::IRKernel& kernel, unsigned int& nextRegister,
const ir::IRKernel& inlinedKernel)
{
typedef std::unordered_map<ir::PTXOperand::RegisterType,
ir::PTXOperand::RegisterType> RegisterMap;
ir::IRKernel copy;
const ir::IRKernel* inlinedKernelPointer = &inlinedKernel;
// create a copy if the call is recursive
if(inlinedKernelPointer == &kernel)
{
copy = inlinedKernel;
inlinedKernelPointer = ©
}
// Insert new blocks
for(auto block = inlinedKernelPointer->cfg()->begin();
block != inlinedKernelPointer->cfg()->end(); ++block)
{
auto newBlock = kernel.cfg()->clone_block(block);
newBlocks.insert(std::make_pair(block, newBlock));
}
// Connect new blocks, rename branch labels
for(auto block = newBlocks.begin(); block != newBlocks.end(); ++block)
{
for(auto edge = block->first->out_edges.begin();
edge != block->first->out_edges.end(); ++edge)
{
auto headBlock = block->second;
auto tail = (*edge)->tail;
auto tailBlock = newBlocks.find(tail);
assert(tailBlock != newBlocks.end());
kernel.cfg()->insert_edge(ir::Edge(headBlock,
tailBlock->second, (*edge)->type));
if((*edge)->type == ir::Edge::Branch)
{
assert(!headBlock->instructions.empty());
auto instruction = headBlock->instructions.back();
auto branch = static_cast<ir::PTXInstruction*>(instruction);
if(branch->opcode == ir::PTXInstruction::Ret) continue;
assertM(branch->opcode == ir::PTXInstruction::Bra, "Expecting "
<< branch->toString() << " to be a branch");
branch->d.identifier = tailBlock->second->label();
}
}
}
// Assign copied blocks new registers
RegisterMap newRegisters;
for(auto block = newBlocks.begin(); block != newBlocks.end(); ++block)
{
for(auto instruction = block->second->instructions.begin();
instruction != block->second->instructions.end(); ++instruction)
{
ir::PTXInstruction& ptx = static_cast<ir::PTXInstruction&>(
**instruction);
ir::PTXOperand* operands[] = {&ptx.pg, &ptx.pq, &ptx.d, &ptx.a,
&ptx.b, &ptx.c};
for(unsigned int i = 0; i < 6; ++i)
{
ir::PTXOperand& operand = *operands[i];
if( operand.addressMode != ir::PTXOperand::Register &&
operand.addressMode != ir::PTXOperand::Indirect &&
operand.addressMode != ir::PTXOperand::ArgumentList)
{
continue;
}
if(operand.type != ir::PTXOperand::pred)
{
if(operand.array.empty() &&
operand.addressMode != ir::PTXOperand::ArgumentList)
{
auto mapping = newRegisters.find(operand.reg);
if(mapping == newRegisters.end())
{
mapping = newRegisters.insert(std::make_pair(
operand.reg, nextRegister++)).first;
}
operand.reg = mapping->second;
}
else
{
for(auto subOperand = operand.array.begin();
subOperand != operand.array.end(); ++subOperand )
{
if(!subOperand->isRegister()) continue;
auto mapping = newRegisters.find(subOperand->reg);
if(mapping == newRegisters.end())
{
mapping = newRegisters.insert(std::make_pair(
subOperand->reg, nextRegister++)).first;
}
subOperand->reg = mapping->second;
}
}
}
else if(operand.addressMode != ir::PTXOperand::ArgumentList)
{
if(operand.condition == ir::PTXOperand::Pred
|| operand.condition == ir::PTXOperand::InvPred)
{
auto mapping = newRegisters.find(operand.reg);
if(mapping == newRegisters.end())
{
mapping = newRegisters.insert(std::make_pair(
operand.reg, nextRegister++)).first;
}
operand.reg = mapping->second;
}
}
}
}
}
// Assign copied blocks new local variables
typedef std::unordered_map<std::string, std::string> LocalMap;
LocalMap locals;
for(auto local = inlinedKernel.locals.begin();
local != inlinedKernel.locals.end(); ++local)
{
std::string newName = "_Zinlined_" + local->first;
locals.insert(std::make_pair(local->first, newName));
auto newLocal = kernel.locals.insert(
std::make_pair(newName, local->second)).first;
newLocal->second.name = newName;
}
for(auto block = newBlocks.begin(); block != newBlocks.end(); ++block)
{
for(auto instruction = block->second->instructions.begin();
instruction != block->second->instructions.end(); ++instruction)
{
ir::PTXInstruction& ptx = static_cast<ir::PTXInstruction&>(
**instruction);
if(!ptx.mayHaveAddressableOperand()) continue;
ir::PTXOperand* operands[] = {&ptx.pg, &ptx.pq, &ptx.d, &ptx.a,
&ptx.b, &ptx.c};
for(unsigned int i = 0; i < 6; ++i)
{
ir::PTXOperand& operand = *operands[i];
if(operand.addressMode != ir::PTXOperand::Address) continue;
auto local = locals.find(operand.identifier);
if(local == locals.end()) continue;
operand.identifier = local->second;
}
}
}
// Get the entry and exit points
auto entryMapping = newBlocks.find(
inlinedKernelPointer->cfg()->get_entry_block());
assert(entryMapping != newBlocks.end());
functionEntry = entryMapping->second;
auto exitMapping = newBlocks.find(
inlinedKernelPointer->cfg()->get_exit_block());
assert(exitMapping != newBlocks.end());
functionExit = exitMapping->second;
}
SearchResult Dlite::FindThePath(LocalMap &map, const Map& const_map, EnvironmentOptions options)
{
opt = options;
std::chrono::time_point<std::chrono::system_clock> tstart, end;
tstart = std::chrono::system_clock::now();
number_of_steps = 0;
current_result.pathlength = 0;
Initialize(map);
last = start;
if(!ComputeShortestPath(map))
std::cout << "OOOPS\n";
else
std::cout << "Done\n";
std::cout << current_result.pathlength <<std::endl;
while(start->point != goal->point) {
Cell jump = start->point;
Node min_val = GetMinPredecessor(start, map);
path.push_back(*start);
if (!min_val.parent) {
OPEN.remove_if(start);
current_result.pathfound = false;
current_result.pathlength = 0;
return current_result;
} else {
current_result.pathlength += GetCost(start->point, min_val.parent->point, map);
start = min_val.parent;
}
min_val = GetMinPredecessor(start, map);
while (opt.allowjump && euclid_dist(jump, min_val.parent->point) < radius && start->point != goal->point) {
path.push_back(*start);
if (!min_val.parent) {
OPEN.remove_if(start);
current_result.pathfound = false;
current_result.pathlength = 0;
return current_result;
} else {
current_result.pathlength += GetCost(start->point, min_val.parent->point, map);
start = min_val.parent;
}
min_val = GetMinPredecessor(start, map);
}
UpdateVertex(start);
Changes changes = map.UpdateMap(const_map, start->point, radius);
if (!changes.cleared.empty() && !changes.occupied.empty()) {
Km += heuristic(last->point, start->point, opt.metrictype);
last = start;
}
for (auto dam : changes.occupied) {
if (NODES.count(vertex(dam, map.height))) {
Node* d = &(NODES.find(vertex(dam, map.height))->second);
OPEN.remove_if(d);
for (auto neighbor: GetSurroundings(d, map)) {
//std::cout << "n: " << neighbor->point << std::endl;
if (neighbor->point != map.goal && (neighbor->parent->point == dam || CutOrSqueeze(neighbor, d))) {
Node min_val = GetMinPredecessor(neighbor, map);
if (!min_val.parent) {
OPEN.remove_if(neighbor);
if(neighbor->point == start->point) {
current_result.pathfound = false;
current_result.pathlength = 0;
return current_result;
}
} else {
neighbor->rhs = min_val.rhs;
neighbor->parent = min_val.parent;
// std::cout << "changed: "
// << neighbor->point << ' ' << neighbor->parent->point << std::endl;
UpdateVertex(neighbor);
}
}
}
}
}
for (auto cleared : changes.cleared) {
Node new_node(cleared);
new_node.rhs = std::numeric_limits<double>::infinity();
new_node.g = std::numeric_limits<double>::infinity();
NODES[vertex(cleared, map.height)] = new_node;
Node * cl = &(NODES.find(vertex(cleared, map.height))->second);
Node min_val = GetMinPredecessor(cl, map);
if (min_val.parent) {
cl->rhs = min_val.rhs;
cl->parent = min_val.parent;
cl->g = min_val.parent->g + GetCost(cl->point, min_val.parent->point, map);
UpdateVertex(cl);
} else {
break;
}
for (auto neighbor : GetSuccessors(cl, map)) {
if (neighbor->rhs > cl->g + GetCost(neighbor->point, cl->point, map)) {
neighbor->parent = cl;
neighbor->rhs = cl->g + GetCost(neighbor->point, cl->point, map);
UpdateVertex(neighbor);
}
if (neighbor->point.x == cl->point.x || neighbor->point.y == cl->point.y) {
Node min_val = GetMinPredecessor(neighbor, map);
if (!min_val.parent) {
OPEN.remove_if(neighbor);
if(neighbor->point == start->point) {
current_result.pathfound = false;
current_result.pathlength = 0;
return current_result;
}
} else {
neighbor->rhs = min_val.rhs;
neighbor->parent = min_val.parent;
//std::cout << "changed: "
// << neighbor->point << ' ' << neighbor->parent->point << std::endl;
UpdateVertex(neighbor);
}
}
}
}
if(ComputeShortestPath(map)){
//std::cout << "ALL OK\n";
continue;
} else {
std::cout << "NOT OK" << std::endl;
if (OPEN.top_key() == Key(std::numeric_limits<double>::infinity(), std::numeric_limits<double>::infinity())) {
current_result.pathfound = false;
current_result.pathlength = 0;
break;
}
}
}
end = std::chrono::system_clock::now();
current_result.time = static_cast<double>(std::chrono::duration_cast<std::chrono::nanoseconds>(end - tstart).count()) / 1000000000;
//map.PrintPath(path);
current_result.lppath = &path;
if (current_result.pathfound) {
makeSecondaryPath();
current_result.hppath = &hpath;
}
//for (auto elem: path) std::cout << elem->point << " ";
//std::cout << std::endl;
return current_result;
}
int main(int argc, char** argv){
boss::IdContext context1;
boss::IdContext context2;
std::vector<PinholeCameraInfo*> camera_infos;
std::vector<BaseCameraInfo*> base_camera_infos;
for (int i = 0; i<10; i++) {
PinholeCameraInfo* c = new PinholeCameraInfo;
camera_infos.push_back(c);
base_camera_infos.push_back(c);
}
MultiCameraInfo* multi_cam = new MultiCameraInfo;
multi_cam->cameraInfos()=base_camera_infos;
for (int i = 0; i<camera_infos.size(); i++) {
PinholeCameraInfo* cam1 = camera_infos[i];
PinholeCameraInfoMsg msg1 = pinholeCameraInfo2msg(cam1, &context1);
cerr << "msg1.id" << msg1.id << endl;
BaseCameraInfo* cam2 = msg2pinholeCameraInfo(msg1, &context2);
cerr << "cam2.id()" << cam2->getId() << endl;
}
MultiCameraInfoMsg mcmsg = multiCameraInfo2msg(multi_cam, &context1);
MultiCameraInfo* mci = msg2multiCameraInfo(mcmsg, &context2);
std::vector<MapNode*> nodes;
for (size_t i = 0; i< 10; i++) {
MapNode* n = new MapNode;
MapNodeMsg msg = mapNode2msg(n, &context1);
MapNode* n2 = msg2MapNode(msg, &context2);
nodes.push_back(n2);
}
for (size_t i = 0; i< 10; i++) {
ImageMapNode* n = new ImageMapNode;
n->setCameraInfo(camera_infos[0]);
ImageMapNodeMsg msg = imageMapNode2msg(n, &context1);
ImageMapNode* n2 = msg2imageMapNode(msg, &context2);
nodes.push_back(n2);
}
for (size_t i = 0; i< nodes.size(); i++) {
MapNode* n = nodes[i];
cerr << "n: " << n->getId() << endl;
}
LocalMap* lmap = new LocalMap;
for (size_t i = 0; i<nodes.size(); i++) {
lmap->nodes().addElement(nodes[i]);
}
lmap->setCloud(new Cloud);
LocalMapMsg lmap_msg=localMap2msg(lmap, &context1);
LocalMap* lmap2 = msg2localMap(lmap_msg, &context2);
for (MapNodeList::iterator it = lmap2->nodes().begin(); it!=lmap2->nodes().end(); it++){
MapNode* n = it->get();
cerr << n->getId() << endl;
}
}
