diff options
Diffstat (limited to 'scene/resources/image_path_finder.cpp')
| -rw-r--r-- | scene/resources/image_path_finder.cpp | 427 |
1 files changed, 427 insertions, 0 deletions
diff --git a/scene/resources/image_path_finder.cpp b/scene/resources/image_path_finder.cpp new file mode 100644 index 000000000..1a7758789 --- /dev/null +++ b/scene/resources/image_path_finder.cpp @@ -0,0 +1,427 @@ +/*************************************************************************/ +/* 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;
+}
|