428 lines
9.7 KiB
C++
428 lines
9.7 KiB
C++
|
/*************************************************************************/
|
||
|
|
/* image_path_finder.cpp */
|
||
|
|
/*************************************************************************/
|
||
|
|
/* This file is part of: */
|
||
|
|
/* GODOT ENGINE */
|
||
|
|
/* http://www.godotengine.org */
|
||
|
|
/*************************************************************************/
|
||
|
|
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
|
||
|
|
/* */
|
||
|
|
/* Permission is hereby granted, free of charge, to any person obtaining */
|
||
|
|
/* a copy of this software and associated documentation files (the */
|
||
|
|
/* "Software"), to deal in the Software without restriction, including */
|
||
|
|
/* without limitation the rights to use, copy, modify, merge, publish, */
|
||
|
|
/* distribute, sublicense, and/or sell copies of the Software, and to */
|
||
|
|
/* permit persons to whom the Software is furnished to do so, subject to */
|
||
|
|
/* the following conditions: */
|
||
|
|
/* */
|
||
|
|
/* The above copyright notice and this permission notice shall be */
|
||
|
|
/* included in all copies or substantial portions of the Software. */
|
||
|
|
/* */
|
||
|
|
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
|
||
|
|
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
|
||
|
|
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
|
||
|
|
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
|
||
|
|
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
|
||
|
|
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
||
|
|
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||
|
|
/*************************************************************************/
|
||
|
|
#include "image_path_finder.h"
|
||
|
|
|
||
|
|
|
||
|
|
void ImagePathFinder::_unlock() {
|
||
|
|
|
||
|
|
lock=DVector<Cell>::Write();
|
||
|
|
cells=NULL;
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
void ImagePathFinder::_lock() {
|
||
|
|
|
||
|
|
lock = cell_data.write();
|
||
|
|
cells=lock.ptr();
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
bool ImagePathFinder::_can_go_straigth(const Point2& p_from, const Point2& p_to) const {
|
||
|
|
|
||
|
|
int x1=p_from.x;
|
||
|
|
int y1=p_from.y;
|
||
|
|
int x2=p_to.x;
|
||
|
|
int y2=p_to.y;
|
||
|
|
|
||
|
|
#define _TEST_VALID \
|
||
|
|
{\
|
||
|
|
uint32_t ofs=drawy*width+drawx;\
|
||
|
|
if (cells[ofs].solid) {\
|
||
|
|
if (!((drawx>0 && cells[ofs-1].visited) ||\
|
||
|
|
(drawx<width-1 && cells[ofs+1].visited) ||\
|
||
|
|
(drawy>0 && cells[ofs-width].visited) ||\
|
||
|
|
(drawy<height-1 && cells[ofs+width].visited))) {\
|
||
|
|
return false;\
|
||
|
|
}\
|
||
|
|
}\
|
||
|
|
}\
|
||
|
|
|
||
|
|
|
||
|
|
int n, deltax, deltay, sgndeltax, sgndeltay, deltaxabs, deltayabs, x, y, drawx, drawy;
|
||
|
|
deltax = x2 - x1;
|
||
|
|
deltay = y2 - y1;
|
||
|
|
deltaxabs = ABS(deltax);
|
||
|
|
deltayabs = ABS(deltay);
|
||
|
|
sgndeltax = SGN(deltax);
|
||
|
|
sgndeltay = SGN(deltay);
|
||
|
|
x = deltayabs >> 1;
|
||
|
|
y = deltaxabs >> 1;
|
||
|
|
drawx = x1;
|
||
|
|
drawy = y1;
|
||
|
|
int pc=0;
|
||
|
|
|
||
|
|
_TEST_VALID
|
||
|
|
|
||
|
|
if(deltaxabs >= deltayabs) {
|
||
|
|
for(n = 0; n < deltaxabs; n++) {
|
||
|
|
y += deltayabs;
|
||
|
|
if(y >= deltaxabs){
|
||
|
|
y -= deltaxabs;
|
||
|
|
drawy += sgndeltay;
|
||
|
|
}
|
||
|
|
drawx += sgndeltax;
|
||
|
|
_TEST_VALID
|
||
|
|
}
|
||
|
|
} else {
|
||
|
|
for(n = 0; n < deltayabs; n++) {
|
||
|
|
x += deltaxabs;
|
||
|
|
if(x >= deltayabs) {
|
||
|
|
x -= deltayabs;
|
||
|
|
drawx += sgndeltax;
|
||
|
|
}
|
||
|
|
drawy += sgndeltay;
|
||
|
|
_TEST_VALID
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return true;
|
||
|
|
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
bool ImagePathFinder::_is_linear_path(const Point2& p_from, const Point2& p_to) {
|
||
|
|
|
||
|
|
int x1=p_from.x;
|
||
|
|
int y1=p_from.y;
|
||
|
|
int x2=p_to.x;
|
||
|
|
int y2=p_to.y;
|
||
|
|
|
||
|
|
#define _TEST_CELL \
|
||
|
|
if (cells[drawy*width+drawx].solid)\
|
||
|
|
return false;
|
||
|
|
|
||
|
|
|
||
|
|
int n, deltax, deltay, sgndeltax, sgndeltay, deltaxabs, deltayabs, x, y, drawx, drawy;
|
||
|
|
deltax = x2 - x1;
|
||
|
|
deltay = y2 - y1;
|
||
|
|
deltaxabs = ABS(deltax);
|
||
|
|
deltayabs = ABS(deltay);
|
||
|
|
sgndeltax = SGN(deltax);
|
||
|
|
sgndeltay = SGN(deltay);
|
||
|
|
x = deltayabs >> 1;
|
||
|
|
y = deltaxabs >> 1;
|
||
|
|
drawx = x1;
|
||
|
|
drawy = y1;
|
||
|
|
int pc=0;
|
||
|
|
|
||
|
|
_TEST_CELL
|
||
|
|
|
||
|
|
if(deltaxabs >= deltayabs) {
|
||
|
|
for(n = 0; n < deltaxabs; n++) {
|
||
|
|
y += deltayabs;
|
||
|
|
if(y >= deltaxabs){
|
||
|
|
y -= deltaxabs;
|
||
|
|
drawy += sgndeltay;
|
||
|
|
}
|
||
|
|
drawx += sgndeltax;
|
||
|
|
_TEST_CELL
|
||
|
|
}
|
||
|
|
} else {
|
||
|
|
for(n = 0; n < deltayabs; n++) {
|
||
|
|
x += deltaxabs;
|
||
|
|
if(x >= deltayabs) {
|
||
|
|
x -= deltayabs;
|
||
|
|
drawx += sgndeltax;
|
||
|
|
}
|
||
|
|
drawy += sgndeltay;
|
||
|
|
_TEST_CELL
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
DVector<Point2> ImagePathFinder::find_path(const Point2& p_from, const Point2& p_to,bool p_optimize) {
|
||
|
|
|
||
|
|
|
||
|
|
Point2i from=p_from;
|
||
|
|
Point2i to=p_to;
|
||
|
|
|
||
|
|
ERR_FAIL_COND_V(from.x < 0,DVector<Point2>());
|
||
|
|
ERR_FAIL_COND_V(from.y < 0,DVector<Point2>());
|
||
|
|
ERR_FAIL_COND_V(from.x >=width,DVector<Point2>());
|
||
|
|
ERR_FAIL_COND_V(from.y >=height,DVector<Point2>());
|
||
|
|
ERR_FAIL_COND_V(to.x < 0,DVector<Point2>());
|
||
|
|
ERR_FAIL_COND_V(to.y < 0,DVector<Point2>());
|
||
|
|
ERR_FAIL_COND_V(to.x >=width,DVector<Point2>());
|
||
|
|
ERR_FAIL_COND_V(to.y >=height,DVector<Point2>());
|
||
|
|
|
||
|
|
if (from==to) {
|
||
|
|
DVector<Point2> p;
|
||
|
|
p.push_back(from);
|
||
|
|
return p;
|
||
|
|
}
|
||
|
|
|
||
|
|
_lock();
|
||
|
|
|
||
|
|
|
||
|
|
if (p_optimize) { //try a line first
|
||
|
|
|
||
|
|
if (_is_linear_path(p_from,p_to)) {
|
||
|
|
_unlock();
|
||
|
|
DVector<Point2> p;
|
||
|
|
p.push_back(from);
|
||
|
|
p.push_back(to);
|
||
|
|
return p;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
//clear all
|
||
|
|
for(int i=0;i<width*height;i++) {
|
||
|
|
|
||
|
|
bool s = cells[i].solid;
|
||
|
|
cells[i].data=0;
|
||
|
|
cells[i].solid=s;
|
||
|
|
}
|
||
|
|
|
||
|
|
#define CELL_INDEX(m_p) (m_p.y*width+m_p.x)
|
||
|
|
#define CELL_COST(m_p) (cells[CELL_INDEX(m_p)].cost+( ABS(m_p.x-to.x)+ABS(m_p.y-to.y))*10)
|
||
|
|
|
||
|
|
|
||
|
|
Set<Point2i> pending;
|
||
|
|
pending.insert(from);
|
||
|
|
|
||
|
|
//helper constants
|
||
|
|
static const Point2i neighbour_rel[8]={
|
||
|
|
Point2i(-1,-1), //0
|
||
|
|
Point2i(-1, 0), //1
|
||
|
|
Point2i(-1,+1), //2
|
||
|
|
Point2i( 0,-1), //3
|
||
|
|
Point2i( 0,+1), //4
|
||
|
|
Point2i(+1,-1), //5
|
||
|
|
Point2i(+1, 0), //6
|
||
|
|
Point2i(+1,+1) }; //7
|
||
|
|
|
||
|
|
static const int neighbour_cost[8]={
|
||
|
|
14,
|
||
|
|
10,
|
||
|
|
14,
|
||
|
|
10,
|
||
|
|
10,
|
||
|
|
14,
|
||
|
|
10,
|
||
|
|
14
|
||
|
|
};
|
||
|
|
|
||
|
|
static const int neighbour_parent[8]={
|
||
|
|
7,
|
||
|
|
6,
|
||
|
|
5,
|
||
|
|
4,
|
||
|
|
3,
|
||
|
|
2,
|
||
|
|
1,
|
||
|
|
0,
|
||
|
|
};
|
||
|
|
|
||
|
|
while(true) {
|
||
|
|
|
||
|
|
if (pending.size() == 0) {
|
||
|
|
_unlock();
|
||
|
|
return DVector<Point2>(); // points don't connect
|
||
|
|
}
|
||
|
|
Point2i current;
|
||
|
|
int lc=0x7FFFFFFF;
|
||
|
|
{ //find the one with the least cost
|
||
|
|
|
||
|
|
Set<Point2i>::Element *Efound=NULL;
|
||
|
|
for (Set<Point2i>::Element *E=pending.front();E;E=E->next()) {
|
||
|
|
|
||
|
|
int cc =CELL_COST(E->get());
|
||
|
|
if (cc<lc) {
|
||
|
|
lc=cc;
|
||
|
|
current=E->get();
|
||
|
|
Efound=E;
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
pending.erase(Efound);
|
||
|
|
}
|
||
|
|
|
||
|
|
Cell &c = cells[CELL_INDEX(current)];
|
||
|
|
|
||
|
|
//search around other cells
|
||
|
|
|
||
|
|
|
||
|
|
int accum_cost = (from==current) ? 0 : cells[CELL_INDEX((current + neighbour_rel[c.parent]))].cost;
|
||
|
|
|
||
|
|
bool done=false;
|
||
|
|
|
||
|
|
for(int i=0;i<8;i++) {
|
||
|
|
|
||
|
|
Point2i neighbour=current+neighbour_rel[i];
|
||
|
|
if (neighbour.x<0 || neighbour.y<0 || neighbour.x>=width || neighbour.y>=height)
|
||
|
|
continue;
|
||
|
|
|
||
|
|
Cell &n = cells[CELL_INDEX(neighbour)];
|
||
|
|
if (n.solid)
|
||
|
|
continue; //no good
|
||
|
|
|
||
|
|
int cost = neighbour_cost[i]+accum_cost;
|
||
|
|
|
||
|
|
if (n.visited && n.cost < cost)
|
||
|
|
continue;
|
||
|
|
|
||
|
|
n.cost=cost;
|
||
|
|
n.parent=neighbour_parent[i];
|
||
|
|
n.visited=true;
|
||
|
|
pending.insert(neighbour);
|
||
|
|
if (neighbour==to)
|
||
|
|
done=true;
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
if (done)
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
// go througuh poins twice, first compute amount, then add them
|
||
|
|
|
||
|
|
Point2i current=to;
|
||
|
|
int pcount=0;
|
||
|
|
|
||
|
|
while(true) {
|
||
|
|
|
||
|
|
Cell &c = cells[CELL_INDEX(current)];
|
||
|
|
c.visited=true;
|
||
|
|
pcount++;
|
||
|
|
if (current==from)
|
||
|
|
break;
|
||
|
|
current+=neighbour_rel[ c.parent ];
|
||
|
|
}
|
||
|
|
|
||
|
|
//now place them in an array
|
||
|
|
DVector<Vector2> result;
|
||
|
|
result.resize(pcount);
|
||
|
|
|
||
|
|
DVector<Vector2>::Write res=result.write();
|
||
|
|
|
||
|
|
current=to;
|
||
|
|
int pidx=pcount-1;
|
||
|
|
|
||
|
|
while(true) {
|
||
|
|
|
||
|
|
Cell &c = cells[CELL_INDEX(current)];
|
||
|
|
res[pidx]=current;
|
||
|
|
pidx--;
|
||
|
|
if (current==from)
|
||
|
|
break;
|
||
|
|
current+=neighbour_rel[ c.parent ];
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
//simplify..
|
||
|
|
|
||
|
|
|
||
|
|
if (p_optimize) {
|
||
|
|
|
||
|
|
int p=pcount-1;
|
||
|
|
while(p>0) {
|
||
|
|
|
||
|
|
|
||
|
|
int limit=p;
|
||
|
|
while(limit>0) {
|
||
|
|
|
||
|
|
limit--;
|
||
|
|
if (!_can_go_straigth(res[p],res[limit]))
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
if (limit<p-1) {
|
||
|
|
int diff = p-limit-1;
|
||
|
|
pcount-=diff;
|
||
|
|
for(int i=limit+1;i<pcount;i++) {
|
||
|
|
|
||
|
|
res[i]=res[i+diff];
|
||
|
|
}
|
||
|
|
}
|
||
|
|
p=limit;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
res=DVector<Vector2>::Write();
|
||
|
|
result.resize(pcount);
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
|
||
|
|
Size2 ImagePathFinder::get_size() const {
|
||
|
|
|
||
|
|
return Size2(width,height);
|
||
|
|
}
|
||
|
|
bool ImagePathFinder::is_solid(const Point2& p_pos) {
|
||
|
|
|
||
|
|
|
||
|
|
Point2i pos = p_pos;
|
||
|
|
|
||
|
|
ERR_FAIL_COND_V(pos.x<0,true);
|
||
|
|
ERR_FAIL_COND_V(pos.y<0,true);
|
||
|
|
ERR_FAIL_COND_V(pos.x>=width,true);
|
||
|
|
ERR_FAIL_COND_V(pos.y>=height,true);
|
||
|
|
|
||
|
|
return cell_data[pos.y*width+pos.x].solid;
|
||
|
|
}
|
||
|
|
|
||
|
|
void ImagePathFinder::create_from_image_alpha(const Image& p_image) {
|
||
|
|
|
||
|
|
ERR_FAIL_COND(p_image.get_format() != Image::FORMAT_RGBA);
|
||
|
|
width = p_image.get_width();
|
||
|
|
height = p_image.get_height();
|
||
|
|
DVector<uint8_t> data = p_image.get_data();
|
||
|
|
cell_data.resize(width * height);
|
||
|
|
DVector<uint8_t>::Read read = data.read();
|
||
|
|
DVector<Cell>::Write write = cell_data.write();
|
||
|
|
for (int i=0; i<width * height; i++) {
|
||
|
|
Cell cell;
|
||
|
|
cell.data = 0;
|
||
|
|
cell.solid = read[i*4+3] < 128;
|
||
|
|
write[i] = cell;
|
||
|
|
};
|
||
|
|
};
|
||
|
|
|
||
|
|
|
||
|
|
void ImagePathFinder::_bind_methods() {
|
||
|
|
|
||
|
|
ObjectTypeDB::bind_method(_MD("find_path","from","to","optimize"),&ImagePathFinder::find_path,DEFVAL(false));
|
||
|
|
ObjectTypeDB::bind_method(_MD("get_size"),&ImagePathFinder::get_size);
|
||
|
|
ObjectTypeDB::bind_method(_MD("is_solid","pos"),&ImagePathFinder::is_solid);
|
||
|
|
ObjectTypeDB::bind_method(_MD("create_from_image_alpha"),&ImagePathFinder::create_from_image_alpha);
|
||
|
|
}
|
||
|
|
|
||
|
|
ImagePathFinder::ImagePathFinder()
|
||
|
|
{
|
||
|
|
|
||
|
|
cells=NULL;
|
||
|
|
width=0;
|
||
|
|
height=0;
|
||
|
|
}
|