server/card_path.cpp

#include <stdio.h>
#include <algorithm>
#include <unordered_map>
#include <set>
#include <memory>
#include <atomic>
#include <thread>
#include <list>
#include "zlist.h"
#include "line.h"
#include "ant.h"
#include "config_file.h"
#include "card_path.h"
#include "visit.h"

namespace{

//static const double PI  = 3.1415926;
//static const double TPI = 2*PI;

inline bool eq(double a,double b,double e=0.0001)
{
	return fabs(a-b)<e;
}

struct vertex:point
{
	float level;
	int sid0,sid1;
	vertex()
		:level(0)
		,sid0(0)
		,sid1(0)
	{}

	vertex(const point&pt,float level_=0,int site_id0=0,int site_id1=0)
		:point(pt)
		,level(level_)
		,sid0(site_id0)
		,sid1(site_id1)
	{
	}

	std::string to_string()const
	{
		char buf[128];
		sprintf(buf,"%.2f,%.3lf,%d,%d,%.2lf",x,y,sid0,sid1,level/PI*180);

		return buf;
	}
};

struct vertex_list;
struct base_path:std::array<int,2>
{
	int sid;
	base_path(int i1=-1,int i2=-1,int _sid=0)
	{
		at(0)=i1;
		at(1)=i2;
		sid=_sid;
	}

	bool operator<(const base_path&o)const
	{
		int c=at(0)-o.at(0);
		if(c!=0)
			return c<0;

		return at(1)-o.at(1)<0;
	}

	point  cross(const vertex_list&v_list, const base_path&o)const;
	bool   contains(const vertex_list&v_list, const point&o, double e)const;
	double arg(const vertex_list&v)const;
	int    type(const vertex_list&v)const;
	line_v as_line(const vertex_list&v)const;
	int    level(const vertex_list&v)const;
};

struct vertex_list
{
	std::vector<vertex> v;

	std::string to_string()const
	{
		std::string ret;
		ret.reserve(1024);

		char buf[128];
		for(int i=0,len=v.size();i<len;i++)
		{
			sprintf(buf,"vertex:%3d,%s\n",i,v[i].to_string().c_str());

			ret+=buf;
		}

		return std::move(ret);
	}

	void log()
	{
		log_info("%s",to_string().c_str());
	}


	static unsigned hash(const point&p)
	{
		return (((((unsigned)p.x)>>2)<<16) + (((unsigned)p.y)>>2));
	}

	int find(const point&p,double x=0.01)const
	{
		int rc=-1;
		double t;

		for(int i=0,len=v.size();i<len;i++)
		{
			t=p.dist(v[i]);
			if(t<x)
			{
				rc=i;
				x=t;
			}
		}

		return rc;
	}

	int add(const point&p,float level=0,int sid0=0,int sid1=0)
	{
		int i=find(p);

		if(i<0)
		{
			v.push_back(vertex(p,level,sid0,sid1));
			return v.size()-1;
		}

		vertex&v0=v[i];

		v0.level=std::max(v0.level,level);

		return i;
	}

	int size()const
	{
		return v.size();
	}

	const vertex&operator[](int i)const
	{
		return v[i];
	}

