Clean up angle calculation

This commit is contained in:
Brady 2018-08-07 19:36:51 -05:00
parent adb6904d53
commit b5ddd17131
No known key found for this signature in database
GPG Key ID: 73A788379A197567

View File

@ -25,11 +25,14 @@ public final class Utils {
* @return Rotation {@link Rotation} * @return Rotation {@link Rotation}
*/ */
public static Rotation calcRotationFromVec3d(Vec3d orig, Vec3d dest) { public static Rotation calcRotationFromVec3d(Vec3d orig, Vec3d dest) {
double yaw = Math.atan2(orig.x - dest.x, -orig.z + dest.z); double[] delta = { orig.x - dest.x, orig.y - dest.y, orig.z - dest.z };
double dist = Math.sqrt((orig.x - dest.x) * (orig.x - dest.x) + (-orig.z + dest.z) * (-orig.z + dest.z)); double yaw = Math.atan2(delta[0], -delta[2]);
double pitch = Math.atan2(orig.y - dest.y, dist); double dist = Math.sqrt(delta[0] * delta[0] + delta[2] * delta[2]);
return new Rotation((float) (yaw * 180 / Math.PI), double pitch = Math.atan2(delta[1], dist);
(float) (pitch * 180 / Math.PI)); return new Rotation(
(float) radToDeg(yaw),
(float) radToDeg(pitch)
);
} }
/** /**
@ -71,4 +74,12 @@ public final class Utils {
double zdiff = z - (pos.getZ() + 0.5D); double zdiff = z - (pos.getZ() + 0.5D);
return Math.sqrt(xdiff * xdiff + ydiff * ydiff + zdiff * zdiff); return Math.sqrt(xdiff * xdiff + ydiff * ydiff + zdiff * zdiff);
} }
public static double degToRad(double deg) {
return deg * Math.PI / 180.0;
}
public static double radToDeg(double rad) {
return rad * 180.0 / Math.PI;
}
} }