AOJ1033 Kuru-Kuru Robot

AOJ Volume10

問題リンク Kuru-Kuru Robot

  • 解法

線分アレンジメントを施したのちダイクストラして解きました。
線分アレンジメントの説明はスパゲッティさんにソースコードと共にあります。ここのソースをおおいに参考にして線分アレンジメントを書きました。オーバーラップしている線分を整理する処理が必要になりますが、それもスパゲッティさんにあります。
線分アレンジメントを施すと、線分の端点と交点が全て列挙され、番号付けされ、更に隣接リストが手に入っている状態になります。
ダイクストラは(現在の頂点、直前の頂点）という拡張頂点にて行います。(b, a)という頂点から(c, b)という頂点へ移るとき、a->bベクトルとb->cベクトルの角度がロボットの必要なエネルギーになります。
ここで自分は、角度を求めるときにMath.acos()を使っており、大きなミスを犯しました。
テストケースを色々食わせて気づいたのですが、このacosはNaNを返す時があり、NaNをそのままにしてダイクストラをしたので大量のWAを生み出したのです。
NaNは、a -> b -> a という進み方をする場合と a -> b -> c (abcがこの順番に一直線に並ぶ)という進み方をするときに出てくることがあります。前者の場合はMath.PI、後者の場合は0と解釈しないといけません。この問題の場合、前者のようなUターンする動きかたは絶対損する動き方なので、このパターンについて角度計算の処理をスキップすることで対処します。よって、acosの結果がNaNになった場合、角度は0として、無事Acceptしました。

WAの連発で発狂しそうになったのですが、原因が分かってよかったです。
幾何はやはりこわいですね

※おまけ
テストケースを沢山作ったので良ければ使ってください
解は誤差を含んでいるので厳密ではないので参考程度に

50
68 208 259 149
235 299 128 116
158 234 171 117
122 170 265 218
246 179 116 229
214 173 219 311
265 268 164 251
183 307 311 234
246 234 226 127
196 143 93 144
106 129 133 290
102 269 270 311
294 206 355 290
259 247 395 243
327 317 389 266
358 275 329 296
329 284 374 326
347 296 347 312
351 309 343 309
344 307 351 355
204 345 480 344
431 323 353 430
299 328 415 445
413 408 345 509
396 520 304 444
292 475 450 473
435 455 429 534
461 508 393 501
415 497 363 535
400 541 292 524
369 517 391 579
318 519 85 583
108 595 90 246
122 267 55 250
95 248 106 335
99 327 156 319
133 308 162 389
130 373 253 392
234 369 234 441
258 422 177 440
191 418 206 509
241 493 141 510
153 486 163 544
184 528 117 565
126 566 121 418
111 477 165 405
107 433 176 429
163 454 144 405
83 211 16 20
156 408 213 408

16 20 213 408

5
116 556 200 360
174 353 256 528
233 533 318 349
290 349 352 531
326 540 420 349

116 556 420 349

21
69 108 478 105
314 64 490 203
362 230 492 40
510 180 252 191
296 162 297 323
243 296 508 293
493 271 501 402
539 373 384 396
431 437 409 335
388 366 465 357
452 366 451 314
465 323 338 324
360 312 350 466
381 464 223 385
242 373 178 528
202 535 82 366
70 428 169 198
179 226 76 150
89 189 93 117
111 128 37 104
42 135 68 100

69 108 68 100

11
206 429 464 428
397 370 399 503
366 399 440 470
369 467 423 403
410 395 370 515
366 444 446 477
425 503 417 364
441 421 334 384
384 382 372 498
350 485 467 485
430 478 407 478


206 429 407 478

3
91 324 412 324
399 294 405 450
445 451 317 436


91 324 317 436

12
120 379 540 379
533 359 533 469
502 452 628 452
613 424 625 540
663 528 141 529
295 333 400 470
341 475 400 314
407 425 307 311
364 348 302 468
280 453 441 453
387 438 395 556
407 549 95 554

120 379 95 554

10
127 104 449 104
360 77 475 151
402 79 403 168
433 79 342 170
438 92 444 187
427 175 497 175
489 161 492 232
515 225 362 230
376 262 375 154
386 163 129 161


127 104 129 161

6
150 191 454 191
415 191 466 191
412 178 474 213
431 202 442 182
433 183 444 218
493 210 415 210

150 191 415 210

10
118 514 119 319
100 341 369 341
369 341 398 341
398 341 344 341
344 341 308 341
371 483 364 235
309 468 393 286
431 437 321 284
266 308 470 375
465 291 274 387

118 514 274 387

8
54 298 445 298
433 265 435 365
413 343 527 340
506 311 514 424
265 250 399 357
273 369 352 258
319 246 341 449
587 417 272 415

54 298 272 415

5
226 211 303 211
303 211 368 208
368 208 436 217
436 217 516 217
516 217 571 180

226 211 571 180

25
33 59 231 53
166 36 288 136
212 31 219 147
163 118 320 112
286 73 260 177
237 148 309 122
302 84 307 209
286 175 377 170
355 133 356 249
273 191 424 191
409 157 403 263
375 225 479 229
455 186 459 302
437 262 519 263
516 223 508 312
553 287 246 294
385 244 428 277
430 250 381 330
425 330 333 265
363 263 309 329
342 335 286 261
266 334 306 265
297 265 184 273
216 248 345 404
264 359 635 361

33 59 635 361

10
78 88 359 88
270 51 411 155
337 49 372 193
313 145 457 145
433 105 443 215
486 211 253 215
327 199 248 282
298 296 173 195
286 196 168 335
141 307 429 307

78 88 429 307

0 0

//answer
2694.270415013
533.171022451
1675.090500109
180.222075553
186.683864108
179.081876998
180.445872552
180.000000000
243.021261416
180.363777839
54.293778289
728.762549347
430.857086176
  • ソース
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Collections;
import java.util.Comparator;
import java.util.List;
import java.util.PriorityQueue;
import java.util.Scanner;

//Kuru-Kuru Robot
public class AOJ1033 {

	final double EPS = 1e-8;
	
	double dot(double[] a, double[] b){
		return a[0]*b[0]+a[1]*b[1];
	}
	double cross(double[] a, double[] b){
		return a[0]*b[1]-a[1]*b[0];
	}
	double norm(double[] a){
		return Math.sqrt(a[0]*a[0]+a[1]*a[1]);
	}
	double norm(double[] a, double[] b){
		return norm(sub(a, b));
	}
	double[] sub(double[] a, double[] b){
		return new double[]{a[0]-b[0], a[1]-b[1]};
	}
	void swap(double[] a, double[] b){
		double t = a[0];
		a[0] = b[0]; b[0] = t;
		t = a[1];
		a[1] = b[1]; b[1] = t;
	}
	double ex(double[] a, double[] b, double[] c){
		double[] s1 = sub(b, a), s2 = sub(c, a);
		return cross(s1, s2);
	}
	double angleCos(double[] a, double[] b){
		double na = norm(a), nb = norm(b);
		return Math.acos(dot(a, b)/na/nb);
	}
	boolean crossing(double[] a, double[] b, double[] s, double[] t){
		if(Math.abs(cross(sub(b, a), sub(t, s)))<EPS){
			return Math.min(dist(a, b, s), Math.min(dist(a, b, t), Math.min(dist(s, t, a), dist(s, t, b))))<EPS;
		}
		if(ex(a, b, s)*ex(a, b, t)>0)return false;
		if(ex(b, a, s)*ex(b, a, t)>0)return false;
		if(ex(s, t, a)*ex(s, t, b)>0)return false;
		return ex(t, s, a)*ex(t, s, b)<EPS;
	}
	double dist(double[] a, double[] b, double[] p){
		if(dot(sub(b, a), sub(p, a))<EPS)return norm(a, p);
		if(dot(sub(a, b), sub(p, b))<EPS)return norm(b, p);
		return Math.abs(cross(sub(b, a), sub(p, a)))/norm(a, b);
	}
	double dist(double[] a, double[] b, double[] s, double[] t){
		if(crossing(a, b, s, t))return 0;
		return Math.min(dist(a, b, s), Math.min(dist(a, b, t), Math.min(dist(s, t, a), dist(s, t, b))));
	}
	double distLP(double[] a, double[] b, double[] p){
		return Math.abs(cross(sub(b, a), sub(p, a)))/norm(a, b);
	}
	double[] cp(double[] a, double[] b, double[] s, double[] t){
		double ds = distLP(a, b, s), dt = distLP(a, b, t);
		double k = ds/(ds+dt);
		double[] d = sub(t, s);
		return new double[]{s[0]+k*d[0], s[1]+k*d[1]};
	}
	
	int ccw(double[] a, double[] b, double[] c){
		b = sub(b, a); c = sub(c, a);
		if(cross(b, c) > 0)return 1;
		if(cross(b, c) < 0)return -1;
		if(dot(b, c) < 0)return +2;
		if(norm(b) < norm(c))return -2;
		return 0;
	}
	
	@SuppressWarnings("unchecked")
	List<E>[] segmentArrangement(double[][][] seg, List<double[]> resPoint){
		seg = mergeSegment(seg);
		List<double[]> pointList = new ArrayList<double[]>();
		for(int i=0;i<seg.length;i++){
			pointList.add(seg[i][0]); pointList.add(seg[i][1]);
			for(int j=i+1;j<seg.length;j++){
				if(crossing(seg[i][0], seg[i][1], seg[j][0], seg[j][1])){
					pointList.add(cp(seg[i][0], seg[i][1], seg[j][0], seg[j][1]));
				}
			}
		}
		for(int i=0;i<pointList.size();i++){
			boolean col = false;
			for(int j=i+1;j<pointList.size();j++){
				if(norm(pointList.get(i), pointList.get(j)) < EPS){
					col = true; break;
				}
			}
			if(!col)resPoint.add(pointList.get(i));
		}
		int N = resPoint.size();
		List<E>[] resAdj = new List[N];
		for(int i=0;i<N;i++)resAdj[i] = new ArrayList<E>();
		for(int i=0;i<seg.length;i++){
			List<E> tmp = new ArrayList<E>();
			for(int j=0;j<N;j++){
				if(crossing(seg[i][0], seg[i][1], resPoint.get(j), resPoint.get(j))){
					tmp.add(new E(j, norm(seg[i][0], resPoint.get(j))));
				}
			}
			Collections.sort(tmp);
			for(int j=0;j+1<tmp.size();j++){
				E e1 = tmp.get(j), e2 = tmp.get(j+1);
				resAdj[e1.id].add(new E(e2.id, norm(resPoint.get(e1.id), resPoint.get(e2.id))));
				resAdj[e2.id].add(new E(e1.id, norm(resPoint.get(e1.id), resPoint.get(e2.id))));
			}
		}
		return resAdj;
	}
	
	boolean pointCompare(double[] a, double[] b){
		if(Math.abs(a[0]-b[0]) < EPS)return ((int)Math.signum(a[1]-b[1]))<=0;
		return ((int)Math.signum(a[0]-b[0]))<=0;
	}

	boolean enableMerge(double[][] s, double[][] t, double[][] res){
		if(Math.abs(cross(sub(s[1], s[0]), sub(t[1], t[0]))) > EPS)return false;
		if(Math.abs(ccw(s[0], t[0], s[1]))==1)return false;
		if(ccw(s[0], s[1], t[0])==-2 || ccw(t[0], t[1], s[0])==-2)return false;
		double[] S = pointCompare(s[0], t[0])?s[0]:t[0], T = pointCompare(s[1], t[1])?t[1]:s[1];
		res[0] = S;
		res[1] = T;
		return true;
	}
	
	double[][][] mergeSegment(double[][][] seg){
		List<double[][]> segs = new ArrayList<double[][]>();
		for(int i=0;i<seg.length;i++){
			if(!pointCompare(seg[i][0], seg[i][1])){
				swap(seg[i][0], seg[i][1]);
			}
			segs.add(seg[i]);
		}
		double[][] tmp = new double[2][2];
		for(int i=0;i<segs.size();i++){
			for(int j=i+1;j<segs.size();j++){
				if(enableMerge(segs.get(i), segs.get(j), tmp)){
					segs.remove(j--);
					segs.remove(i);
					segs.add(i, new double[][]{{tmp[0][0],tmp[0][1]},{tmp[1][0],tmp[1][1]}});
				}
			}
		}
		int N = segs.size();
		double[][][] res = new double[N][2][2];
		for(int i=0;i<N;i++){
			res[i] = segs.get(i);
		}
		return res;
	}

	class E implements Comparable<E>{
		int id;
		double dist;
		public E(int id, double dist) {
			this.id = id;
			this.dist = dist;
		}
		public int compareTo(E o) {
			return (int)Math.signum(dist-o.dist);
		}
	}
	
	double[][] d;
	
	void run(){
		Scanner sc = new Scanner(System.in);
		int INF = 1<<29;
		for(;;){
			int n = sc.nextInt();
			if(n==0)break;
			double[][][] seg = new double[n][2][2];
			for(int i=0;i<n;i++)for(int j=0;j<2;j++)for(int k=0;k<2;k++)seg[i][j][k]=sc.nextDouble();
			double[] S = {sc.nextDouble(), sc.nextDouble()}, G = {sc.nextDouble(), sc.nextDouble()};
			if(S[0]==G[0] && S[1]==G[1]){
				System.out.printf("%.9f\n", 0.); continue;
			}
			List<double[]> ps = new ArrayList<double[]>();
			List<E>[] adj = segmentArrangement(seg, ps);
			int N = ps.size(), si = -1, gi = -1;
			for(int i=0;i<N;i++){
				if(si==-1 && norm(S, ps.get(i)) == 0)si = i;
				if(gi==-1 && norm(G, ps.get(i)) == 0)gi = i;
			}
			d = new double[N][N];
			
			int[][][] rev = new int[N][N][2];
			for(int i=0;i<N;i++)for(int j=0;j<N;j++)for(int k=0;k<2;k++)rev[i][j][k]=-1;
			
			for(double[] D:d)Arrays.fill(D, INF);
			PriorityQueue<int[]> q = new PriorityQueue<int[]>(N, new Comparator<int[]>() {
				public int compare(int[] o1, int[] o2) {
					return (int)Math.signum(d[o1[0]][o1[1]] - d[o2[0]][o2[1]]);
				}
			});
			for(E e:adj[si]){
				d[e.id][si] = 0;
				rev[e.id][si][0] = rev[e.id][si][1] = -1;
				q.add(new int[]{e.id, si});
			}
			while(!q.isEmpty()){
				int[] V = q.poll();
				int now = V[0], pre = V[1];
				for(E e:adj[now]){
					if(e.id==pre)continue;
					double thita = angleCos(sub(ps.get(now), ps.get(pre)), sub(ps.get(e.id), ps.get(now)));
					if(Double.isNaN(thita)){
						thita = 0;
					}
					double w = d[now][pre] + thita;
					if(w < d[e.id][now]){
						d[e.id][now] = w;
						rev[e.id][now][0] = now; rev[e.id][now][1] = pre;
						q.add(new int[]{e.id, now});
					}
				}
			}
			double res = INF;
			for(E e:adj[gi])res = Math.min(res, d[gi][e.id]);
			if(res==INF)System.out.println(-1);
			else System.out.printf("%.9f\n", res*180/Math.PI);
		}
	}
	
	public static void main(String[] args) {
		new AOJ1033().run();
	}
}