static void readCornersFromSockets(rcti *rect, SocketReader *readers[4], float corners[4][2])
{
for (int i = 0; i < 4; ++i) {
float result[4] = {0.0f, 0.0f, 0.0f, 0.0f};
readers[i]->readSampled(result, rect->xmin, rect->ymin, COM_PS_NEAREST);
corners[i][0] = result[0];
corners[i][1] = result[1];
}
/* convexity check:
* concave corners need to be prevented, otherwise
* BKE_tracking_homography_between_two_quads will freeze
*/
if (!check_corners(corners)) {
/* simply revert to default corners
* there could be a more elegant solution,
* this prevents freezing at least.
*/
corners[0][0] = 0.0f;
corners[0][1] = 0.0f;
corners[1][0] = 1.0f;
corners[1][1] = 0.0f;
corners[2][0] = 1.0f;
corners[2][1] = 1.0f;
corners[3][0] = 0.0f;
corners[3][1] = 1.0f;
}
}
multimap<unsigned,node*> find_path(unsigned id, node* n0, node* n1, bool allow_diag, bool &status)
{
node* current;
for (unsigned int i = 0; i < gridstructarray[id]->hcells*gridstructarray[id]->vcells; i++)
{
gridstructarray[id]->nodearray[i].F=0;
gridstructarray[id]->nodearray[i].G=0;
gridstructarray[id]->nodearray[i].H=0;
gridstructarray[id]->nodearray[i].came_from=NULL;
}
node* start = n0;
node* destination = n1;
multimap<unsigned,node*> OPEN;
list<node*> CLOSED;
multimap<unsigned,node*> mm_ret;
if (start == destination)
return mm_ret;
start->H = find_heuristic(start,destination,allow_diag);
start->F = start->H;
OPEN.insert(pair<unsigned,node*>(start->F,start));
while (find(CLOSED.begin(), CLOSED.end(), destination) == CLOSED.end() && !OPEN.empty())
//loop until destination is in the closed list or the open multimap is empty
{
//if (OPEN.empty()){break;}
//multimap<unsigned,node*>::iterator tit;
/*std::cout << "Begin" << std::endl;
for ( tit=OPEN.begin() ; tit != OPEN.end(); tit++ )
std::cout << (*tit).first << " => " << (*tit).second << std::endl;*/
//this is basically ds_priority_delete_min
{
multimap<unsigned,node*>::iterator it = OPEN.begin(), it_check;
it_check = it++;
while (it != OPEN.end())
{
if ((*it).first < (*it_check).first) {it_check = it;}
it++;
}
current = (*it_check).second;
OPEN.erase(it_check);
}
//std::cout << OPEN.size() << std::endl;
//std::cout << "End" << std::endl;
//multimap<unsigned,node*>::iterator tit;
/*for ( tit=OPEN.begin() ; tit != OPEN.end(); tit++ )
std::cout << (*tit).first << " => " << (*tit).second << std::endl;*/
//std::cout << "Ended" << std::endl;
CLOSED.push_front(current);
for (vector<enigma::node*>::iterator it = current->neighbor_nodes.begin(); it!=current->neighbor_nodes.end(); ++it)
{ //go trough all the neighbors (should be faster then 8 if cycles like in the init)
if (!allow_diag && (*it)->x != current->x && (*it)->y != current->y)
continue;
bool cutting = false;
if (allow_diag == true && (*it)->x != current->x && (*it)->y != current->y)
cutting = check_corners(id, current, (*it));
if (find(CLOSED.begin(), CLOSED.end(), (*it)) == CLOSED.end() && (*it)->cost<gridstructarray[id]->threshold && cutting == false){
if (find_priority(OPEN,(*it)) == -1){ //if the node isn't on the open list
(*it)->came_from = current;
(*it)->G = current->G + (*it)->cost;
if ((*it)->x != current->x && (*it)->y != current->y)
(*it)->G += ceil((*it)->cost/2.5); //if it is diagonal increase the move cost
(*it)->H = find_heuristic(*it,destination,allow_diag);
(*it)->F = (*it)->G + (*it)->H;
//std::cout << "Node: " << (*it)->x*gridstructarray[id]->vcells+(*it)->y << " F = " << (*it)->F << " H = " << (*it)->H << " G = " << (*it)->G << std::endl;
OPEN.insert(pair<unsigned,node*>((*it)->F,(*it)));
}else{ //if the node is already on the open list we just have to see if it is a better path
unsigned temp_G = current->G + (*it)->cost;
if (temp_G < (*it)->G) {
(*it)->came_from = current;
(*it)->G = current->G + (*it)->cost;
if ((*it)->x != current->x && (*it)->y != current->y)
(*it)->G += ceil((*it)->cost/2.5);
(*it)->H = find_heuristic(*it,destination,allow_diag);
(*it)->F = (*it)->G + (*it)->H;
}
}
}
}
}
node *last, *nearest;
unsigned i = 0;
status = true;
if (find(CLOSED.begin(), CLOSED.end(), destination) == CLOSED.end())
{ //this is for if the destionation can't be found if
status = false;
nearest = start;
list<node*>::iterator it;
for (it=CLOSED.begin(); it != CLOSED.end(); it++){
(*it)->H = find_heuristic(*it,destination,allow_diag);
if ((*it)->H < nearest->H)
nearest = *it;
}
destination = nearest;
if (start == destination)
return mm_ret;
}
//mm_ret.insert(pair<unsigned,node*>(i,destination));
last = destination;
while (last->came_from != start){
i += 1;
mm_ret.insert(pair<unsigned,node*>(i,last->came_from));
last = last->came_from;
}
//delete CLOSED;
//delete OPEN;
return mm_ret; //return the multimap containing a list of all the nodes that are in the path.
}
int l_link::travel(point_t start, point_t end)
{
++deep;
//DEBUG_LOG("%u, start(%u, : %u ), end(%u, %u)", deep, start.x, start.y, end.x, end.y);
path.already_path.push_back(start);
if(start == end){
map[end.x][end.y] = 0;
map[path.already_path[0].x][path.already_path[0].y] = 0;
++find_pairs;
//DEBUG_LOG("----find_pairs: %u, pair_count: %u ----", find_pairs, pair_count);
return 0;
}
/*
find_optimal_direction(start, end);
*/
uint32_t i = 0;
for(; i < DIRECTIONS; ++i) {
int step_x = 0, step_y = 0;
switch(directions[i]){
case EAST:
{
step_y = 1;
break;
}
case WEST:
{
step_y = -1;
break;
}
case SOUTH:
{
step_x = 1;
break;
}
case NORTH:
{
step_x = -1;
break;
}
}
//new initial point;
point_t new_start;
new_start.x = step_x + start.x;
new_start.y = step_y + start.y;
//out of range
if( new_start.x >= ROWS || new_start.y >= COLS){
continue;
}
//some obstacles
if(map[new_start.x][new_start.y] != 0 && map[new_start.x][new_start.y] != map[end.x][end.y]){
continue;
}
//have already travel
std::vector<point_t>::reverse_iterator iter = std::find( path.already_path.rbegin(), path.already_path.rend(), new_start);
if(iter != path.already_path.rend()){
continue;
}
//check corners,if corners > 2, bad over this path
int able_corner = check_corners(new_start);
if(able_corner == -1){
continue;
}
//able to continue travel along this path
int ret = travel(new_start, end);
if(ret == 0){
return ret;
}
if(able_corner == 1){
--path.corner;
}
}
if(i >= DIRECTIONS){
path.already_path.pop_back();
return -1;
}
return 0;
}
