|
@@ -1,945 +0,0 @@
|
|
|
-#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 "visit.h"
|
|
|
-
|
|
|
-namespace{
|
|
|
-
|
|
|
-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=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)
|
|
|
-{
|
|
|
- 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());
|
|
|
- }
|
|
|
-}
|
|
|
-struct handle_path :visitor<std::shared_ptr<site>>
|
|
|
-{
|
|
|
- std::vector<base_path> ret;
|
|
|
- bool visit(std::shared_ptr<site> sit)
|
|
|
- {
|
|
|
- //auto&s=sites[i];
|
|
|
- const auto &s = *sit;
|
|
|
-
|
|
|
- if(!s.have_valid_path())
|
|
|
- continue;
|
|
|
-
|
|
|
- if(s.path(0).empty()||s.path(1).empty())
|
|
|
- continue;
|
|
|
- if(s[0].size()<2)
|
|
|
- continue;
|
|
|
- line_v l000 = s[0][0][0];
|
|
|
- line_v l010 = s[0][1][0];
|
|
|
- if(l000.is_same(l010))
|
|
|
- {
|
|
|
- printf("same....%d",s.m_id);
|
|
|
- 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(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;
|
|
|
-// if(s[0].size()<2)
|
|
|
-// continue;
|
|
|
-// line_v l000 = s[0][0][0];
|
|
|
-// line_v l010 = s[0][1][0];
|
|
|
-// if(l000.is_same(l010))
|
|
|
-// {
|
|
|
-// printf("same....%d",s.m_id);
|
|
|
-// 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;
|
|
|
-//
|
|
|
-// }
|
|
|
-// }
|
|
|
-// }
|
|
|
-
|
|
|
-/*
|
|
|
-
|
|
|
- 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;
|
|
|
- }
|
|
|
- }
|
|
|
-*/
|
|
|
- log_path(ret,v);
|
|
|
- printf("++++++++++++++++++++++++++++++++++++++++++++");
|
|
|
- std::sort(ret.begin(),ret.end());
|
|
|
- log_path(ret,v);
|
|
|
- printf("++++++++++++++++++++++++++++++++++++++++++++");
|
|
|
- ret.erase(std::unique(ret.begin(),ret.end()),ret.end());
|
|
|
- log_path(ret,v);
|
|
|
- printf("++++++++++++++++++++++++++++++++++++++++++++");
|
|
|
-
|
|
|
- std::sort(ret.begin(),ret.end(),[&v](const base_path&p1,const 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](const base_path&p1,const 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)//相交小于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);
|
|
|
- });
|
|
|
- */
|
|
|
-
|
|
|
-#ifdef __DEBUG__
|
|
|
- std::sort(ret2.begin(),ret2.end(),[&v](const base_path&p1,const 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(ret2,v);
|
|
|
-#endif
|
|
|
-
|
|
|
- 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]);
|
|
|
-
|
|
|
- printf("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=-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;
|
|
|
- }
|
|
|
-
|
|
|
- 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 {};
|
|
|
- }
|
|
|
-};
|
|
|
-
|
|
|
-
|
|
|
-std::atomic<int> g_init_flag(0);
|
|
|
-graph g_graph;
|
|
|
-
|
|
|
-}//namespace
|
|
|
-
|
|
|
-void card_path::init(const sit_list&sites)
|
|
|
-{
|
|
|
- //Ensure only ont thread can init path.
|
|
|
- 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);
|
|
|
- handle_path hp;
|
|
|
- card_list::instance()->accept(hp);
|
|
|
-
|
|
|
- g_graph.init(v_list,opath);
|
|
|
-
|
|
|
- ++g_init_flag;
|
|
|
- }
|
|
|
- //if != 2 then wait here until init over.
|
|
|
- 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));
|
|
|
-}
|
|
|
-
|
|
|
-#ifdef __DEBUG__
|
|
|
-
|
|
|
-void test_at_path(card_path&cp,const point&pt)
|
|
|
-{
|
|
|
- if(cp.is_at_path(pt))
|
|
|
- printf("test (%.3lf,%.3lf) is-at-path:true\n",pt.x,pt.y);
|
|
|
- else
|
|
|
- printf("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++)
|
|
|
- printf("x=%.3lf,y=%.3lf\n",rc[i].x,rc[i].y);
|
|
|
-}
|
|
|
-
|
|
|
-void test_find_poss_path(const card_path&cp ,const point&from)
|
|
|
-{
|
|
|
- printf("\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++)
|
|
|
- {
|
|
|
- printf("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());
|
|
|
- sites->load("data_reader_antenna.txt","path_tof.txt");
|
|
|
-
|
|
|
- card_path cp;
|
|
|
-
|
|
|
- cp.init(*sites);
|
|
|
-#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
|
|
|
-
|