3D空間における追尾弾について

[にほ]

どうも、いつもお世話になっております。
今回、Minecraftでbukkitプラグインを用いた3D空間を追尾する弾を作りたく、追尾するための関数を作っていたのですが、思ったように動作しなかったため質問させていただきました。具体的には、水平方向及び垂直方向にそれぞれ追尾する弾を作って発射したところ、最初の数発は対象に向かって追尾するのですが、何度か見当違いの方向を追尾(主に、Y座標マイナス方向に急降下・Y座標プラス方向に急上昇)してしまいます。水平方向には正常に追尾しており、垂直方向の追尾のみでエラーが起きています。

コードは以下の通りです。

コード:

public class TraceBullet extends BukkitRunnable {
	// 1フレームに追尾すべき角度
	private static final double HANG = Math.PI / 24;
	
	// 省略

	// p : 追尾する投擲物
	// e : 追尾対象
	public static Projectile Trace(Projectile p, LivingEntity e) {
		Location loc0 = p.getLocation(); // 追尾弾の座標取得
		Location loc1 = e.getLocation(); // 追尾対象の座標取得
		loc1.add(0.0, 1.0, 0.0); // このままでは足元が座標になるので胸元あたりまで座標を上げる
		Vector vec = p.getVelocity(); // ベクトル取得
		double dir1 = Math.atan2(loc1.getZ() - loc0.getZ(), loc1.getX() - loc0.getX()); // 追尾すべき水平向き
		double dir2 = Math.atan2(loc1.getY() - loc0.getY(), loc1.getX() - loc0.getX()); // 追尾すべき垂直向き
		double vec1 = Math.atan2(vec.getZ(), vec.getX()); // 現在の水平向き
		double vec2 = Math.atan2(vec.getY(), vec.getX()); // 現在の垂直向き
		double angle1 = dir1 - vec1; // 水平の角度差
		double angle2 = dir2 - vec2; // 垂直の角度差
		
		// 角度が-PI~PIの間になかったらその間にする
		while (angle1 > Math.PI) {
			angle1 -= Math.PI * 2;
		}
		while (angle1 < -Math.PI) {
			angle1 += Math.PI * 2;
		}
		while (angle2 > Math.PI) {
			angle2 -= Math.PI * 2;
		}
		while (angle2 < -Math.PI) {
			angle2 += Math.PI * 2;
		}
		
		if (-HANG < angle1 && angle1 < HANG) {
			// 追尾する最大角よりも小さかったら目標方向に転換
			vec1 = dir1;
		} else if (angle1 > 0) {
			// 角度差が正なら足す
			vec1 += HANG;
		} else {
			// 角度差が負なら引く
			vec1 -= HANG;
		}
		if (-HANG < angl2 && angle2 < HANG) {
			// 追尾する最大角よりも小さかったら目標方向に転換
			vec2 = dir2;
		} else if (angle2 > 0) {
			// 角度差が正なら足す
			vec2 += HANG;
		} else {
			// 角度差が負なら引く
			vec2 -= HANG;
		}
		
		double len = vec.length(); // 弾の速度を取得
		// ベクトルの各成分を指定
		vec.setZ(Math.sin(vec1) * len);
		vec.setX(Math.cos(vec1) * len);
		vec.setY(Math.sin(vec2) * len);
		// 指定したベクトルを投擲物の速度に設置する
		p.setVelocity(vec);
		
		return p;
	}
}

このコードは、前に2Dシューティングゲームを作ったときに実装した追尾弾をそのまま流用したものです。そのシューティングゲームと水平方向のみの追尾はうまくいったためそれを単に垂直方向にも利用しようとしたのですが、上記のような結果になってしまいました。原因がわかる方はいらっしゃるでしょうか。また、解決方法についてアドバイスを頂けると大変嬉しいです。期限などは特にありません。

OS : Windows10
IDE : Eclipse OXYGEN(日本語化済み)
ライブラリ : craftbukkit-1.11.jar、craftbukkit-1.12.2.jar

[usao]

例えば15行目の式とか，やりたいことと合っていますか？

> double dir2 = Math.atan2(loc1.getY() - loc0.getY(), loc1.getX() - loc0.getX()); // 追尾すべき垂直向き

目標とx座標が一致している場合には（z座標の不一致具合に関わらず）真上か真下を目指す？
（本当はX-Z平面との角度を考えたいのだろうか？）

[にほ]

ご指摘ありがとうございます。

15行目と17行目を以下のように変更したところ、ある程度期待通りの動きを見せてくれました。

コード:

double x = loc1.getX() - loc0.getX();
double z = loc1.getZ() - loc0.getZ();
double dir2 = Math.atan2(loc1.getY() - loc0.getY(), Math.sqrt(x * x + z * z));
double vec2 = Math.atan2(vec.getY(), Math.sqrt(vec.getX() * vec.getX() + vec.getZ() * vec.getZ()));

この状態でまだ十分なテストができているわけではありませんが、今のところ正常な追尾をしており、特に目立ったエラーは無いようです。ご教授ありがとうございました。