111 lines
3.2 KiB
Java
111 lines
3.2 KiB
Java
package me.tofaa.entitylib.bb;
|
|
|
|
import com.github.retrooper.packetevents.util.Vector3d;
|
|
|
|
public final class Quaternion {
|
|
|
|
private final double x;
|
|
private final double y;
|
|
private final double z;
|
|
|
|
private final double w;
|
|
|
|
public Quaternion(double x, double y, double z, double w) {
|
|
this.x = x;
|
|
this.y = y;
|
|
this.z = z;
|
|
this.w = w;
|
|
}
|
|
|
|
public Quaternion(Vector3d p) {
|
|
p = ModelMath.toRadians(p);
|
|
|
|
double cy = Math.cos(p.getZ() * 0.5);
|
|
double sy = Math.sin(p.getZ() * 0.5);
|
|
double cp = Math.cos(p.getY() * 0.5);
|
|
double sp = Math.sin(p.getY() * 0.5);
|
|
double cr = Math.cos(p.getX() * 0.5);
|
|
double sr = Math.sin(p.getX() * 0.5);
|
|
|
|
w = cr * cp * cy + sr * sp * sy;
|
|
x = sr * cp * cy - cr * sp * sy;
|
|
y = cr * sp * cy + sr * cp * sy;
|
|
z = cr * cp * sy - sr * sp * cy;
|
|
}
|
|
|
|
public double x() {
|
|
return x;
|
|
}
|
|
|
|
public double y() {
|
|
return y;
|
|
}
|
|
|
|
public double z() {
|
|
return z;
|
|
}
|
|
|
|
public double w() {
|
|
return w;
|
|
}
|
|
|
|
public Vector3d toEuler() {
|
|
double t0 = (x + z) * (x - z); // x^2-z^2
|
|
double t1 = (w + y) * (w - y); // w^2-y^2
|
|
double xx = 0.5 * (t0 + t1); // 1/2 x of x'
|
|
double xy = x * y + w * z; // 1/2 y of x'
|
|
double xz = w * y - x * z; // 1/2 z of x'
|
|
double t = xx * xx + xy * xy; // cos(theta)^2
|
|
double yz = 2.0 * (y * z + w * x); // z of y'
|
|
|
|
double vx, vy, vz;
|
|
|
|
vz = (float) Math.atan2(xy, xx); // yaw (psi)
|
|
vy = (float) Math.atan(xz / Math.sqrt(t)); // pitch (theta)
|
|
|
|
if (t != 0)
|
|
vx = (float) Math.atan2(yz, t1 - t0);
|
|
else
|
|
vx = (float) (2.0 * Math.atan2(x, w) - Math.signum(xz) * vz);
|
|
|
|
return ModelMath.toDegrees(new Vector3d(vx, vy, vz));
|
|
}
|
|
|
|
public Quaternion multiply(Quaternion q) {
|
|
double w = this.w * q.w - this.x * q.x - this.y * q.y - this.z * q.z;
|
|
double x = this.w * q.x + this.x * q.w + this.y * q.z - this.z * q.y;
|
|
double y = this.w * q.y - this.x * q.z + this.y * q.w + this.z * q.x;
|
|
double z = this.w * q.z + this.x * q.y - this.y * q.x + this.z * q.w;
|
|
|
|
return new Quaternion(
|
|
x, y, z, w
|
|
);
|
|
}
|
|
|
|
@Override
|
|
public String toString() {
|
|
return "me.tofaa.entitylib.bb.Quaternion{" +
|
|
"x=" + x +
|
|
", y=" + y +
|
|
", z=" + z +
|
|
", w=" + w +
|
|
'}';
|
|
}
|
|
|
|
Vector3d threeAxisRot(double r11, double r12, double r21, double r31, double r32) {
|
|
double x = Math.atan2(r31, r32);
|
|
double y = Math.asin(r21);
|
|
double z = Math.atan2(r11, r12);
|
|
return new Vector3d(x, z, y);
|
|
}
|
|
|
|
public Vector3d toEulerYZX() {
|
|
Quaternion q = this;
|
|
return ModelMath.toDegrees(threeAxisRot(-2 * (q.x * q.z - q.w * q.y),
|
|
q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
|
|
2 * (q.x * q.y + q.w * q.z),
|
|
-2 * (q.y * q.z - q.w * q.x),
|
|
q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z));
|
|
}
|
|
|
|
}
|