	vertex&operator[](int i)
	{
		return v[i];
	}
};

int base_path::level(const vertex_list&v)const
{
	const vertex&p0=v[at(0)];
	const vertex&p1=v[at(1)];

	return p0.level+p1.level;
}

line_v base_path::as_line(const vertex_list&v)const
{
	const vertex&p0=v[at(0)];
	const vertex&p1=v[at(1)];

	return line_v(p0,p1);
}

double  base_path::arg(const vertex_list&v)const
{
	return as_line(v).arg();
}

bool  base_path::contains(const vertex_list&v, const point&o, double e)const
{
	return as_line(v).contain(o,e);
}

point  base_path::cross(const vertex_list&v, const base_path&o)const
{
	line_v l0=as_line(v);
	line_v l1=o.as_line(v);

	return l0.crossing(l1);
}
void log_path(const std::vector<base_path>&path,const vertex_list&v_list,const char*add_info="")
{
	log_info("%s\n","----------------------------------------------------------------");
	for(int i=0,len=path.size();i<len;i++)
	{
		double c=path[i].arg(v_list)/PI*180;
		log_info("base_path:%s %.6lf, %03d,(%d,%s),(%d,%s)",add_info,c,i,path[i][0],v_list[path[i][0]].to_string().c_str(),path[i][1],v_list[path[i][1]].to_string().c_str());
	}
}
struct handle_path :visitor<std::shared_ptr<site>>
{
	std::vector<base_path> ret;
	vertex_list v;
	handle_path()
	{
		v.add(point(0,0),0,-1);
	}
	bool visit(std::shared_ptr<site> sit)
	{
		//auto&s=sites[i];
		const auto &s = *sit;

		if(!s.have_valid_path())
			return false;

		if(s.path(0).empty()||s.path(1).empty())
			return false;
        if(s[0].size()<2)
			return false;
        line_v l000 = s[0][0][0]; //天线->分站天线路径集合->路径
        line_v l010 = s[0][1][0];
        if(l000.is_same(l010))
        {
			int p0=v.add(point::min(s.path(0),s.path(1)),0,s.m_id);
			int p1=v.add(point::max(s.path(0),s.path(1)),0,s.m_id);

		    ret.push_back(base_path(p0,p1));
			ret.back().sid=s.m_id;
        }
        else
        {
            point px = l000.line::crossing(l010);
            for(int i=0;i<2;i++)
            {
			    int p0=v.add(point::min(px,s.path(i)),0,s.m_id);
			    int p1=v.add(point::max(px,s.path(i)),0,s.m_id);

		        ret.push_back(base_path(p0,p1));
			    ret.back().sid=s.m_id;
            }
        }
        for(int i=0;i<2;i++)
        {
            if(!s[0][i][1].empty())
            {
			    int p0=v.add(point::min(s[0][i][1][0],s[0][i][1][1]),0,s.m_id);
			    int p1=v.add(point::max(s[0][i][1][0],s[0][i][1][1]),0,s.m_id);

		        ret.push_back(base_path(p0,p1));
			    ret.back().sid=s.m_id;
            }
        }
		return true;
	}
};
static std::vector<base_path> init_path(std::vector<base_path> & ret,vertex_list&v)
{
	do
	{
		config_file cfg;
		if(cfg.open("../etc/path-not-cross.ini"))
			break;

		auto kset=cfg.keys();
		if(kset.empty())
			break;

		for(auto k:kset)
		{
			if(strncmp(k,"add_path",8)!=0)
				continue;

			const char* vv=cfg.get(k,"");

			point p0;
			point p1;
			if(sscanf(vv,"%lf,%lf,%lf,%lf",&p0.x,&p0.y,&p1.x,&p1.y)==4)
			{
				point p_0=point::min(p0,p1);
				point p_1=point::max(p0,p1);
				int p00=v.add(p_0,0,0);
				int p10=v.add(p_1,0,0);

				ret.push_back(base_path(p00,p10,0));
			}
		}
		break;
	}while(1);

	auto remove_dup=[](std::vector<base_path>&ret){
		std::sort(ret.begin(),ret.end());
		ret.erase(std::unique(ret.begin(),ret.end()),ret.end());
	};

	auto remove_empty=[](std::vector<base_path>&ret,vertex_list&v){
		ret.erase(std::remove_if(ret.begin(),ret.end(),[&v](base_path&p){
						point&p0=v[p[0]];
						point&p1=v[p[1]];
						return p0.dist(p1)<.00001;
						}),
					ret.end());
	};

	auto sort_path=[](std::vector<base_path> & ret,vertex_list&v){
		std::sort(ret.begin(),ret.end(),[&v](const base_path&p1,const base_path&p2){
				return point::min(v[p1[0]],v[p1[1]]) < point::min(v[p2[0]],v[p2[1]]);
		});
	};

	remove_dup(ret);
	sort_path(ret,v);
	log_path(ret,v,"合并前");
	//合并相同
	for(int i=0,len=ret.size();i<len;i++)
	{
		line_v li=ret[i].as_line(v);
		for(int j=i+1;j<len;j++)
		{
			line_v lj=ret[j].as_line(v);
			if(!lj.is_same(li,1)) //li线段两个点在lj直线上
				continue;
			line_v ij=lj.projection(li);//重叠

			if(ij.empty()&&ij[0].empty()&&ij[0].empty())
				continue;			

			point p0=li.as_line().projection(v[ret[j][0]]);
			point p1=li.as_line().projection(v[ret[j][1]]);

			p0=point::min(p0,v[ret[i][0]]);
			p1=point::max(p1,v[ret[i][1]]);

			ret[j][0]=v.add(p0,0);
			ret[j][1]=v.add(p1,0);

			ret[i][0]=0;
			ret[i][1]=0;
			break;
		}
	}

	ret.erase(std::remove_if(ret.begin(),ret.end(),[&v](const base_path&p){
		return p[0]==0||p[1]==0;
	}),ret.end());

	sort_path(ret,v);
	log_path(ret,v,"合并后");

	//非路径交点配置
	std::vector<point> no_cross;
	auto in_no_cross=[&no_cross](const point&pt){
		for(auto p:no_cross)
		{
			if(p.dist(pt)<3)
				return true;
		}

		return false;
	};

	do
	{
		config_file cfg;
		if(cfg.open("../etc/path-not-cross.ini"))
			break;

		auto kset=cfg.keys();
		if(kset.empty())
			break;

		for(auto k:kset)
		{
			if(strncmp(k,"pt",2)!=0)
				continue;

			const char* v=cfg.get(k,"");

			point tmp;
			if(sscanf(v,"%lf,%lf",&tmp.x,&tmp.y)==2)
				no_cross.push_back(tmp);
		}

		for(auto p:no_cross)
		{
			log_info("no_cross_point:x=%lf,y=%lf",p.x,p.y);
		}
		break;
	}while(1);

	std::vector<std::vector<int>> p0(ret.size());
	std::vector<base_path> ret2;
	for(int i=0,len=ret.size();i<len;i++)
	{
		p0[i].push_back(ret[i][0]);
		for(int j=i+1;j<len;j++)
		{
			if(i==j) continue;

			point cr=ret[i].cross(v,ret[j]);
			if(cr.empty())
				continue;
			
			if(in_no_cross(cr))
				continue;
			
			double arg=fabs(ret[i].as_line(v).arg()-ret[j].as_line(v).arg());
			while(arg>PI/2) 
				arg=PI-arg;
			
			if(arg/PI*180<5)//相交小于5度，不切分已有路径
				continue;
			
			int id=v.add(cr,arg,v[ret[i][0]].sid0,v[ret[j][0]].sid0);
			p0[i].push_back(id);
			p0[j].push_back(id);
		}
		p0[i].push_back(ret[i][1]);

		std::sort(p0[i].begin(),p0[i].end(),[&v](int i0,int i1){ return v[i0]<v[i1]; });
#if 0
		if(p0[i].size())
		{
			for(int v=0;v<2;v++)
			{
				if(v[ret[i][v]].dist(v[p0[i].front()])<3)
					continue;
				if(v[ret[i][v]].dist(v[p0[i].back()])<3)
					continue;

				p0[i].push_back(ret[i][v]);
				std::sort(p0[i].begin(),p0[i].end(),[&v](int i0,int i1){ return v[i0]<v[i1]; });
			}
		}
		else
		{
			std::sort(p0[i].begin(),p0[i].end(),[&v](int i0,int i1){ return v[i0]<v[i1]; });
		}
#endif

		auto it=std::unique(p0[i].begin(),p0[i].end());
		p0[i].erase(it,p0[i].end());

		for(int j=1,cnt=p0[i].size();j<cnt;j++)
		{
			ret2.push_back(base_path(p0[i][j - 1], p0[i][j]));
			ret2.back().sid = ret[i].sid;
		}
	}

	remove_empty(ret2,v);
	remove_dup(ret2);
	sort_path(ret,v);
	log_path(ret2,v,"拆分后");

	return std::move(ret2);
}



#if 0
struct ghash
{
	static std::tuple<int,int> decode(unsigned h)
	{
		const uint64_t S=0x8000000000000000;

		int x=0,y=0;
		for(int i=0;i<64;i++)
		{
			x<<=1;
			y<<=1;

			if(h&S) 
				x|=1;
			h<<=1;

			if(h&S) 
				y|=1;
			h<<=1;
		}

		return std::make_tuple(x-2147483648,y-2147483648);
	}

