aoj1033 クルクルロボット
#include<stdio.h>
#include<vector>
#include<set>
#include<math.h>
#include <algorithm>
#include<map>
#include<queue>
struct P{
double x,y;
bool operator<(const P& p)const{
if(x!=p.x)return x<p.x;
return y<p.y;
}
bool operator==(const P& p)const{
return x==p.x&&y==p.y;
}
bool operator>(const P& p)const{
if(x!=p.x)return x>p.x;
return y>p.y;
}
};
struct Line{
P p1,p2;
bool isUnion;//他の線に統合されたらtrueが設定される
};
struct cP{
double len;
int lineNo;
bool operator<(const cP& p)const{
return len<p.len;
}
};
std::vector<cP> cons[51];//線分のつながりを表す
Line lines[51];
struct robo{
int nowLine;//今いるラインNo
int nowPoint;//今いるポイントNo
double oldDX,oldDY;//一つ前に移動した移動量の向き
double cost;//回転コストの計
double len;
robo move(int nextPoint){
robo re;
double dx,dy;
dx=lines[nowLine].p2.x-lines[nowLine].p1.x;
dy=lines[nowLine].p2.y-lines[nowLine].p1.y;
//平行な線分は最初の時点ですべて取り除いているはずなので大丈夫
if(len>cons[nowLine][nextPoint].len){
dx=-dx;
dy=-dy;
}
double naiseki=dx*oldDX+dy*oldDY;
double lineLen=sqrt(dx*dx+dy*dy)*sqrt(oldDX*oldDX+oldDY*oldDY);
////printf("(len=%.3lf nai=%.3lf)",lineLen,naiseki);
if(lineLen==0){
re.cost=cost;
}else{
re.cost=cost+fabs(acos(naiseki/lineLen)/M_PI*180.0);
}
re.nowLine=cons[nowLine][nextPoint].lineNo;//次のライン
//-1は次がゴール
if(re.nowLine!=-1){
for(unsigned int i=0;i<cons[re.nowLine].size();i++){
if(nowLine==cons[re.nowLine][i].lineNo){
re.nowPoint=i;//次のポイント
break;
}
}
//次の線上での自分の位置
re.len=cons[re.nowLine][re.nowPoint].len;
}
//printf("<dx=%.3lf dy=%.3lf), ",dx,dy);
re.oldDX=dx;
re.oldDY=dy;
return re;
}
};
class costSorter {
public:
bool operator()(const robo& l, const robo& r) const {
//今いる線とポイントと向きとでチェック
return l.cost>r.cost;
}
};
class roboSorter {
public:
bool operator()(const robo& l, const robo& r) const {
//今いる線とポイントと向きとでチェック
if(l.nowPoint!=r.nowPoint)return l.nowPoint<r.nowPoint;
if(l.nowLine!=r.nowLine)return l.nowLine<r.nowLine;
if(l.oldDX!=r.oldDX)return l.oldDX<r.oldDX;
return l.oldDY<r.oldDY;
}
};
bool calcCross(Line& l1,Line& l2){
P p1,p2,p3,p4;
p1=l1.p1;
p2=l1.p2;
p3=l2.p1;
p4=l2.p2;
if (((p1.x - p2.x) * (p3.y - p1.y) + (p1.y - p2.y) * (p1.x - p3.x)) * ((p1.x - p2.x) * (p4.y - p1.y) + (p1.y - p2.y) * (p1.x - p4.x)) <= 0.0){
if(((p3.x - p4.x) * (p1.y - p3.y) + (p3.y - p4.y) * (p3.x - p1.x)) * ((p3.x - p4.x) * (p2.y - p3.y) + (p3.y - p4.y) * (p3.x - p2.x)) <= 0.0){
return true;
}
}
return false;
}
double lineInP(Line& l1,P& p3){
P p1,p2;
p1=l1.p1;
p2=l1.p2;
return (p2.y-p1.y)*(p3.x-p1.x)+p1.y*(p2.x-p1.x)-p3.y*(p2.x-p1.x);
}
void crossSet(Line& l1,Line& l2,int no1,int no2){
if(lineInP(l1,l2.p1)==0.0&&lineInP(l1,l2.p2)==0.0)return;//同じ直線上で共有部分を持たないことが判明している
if(calcCross(l1,l2)){
//printf("<%d %d>",no1,no2);
P p1,p2,p3,p4;
p1=l1.p1;
p3=l1.p2;
p2=l2.p1;
p4=l2.p2;//ばらばらなサイトを参考に組み上げたのでこのへん統一感がない
cP p;
p.lineNo=no2;
double s1=(p4.x-p2.x)*(p1.y-p2.y)-(p4.y-p2.y)*(p1.x-p2.x);
double s2=(p4.x-p2.x)*(p2.y-p3.y)-(p4.y-p2.y)*(p2.x-p3.x);
if(s1+s2==0.0){
//printf("?");
p.len=0;
}else{
double x=p1.x+(p3.x-p1.x)*s1/(s1+s2);
double y=p1.y+(p3.y-p1.y)*s1/(s1+s2);
//printf("(%lf %lf %lf)\n",x,y,s1/(s1+s2));
p.len=s1/(s1+s2);
}
cons[no1].push_back(p);
}
}
void crossUnion(Line& l1,Line& l2){
if(l1.isUnion||l2.isUnion)return ;//統合済みなので次へ
//lineの中身をp1<p2とソート
if(l1.p2<l1.p1)std::swap(l1.p1,l1.p2);
if(l2.p2<l2.p1)std::swap(l2.p1,l2.p2);
double d1=lineInP(l1,l2.p1);
double d2=lineInP(l1,l2.p2);
bool okUnion=false;
if(d1==0&&d2==0){
//両線分が平行だった
//両線分に共通部分があるかのチェック。
if(l1.p1==l2.p1 || l1.p1==l2.p2 || l1.p2==l2.p1 || l1.p2==l2.p2) okUnion=true;
if(( l1.p1<l2.p1&& l2.p1<l1.p2) || ( l1.p1<l2.p2&& l2.p2<l1.p2))okUnion=true;
}
if(okUnion==true){
//l1に統合する
l1.p1=std::min(l1.p1,l2.p1);
l1.p2=std::max(l1.p2,l2.p2);
l2.isUnion=true;//l2を使用済みに
}
}
void setData(int n){
//まずは平行で重なる線分を統合する作業
//長さ0の線分は無視する
//次にゴールを線分上に持つ線分の一覧を作る
P sp,ep;
Line l1,l2;
l1.isUnion=false;
for(int i=0;i<n;i++){
scanf("%lf %lf %lf %lf",&l1.p1.x,&l1.p1.y,&l1.p2.x,&l1.p2.y);
lines[i]=l1;
}
//まずは平行な線分を全部統合する
for(int t=0;t<n;t++){
for(int i=0;i<n;i++){
for(int j=0;j<n;j++){
if(i==j)continue;
crossUnion(lines[i],lines[j]);
}
}
}
//統合がうまくいったか確認
for(int i=0;i<n;i++){
//接合部分のリスト作り
cons[i].clear();
l1=lines[i];
////printf("(%lf %lf) (%lf %lf) %s\n",l1.p1.x,l1.p1.y,l1.p2.x,l1.p2.y,l1.isUnion?"uni":"no");
}
//線分の交点を求める
for(int i=0;i<n;i++){
if(lines[i].isUnion)continue;
for(int j=0;j<n;j++){
if(i==j || lines[j].isUnion)continue;
//統合済みの線分は無視
crossSet(lines[i],lines[j],i,j);
}
}
scanf("%lf %lf %lf %lf",&sp.x,&sp.y,&ep.x,&ep.y);
for(int i=0;i<n;i++){
if(lineInP(lines[i],ep)==0.0&&lines[i].isUnion==false){
cP p;
P p1,p2;
p1=lines[i].p1;
p2=lines[i].p2;
//長さ0の線分は無いと信じたい
if(p2.x==p1.x){
p.len=(ep.y-p1.y)/(p2.y-p1.y);
}else{
p.len=(ep.x-p1.x)/(p2.x-p1.x);
}
if(0.0<=p.len&&p.len<=1.0){
//printf("<<%d>>",i);
p.lineNo=-1;
cons[i].push_back(p);
}
}
std::sort(cons[i].begin(),cons[i].end());
}
std::map<robo,double,roboSorter> costMemo;//ロボがルート探索中過去に同じ状態でより低いコストで通ってないかのメモ
std::priority_queue<robo,std::vector<robo>,costSorter> roboQ;
robo r,nextR;
r.cost=0;
r.oldDX=r.oldDY=0.0;
r.nowPoint=0;
for(int i=0;i<n;i++){
r.nowLine=i;
if(lineInP(lines[i],sp)==0.0&&lines[i].isUnion==false){
//スタート地点の候補
P p1,p2;
p1=lines[i].p1;
p2=lines[i].p2;
if(p2.x==p1.x){
r.len=(sp.y-p1.y)/(p2.y-p1.y);
}else{
r.len=(sp.x-p1.x)/(p2.x-p1.x);
}
if(0.0<=r.len&&r.len<=1.0){
//線分の間にスタート地点があった
//printf("<<%d>>",i);
roboQ.push(r);
costMemo[r]=0.0;
}
}
}
r.nowLine=100;//適当な数を入れる
while(!roboQ.empty()){
r=roboQ.top();
roboQ.pop();
//printf("\n<?%d %lf %lf %lf>,",r.nowLine,r.cost,r.oldDX,r.oldDY);
if(r.nowLine==-1)break;
for(unsigned int i=0;i<cons[r.nowLine].size();i++){
nextR=r.move(i);
if(costMemo.find(nextR)==costMemo.end()){
costMemo[nextR]=nextR.cost;
roboQ.push(nextR);
}else if(costMemo[nextR]>nextR.cost){
costMemo[nextR]=nextR.cost;
roboQ.push(nextR);
}
}
}
if(r.nowLine==-1)printf("%.8lf\n",r.cost);
else printf("-1\n");
}
int main(){
int n;
while(1){
scanf("%d",&n);
if(n==0)break;
setData(n);
}
}
最終更新：2012年07月09日 20:14
