server/pointSmooth/card_path.cpp

#include "stdafx.h"
#include <stdio.h>
#include <algorithm>
#include <unordered_map>
#include <set>
#include <memory>
#include <atomic>
#include <thread>
#include "zlist.h"
#include "line.h"
#include "ant.h"

#include "card_path.h"
#include "ProcessRemodule.h"


NAMESPACE_POINT_BEGIN (NAMESPACE_POINT)

static const double PI=3.1416;
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)
	{
		at(0)=i1;
		at(1)=i2;
	}

	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()
	{
		printf("%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=1)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=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);
}

#if 0
void log_path(const std::vector<base_path>&path,const vertex_list&v_list)
{
	printf("%s\n","----------------------------------------------------------------");
	for(int i=0,len=path.size();i<len;i++)
	{
		double c=path[i].arg(v_list)/PI*180;
		printf("base_path %.6lf, %03d,(%d,%s),(%d,%s)\n",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());
	}
}
#endif

static std::vector<base_path> init_path(const sit_list&sites,vertex_list&v)
{
	std::vector<base_path> ret;
	for(uint32_t i=0;i<sites.m_list.size();i++)
	{
		auto&s=sites[i];

		if(!s.have_valid_path())
			continue;

		if(s.path(0).empty()||s.path(1).empty())
			continue;

		line x(s.path(0),s.path(1));

		if(x.contain(s,2.5))
		{
			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;
			continue;
		}

		for(int j=0;j<2;j++)
		{
			if(!s.path(j).empty() && s.dist(s.path(j))<2)
				continue;

			point p=s.path(j);
			int p0=v.add(point::min_(s,p),0,s.m_id);
			int p1=v.add(point::max_(s,p),0,s.m_id);
			ret.push_back(base_path(p0,p1));
			ret.back().sid=s.m_id;
		}
	}

	std::sort(ret.begin(),ret.end());
	ret.erase(std::unique(ret.begin(),ret.end()),ret.end());

	std::sort(ret.begin(),ret.end(),[&v](base_path&p1,base_path&p2){
		double arg=p1.arg(v)-p2.arg(v);

		if(fabs(arg)<0.1)
		{
			return v[p1[0]]<v[p2[0]];
		}

		return arg<0;
	});

//	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,2.5))
				continue;

			line_v ij=lj.projection(li);

			if(ij.empty())
				continue;

			point p0=point::min_(v[ret[j][0]],v[ret[i][0]]);
			point p1=point::max_(v[ret[j][1]],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());
#ifdef __DEBUG__
	std::sort(ret.begin(),ret.end(),[&v](base_path&p1,base_path&p2){
		double arg=p1.arg(v)-p2.arg(v);

		if(fabs(arg)<0.1)
		{
			return v[p1[0]]<v[p2[0]];
		}

		return arg<0;
	});

	log_path(ret,v);
#endif

	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;

			double arg=fabs(ret[i].as_line(v).arg()-ret[j].as_line(v).arg());
			while(arg>PI/2) 
				arg-=PI/2;

			if(arg/PI*180<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];
		});

		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.erase(std::remove_if(ret2.begin(),ret2.end(),[&v](base_path&p){
		point&p0=v[p[0]];
		point&p1=v[p[1]];

		return p0.dist(p1)<0.1;
	}),ret2.end());

	std::sort(ret2.begin(),ret2.end());
	ret2.erase(std::unique(ret2.begin(),ret2.end()),ret2.end());

/*
	ret.clear();
	for(int i=0,len=ret2.size();i<len;i++)
	{
		std::vector<line_v> tmp;
		tmp.push_back(ret2[i].as_line(v));
		for(int j=0;j<len;j++)
		{
			if(i==j) continue;

			if(ret[j].level(v)<ret[i].level(v))
				continue;

			line l1=ret2[j].as_line(v);
		}
	}

	std::sort(ret2.begin(),ret2.end(),[&v](base_path&p1,base_path&p2){
		return p1.arg(v)<p2.arg(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";
		}
	}

	geolist()
	{
	}

	~geolist()
	{
	}
};

struct graph
{
	vertex_list v;
	std::vector<std::vector<int>>  d;
	std::vector<std::array<int,2>> l;
	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]]);
			std::array<int,2> tf  = {from,to};
			l.push_back(tf);

			int id=l.size()-1;

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

			debug_print_syslog(0,"line:%s\n",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=-2;i<=2;i++)
				for(int j=-2;j<=2;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;
	}

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

		if(f<0 || t<0 || f==t)
			return tmp;

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

std::atomic<int> g_init_flag(0);
graph g_graph;

void card_path::init(const sit_list&sites)
{
	int expect=0;
	if(g_init_flag.compare_exchange_strong(expect,1))
	{
		vertex_list v_list;
		v_list.add(point(0,0),0,-1);
		std::vector<base_path> opath=init_path(sites,v_list);
		g_graph.init(v_list,opath);

		++g_init_flag;
	}

	while(g_init_flag.load()!=2)
	{
		std::this_thread::sleep_for (std::chrono::seconds(1));
	}
}


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.find(from,to);
}
bool card_path::is_at_path(const point&pt)const
{
	return g_graph.is_at_path(pt);
}
std::vector<line_v>  card_path::find_possible_path(const point&from,double dist) const
{
	return std::move(g_graph.find_possible_path(from,dist));
}

NAMESPACE_POINT_END(NAMESPACE_POINT)

#ifdef __DEBUG__
int main()
{
	std::unique_ptr<sit_list> sites(new sit_list());
	sites->load("data_reader_antenna.txt","dat_reader_path_tof.txt");

	card_path cp;

	cp.init(*sites);

	{
		std::vector<point> rc=cp.find_path(point(4707.635909,107.789284),point(4652.845741, 54.043673));
		for(int i=0;i<rc.size();i++)
			printf("x=%lf,y=%lf\n",rc[i].x,rc[i].y);
	}

	printf("%s\n","---------------------");
	{
		std::vector<point> rc=cp.find_path(point(4220,-75),point(4947.25, -156.76));
		for(int i=0;i<rc.size();i++)
			printf("x=%lf,y=%lf\n",rc[i].x,rc[i].y);
	}
	printf("%s\n","---------------------");

	{
		std::vector<point> rc=cp.find_path(point(4220,-75),point(4727, 200));
		for(int i=0;i<rc.size();i++)
			printf("x=%lf,y=%lf\n",rc[i].x,rc[i].y);
	}

	return 0;
}

#endif