	static uint64_t encode(int64_t x, int64_t y)
	{
		return encode_(x+2147483648,y+2147483648);
	}

public: //test
	static void test_code(int64_t x,int64_t y)
	{
		uint64_t h=ghash::encode(x,y);
		auto t=ghash::decode(h);

		printf("src x=%X,y=%X hash=%X,check x=%X,y=%X\n",x,y,h,std::get<0>(t),std::get<1>(t));
	}

	static void test()
	{
		for(int i=0;i<10;i++)
		{
			test_code((4<<i)-1,(4<<i)-1);
			test_code((4<<i)-1,(4<<i));
			test_code((4<<i)-1,(4<<i)-1);
			test_code((4<<i),(4<<i)-1);
		}
	}
private:
	static unsigned encode_(unsigned short x, unsigned short y)
	{
		const unsigned S=0x8000;
		unsigned r=0;
		for(int i=0;i<16;i++)
		{
			r<<=2;
			if(x&S)
			{
				r|=(y&S)?3:2;
			}
			else
			{
				if(y&S) r|=1;
			}

			x<<=1;
			y<<=1;
		}
		
		return r;
	}
};
#endif


struct geolist
{
	std::unordered_map<uint64_t,int> geo2path;
public:

	static uint64_t encode(double x0,double y0)
	{
		uint64_t x=(uint32_t)(x0*10);
		uint64_t y=(uint32_t)(y0*10);

		return (x<<32)|y;
	}

