godot/scene/resources/image_path_finder.cpp

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