問題F Charlie the Cockchafer

合同練習会

解法

グラフを作成してdp[現在の頂点][直前の頂点] := 最小時間 となるようにダイクストラ法をします。二点間の距離と角度の計算(内積を使う)ができれば解けるかと思います。スタートとゴールが同じケースや直前に来た道に戻る時に誤差死しないように気をつけましょう。(直前に来た道には戻らないという処理をするのが楽かも)

コード

#include <iostream>
#include <vector>
#include <map>
#include <algorithm>
#include <queue>
#include <cstdio>
#include <cmath>
using namespace std;

const int INF = 1e+8;
const double EPS = 1e-8;
const double PI = acos(-1.0);

struct State{
	int v, prev;
	double cost;
	State(){}
	State(int v_, int prev_, double cost_){
		v = v_; prev = prev_; cost = cost_;
	}
};
bool operator<(const State &a, const State &b){
	return a.cost > b.cost;
}

struct P{
	double x, y, z;
	P(){}
	P(double x_, double y_, double z_){
		x = x_; y = y_; z = z_;
	}
};
P operator-(P p1, P p2) {
	return P(p1.x - p2.x, p1.y - p2.y, p1.z - p2.z);
}
bool operator==(const P &a, const P &b){
	return a.x == b.x && a.y == b.y && a.z == b.z;
}
bool operator<(const P &a, const P &b){
	if( a.x == b.x ){
		if( a.y == b.y ) return a.z < b.z;
		return a.y < b.y;
	}
	return a.x < b.x;
}

double dist(P a, P b){
	return sqrt((a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y) + (a.z - b.z) * (a.z - b.z));
}

double dot(P p1, P p2){
	return p1.x * p2.x + p1.y * p2.y + p1.z * p2.z;
}

double norm(P p){
	return dot(p, p);
}

double abs(P p){
	return sqrt(norm(p));
}

double get_angle(P p1, P p2) {
	double a = dot(p1, p2);
	double b = abs(p1);
	double c = abs(p2);
	double x = a / (b * c);
	if( 1.0 < x && x <= 1.0 + EPS ){
		return acos(1);
	}
	if( -1.0 - EPS <= x && x < -1.0 ){
		return acos(-1);
	}
	return acos(x);
}

double calc_angle(int to, int v, int prev, map<int,P> h) {
	P v1 = h[to] - h[v];
	P v2 = h[v] - h[prev];
	double rad = get_angle(v1, v2);
	double res = rad * 180.0 / PI;
	return res;
}

// dp[v][prev] := 前に来た頂点 prev で頂点 v までにたどりつく最小時間 
double dp[2001][2001];
// G[v] := 頂点 v と隣接する頂点のリストを返す
vector<int> G[2001];

double dijkstra(map<P,int> h, map<int,P> h_inv, int V, int S, int T){
	for(int i = 0 ; i < V ; i++ ){
		for(int j = 0 ; j < V ; j++ ){
			dp[i][j] = INF;
		}
	}
	
	priority_queue<State> q;
	dp[0][0] = 0;
	for(int i = 0 ; i < G[0].size() ; i++ ){
		int to = G[0][i];
		double cost = dist(h_inv[0], h_inv[to]) / S;
		q.push( State(to, 0, cost));
		dp[to][0] = cost;
	}
		
	double ans = -1;
	while( !q.empty() ){
		int v = q.top().v, prev = q.top().prev;
		double cost = q.top().cost;
		q.pop();
		
		// ゴールのとき
		if( v == 1 ){
			ans = cost;
			break;
		}
		
		for(int i = 0 ; i < G[v].size() ; i++ ){
			int to = G[v][i];
			
			// さっき訪れた頂点には行かない
			if( to == prev ) continue;
			
			double next_cost = cost;
			next_cost +=  dist(h_inv[v], h_inv[to]) / S;
			next_cost += calc_angle(to, v, prev, h_inv) / T;
			
			if( next_cost < dp[to][v] ){
				q.push( State(to, v, next_cost));
				dp[to][v] = next_cost;
			}
		}
	}
	return ans;
}

int main(){
	int N, S, T;
	while( cin >> N >> S >> T){
		if( N == 0 ) break;
		for(int i = 0 ; i < 2001 ; i++ ) G[i].clear();
		
		int V = 2, sx, sy, sz, gx, gy, gz;
		// h[p] := 座標 p = (x,y,z) に対応する頂点番号を返す
		map<P, int> h;
		// h_inv[v] := 頂点番号 v に対応する座標 (x,y,z) を返す
		map<int,P> h_inv;
		
		cin >> sx >> sy >> sz >> gx >> gy >> gz;
		P start(sx, sy, sz), goal(gx, gy, gz);
		h[start] = 0;
		h_inv[0] = start;
		h[goal] = 1;
		h_inv[1] = goal;
		for(int i = 0 ; i < N ; i++ ){
			int x1, y1, z1, x2, y2, z2;
			cin >> x1 >> y1 >> z1 >> x2 >> y2 >> z2;
			P p1(x1, y1, z1), p2(x2, y2,z2);
			int u, v;
			if( h.count(p1) ){
				u = h[p1];
			}else{
				u = h[p1] = V;
				h_inv[V] = p1;
				V++;
			}
			if( h.count(p2) ){
				v = h[p2];
			}else{
				v = h[p2] = V;
				h_inv[V] = p2;
				V++;
			}
			G[u].push_back(v);
			G[v].push_back(u);
		}
		
		// スタートとゴールが同じ時の例外処理
		if( start == goal ){
			printf("%.4f\n", 0.0);
			continue;
		}
		
		double ans = dijkstra(h, h_inv, V, S, T);
		printf("%.4f\n",ans);
	}
}