	int find(double x,double y)const
	{
		uint64_t h=encode(x,y);

		auto it=geo2path.find(h);

		if(it==geo2path.end())
			return -1;

		return it->second;
	}

	void add(double x,double y,int id)
	{
		uint64_t h=encode(x,y);
		geo2path.insert(std::make_pair(h,id));
	}

	int size()
	{
		return geo2path.size();
	}

	void print()
	{
		for(auto it=geo2path.begin();it!=geo2path.end();++it)
		{
			//std::cout<<it->first<<"\n";
            printf("%ld\n",it->first);
		}
	}

	geolist()
	{
	}

	~geolist()
	{
	}
};

struct graph
{
	vertex_list v;
	std::vector<std::vector<int>>  d;   //索引为起始顶点，数组内容为目标点
	std::vector<std::array<int,2>> l;	//bast_path list
	geolist geo;

	graph()
	{}

	void init(const vertex_list&v_,const std::vector<base_path>&bp)
	{
		for(auto&p:bp)
		{
			int from=v.add(v_[p[0]]);
			int to=v.add(v_[p[1]]);

			l.push_back({from,to});

			int id=l.size()-1;

			line_v lv(v[from],v[to]);

			log_info("graph::init() site:%d line:%s\n",p.sid, lv.to_string().c_str());

			double cos=lv.cos_k();
			double sin=lv.sin_k();

			double x,y;
			for(double r=0,len=lv.length();r<len+0.1;r+=0.05)
			{
				x=lv[0].x+r*cos;
				y=lv[0].y+r*sin;

//				printf("x=%lf,y=%lf\n",x,y);

				for(int i=-1;i<=1;i++)
				for(int j=-1;j<=1;j++)
				{
					geo.add(x+i/10.,y+j/10.,id);
				}
			}

			add(from,to);
		}
	}

	std::vector<line_v>  find_possible_path(const point&from,double dist) const
	{
		std::vector<line_v> ret;

		int start_pt_id=v.find(from,dist);
		if(start_pt_id==-1)
			return std::move(ret);

		point start_pt=v[start_pt_id];

		for(uint32_t i=0;i<d[start_pt_id].size();i++)
		{
			ret.push_back(line_v(start_pt,v[d[start_pt_id][i]]));
		}

		return std::move(ret);
	}

	void add(int from,int to)
	{
		if((int)d.size()<=from) d.resize(from+1);
		if((int)d.size()<=to) d.resize(to+1);

		d[from].push_back(to);
		d[to].push_back(from);
	}

	bool find(std::vector<int>&ret,int from,std::array<int,2>&to,std::set<int>&ex,int level)
	{
		if(level--<0)
			return false;

		ret.push_back(from);

		for(int p:d[from]) //在当前路径
		{
			if(ex.find(p)!=ex.end()) //找过
				continue;

			if(p==to[0]||p==to[1])//找到
				return true;
//			{
//				ret.push_back(p);
//				return true;
//			}
		}

		ex.insert(from);

		for(int p:d[from])//在下级路径
		{
			if(ex.find(p)!=ex.end())//找过
				continue;
			if(find(ret,p,to,ex,level)) //找到
			{
				return true;
			}
		}

		ret.pop_back();
		return false;
	}

	bool is_at_path(const point&pt)const
	{
		return geo.find(pt.x,pt.y)>=0;
	}

	struct item
	{
		item(int _vid=-1, int _g=0,int _h=0,std::shared_ptr<item> _parent=nullptr)
			:vid(_vid)
			,g(_g)
			,h(_h)
			,parent(_parent)
		{
		}
		int vid=-1,g=0,h=0;
		std::shared_ptr<item> parent;

		int cost()const {return g+h;}

		bool operator< (const item&i) const
		{
			return vid<i.vid;
		}
	};

	std::vector<point> find2(const point&from,const point&to)
	{
		int f=geo.find(from.x,from.y);
		int t=geo.find(to.x,to.y);

		if(f<0 || t<0 || f==t)
			return {};

		std::set<std::shared_ptr<item>> open_set;
		std::set<int> close_set;
		open_set.insert(std::make_shared<item>(l[f][0],from.dist(v[l[f][0]]),to.dist(v[l[f][0]])));
		open_set.insert(std::make_shared<item>(l[f][1],from.dist(v[l[f][1]]),to.dist(v[l[f][1]])));

		std::shared_ptr<item> cur;
		while(open_set.size()>0)
		{
			cur=*open_set.begin();
			for(auto&i:open_set)
			{
				if(i->cost()<cur->cost())
					cur=i;
			}
			open_set.erase(cur);
			close_set.insert(cur->vid);
			if(cur->vid==l[t][0]||cur->vid==l[t][1])
				break;

			for(auto vid:d[cur->vid])
			{
				if(close_set.find(vid)!=close_set.end())
					continue;

				open_set.insert(std::make_shared<item>(vid,cur->g+v[cur->vid].dist(v[vid]),to.dist(v[vid]),cur));
			}
		}

		if(open_set.size()==0)
		{
			return {};
		}

		std::vector<point> rc;
		while(cur.get())
		{
			rc.push_back(v[cur->vid]);
			cur=cur->parent;
		}

		std::reverse(rc.begin(),rc.end());
		rc.push_back(to);
		auto bp=from;
		auto it1=rc.begin();
		for(auto it2=it1+1;it1!=rc.end() && it2!=rc.end();it2++)
		{
			if(line(bp,*it1).dist(*it2)<0.1)
			{
				*it1=*it2;
			}
			else
			{
				bp=*it1++;
				*it1=*it2;
			}
		}

		rc.erase(it1,rc.end());

		if(rc.size() && from.dist(*rc.begin())<0.1)
		{
			rc.erase(rc.begin());
		}

		return std::move(rc);
	}

	std::vector<point> find(const point&from,const point&to)
	{
		int f=geo.find(from.x,from.y);
		int t=geo.find(to.x,to.y);

		if(f<0 || t<0 || f==t)
			return {};
#if 0
		std::vector<point> rc[2];
		for(int i=0;i<2;i++)
		{
			std::vector<int> ret;
			std::set<int> ex({l[f][1-i]});
			if(find(ret,l[f][i],l[t],ex,2))
			{
				for(auto id:ret)
					rc[i].push_back(v[id]);
			}
		}
		if(!rc[0].empty() && (rc[1].empty() || rc[0].size() < rc[1].size()))
			return std::move(rc[0]);
		else if(!rc[1].empty())
			return std::move(rc[1]);
#endif
		std::vector<int> ret;
		int size=v.size();
		std::vector<double> mat(size+1,-1);
		std::vector<double> trace(size+1,-1);
		std::vector<bool> val(size+2,false);
		mat[l[t][0]]=to.dist(v[l[t][0]]);
		mat[l[t][1]]=to.dist(v[l[t][1]]);
		int f0=l[f][0],f1=l[f][1];
		for(int i=0;i<2;i++)
		{
			if(mat[f0]!=-1 && mat[f1]!=-1)
				break;
			for(int j=0;j<size;j++)
				if(mat[j]!=-1)val[j]=true;
			for(int j=0;j<size;j++)
			{
				if(val[j]==false)continue;
				for(unsigned int k=0;k<d[j].size();k++)
				{
					double dist=v[j].dist(v[d[j][k]]);
					if(mat[d[j][k]]==-1 || mat[j]+dist < mat[d[j][k]])
					{
						mat[d[j][k]] = mat[j]+dist;
						trace[d[j][k]] = j;
					}
				}
			}
		}
		mat[f0]=mat[f0]+from.dist(v[l[f][0]]);
		mat[f1]=mat[f1]+from.dist(v[l[f][1]]);
		std::vector<point> rc;
		if(mat[f0]!=-1 && (mat[f1]==-1 || mat[f0] < mat[f1]))
		{
			int temp=f0;
			while(temp!=-1)
			{
				rc.push_back(v[temp]);
				temp=trace[temp];
			}
			return std::move(rc);
		}
		else if(mat[f1]!=-1)
		{
			int temp=f1;
			while(temp!=-1)
			{
				rc.push_back(v[temp]);
				temp=trace[temp];
			}
			return std::move(rc);
		}
		return {};
	}
};

safe_shared_ptr<graph> g_graph(std::make_shared<graph>());
}//namespace

void card_path::init()
{
	std::shared_ptr<graph> local_graph=std::make_shared<graph>();

	handle_path hp;
	sit_list::instance()->accept(hp);
	std::vector<base_path> opath=init_path(hp.ret,hp.v);
	local_graph->init(hp.v,opath);

	g_graph.set(local_graph);
}

card_path&card_path::inst()
{
	static card_path path;
	return path;
}

std::vector<point> card_path::find_path(const point&from,const point&to)const
{
	return g_graph.get()->find2(from,to);
}

bool card_path::is_at_path(const point&pt)const
{
	return g_graph.get()->is_at_path(pt);
}

std::vector<line_v>  card_path::find_possible_path(const point&from,double dist) const
{
	return std::move(g_graph.get()->find_possible_path(from,dist));
}

#ifdef __DEBUG__

void test_at_path(card_path&cp,const point&pt)
{
	if(cp.is_at_path(pt))
		log_info("test (%.3lf,%.3lf) is-at-path:true\n",pt.x,pt.y);
	else
		log_info("test (%.3lf,%.3lf) is-at-path:false\n",pt.x,pt.y);
}

void test_find_path(card_path&cp,const point&p1,const point&p2)
{
	printf("\nfind-path:  from=(%.3lf,%.3lf),to=(%.3lf,%.3lf)\n",p1.x,p1.y,p2.x,p2.y);
	std::vector<point> rc=cp.find_path(p1,p2);
	for(uint32_t i=0;i<rc.size();i++)
		log_info("x=%.3lf,y=%.3lf\n",rc[i].x,rc[i].y);
}

void test_find_poss_path(const card_path&cp ,const point&from)
{
	log_info("\nfind-poss_path:  from=(%.3lf,%.3lf)\n",from.x,from.y);
	std::vector<line_v> rc=cp.find_possible_path(from,3);
	for(uint32_t i=0;i<rc.size();i++)
	{
		log_info("poss path from=(%.3lf,%.3lf) to=(%.3lf,%.3lf)\n", rc[i][0].x,rc[i][0].y, rc[i][1].x,rc[i][1].y);
	}
}

int main()
{
//	std::unique_ptr<sit_list> sites(new sit_list());
	//sit_list::instance()->load("data_reader_antenna.txt","path_tof.txt");
	sit_list::instance()->load("1.txt","2.txt");

	card_path cp;

	cp.init();
#if 0
	{
		test_at_path(cp,point(4773.104654,-104.0887));

		test_at_path(cp,point(4727,-74.8));
		test_at_path(cp,point(4727,-75.0));
		test_at_path(cp,point(4727,-75.2));
		test_at_path(cp,point(4727,-75.8));
		test_at_path(cp,point(4727,-90));
		test_at_path(cp,point(4727,-89.6));
		test_at_path(cp,point(4727,-89.8));
		test_at_path(cp,point(4727,-90));
		test_at_path(cp,point(4727,-90.2));
		test_at_path(cp,point(4727,-90.4));
	}
#endif
	//test_at_path(cp,point(4726.90,376.85));
	test_find_path(cp,point(-227.6,174.9),point(-201.3,175.5));
	test_find_path(cp,point(-227.625219,174.945670),point(-170.085722,176.101574));
	test_find_path(cp,point(-393.1944,262.5324),point(-366.0754,274.1253));
	//test_find_path(cp,point(4637.000000,-75),point(4727,135));
#if 1
	{
		test_find_path(cp,point(4637.000000,-75),point(4727,135));
		test_find_path(cp,point(4637.000000,-75),point(4727,120));
		test_find_path(cp,point(4700,-75),point(4727,120));
		test_find_path(cp,point(4725,-75),point(4727,135));
		test_find_path(cp,point(4727,-89.7),point(6144.3,-523));
	}
#endif
#if 0
	{
		test_find_poss_path(cp, point(4727,-89));
		test_find_poss_path(cp, point(4727,-90));

		test_find_poss_path(cp, point(4724,-75));
		test_find_poss_path(cp, point(4725,-75));
		test_find_poss_path(cp, point(4726,-75));
		test_find_poss_path(cp, point(4727,-75));
		test_find_poss_path(cp, point(4728,-75));
		test_find_poss_path(cp, point(4727,-7));

		test_find_poss_path(cp, point(4726.6,33));
	}
#endif
	return 0;
}
#endif