192 lines
9.8 KiB
Java
192 lines
9.8 KiB
Java
package speiger.src.coreengine.math.vector.quaternion;
|
|
|
|
import java.nio.ByteBuffer;
|
|
import java.nio.FloatBuffer;
|
|
|
|
import speiger.src.coreengine.math.MathUtils;
|
|
import speiger.src.coreengine.math.vector.floats.Vec4f;
|
|
import speiger.src.coreengine.math.vector.matrix.Matrix4f;
|
|
|
|
public interface Quaternion {
|
|
public static final Quaternion ZERO = of(0F, 0F, 0F, 0F);
|
|
public static final Quaternion IDENTITY = of();
|
|
|
|
public static Quaternion mutable() { return new QuaternionMutable(); }
|
|
public static Quaternion mutable(Quaternion source) { return mutable(source.getX(), source.getY(), source.getZ(), source.getW()); }
|
|
public static Quaternion mutable(float x, float y, float z, float w) { return new QuaternionMutable(x, y, z, w); }
|
|
public static Quaternion mutableRad(float x, float y, float z, float angle) { return mutable().setAxisRad(x, y, z, angle); }
|
|
public static Quaternion mutableDeg(float x, float y, float z, float angle) { return mutable().setAxisDeg(x, y, z, angle); }
|
|
public static Quaternion of() { return new QuaternionImmutable(); }
|
|
public static Quaternion of(Quaternion source) { return of(source.getX(), source.getY(), source.getZ(), source.getW()); }
|
|
public static Quaternion of(float x, float y, float z, float w) { return new QuaternionImmutable(x, y, z, w); }
|
|
public static Quaternion ofRad(float x, float y, float z, float angle) { return ZERO.setAxisRad(x, y, z, angle); }
|
|
public static Quaternion ofDeg(float x, float y, float z, float angle) { return ZERO.setAxisDeg(x, y, z, angle); }
|
|
public static double acos(double v) { return v < -1D ? 3.141592653589793D : (v > 1D ? 0D : Math.acos(v)); }
|
|
|
|
public Quaternion setX(float x);
|
|
public Quaternion setY(float y);
|
|
public Quaternion setZ(float z);
|
|
public Quaternion setW(float w);
|
|
public float getX();
|
|
public float getY();
|
|
public float getZ();
|
|
public float getW();
|
|
|
|
public default float[] asArray() { return new float[] {getX(), getY(), getZ(), getW()}; }
|
|
public default Quaternion negate() { return set(0F, 0F, 0F, 0F); }
|
|
public default Quaternion invert() { return set(-getX(), -getY(), -getZ(), -getW()); }
|
|
public default Quaternion normalize() { return scale(1.0F / length()); }
|
|
public default Quaternion conjugate() { return set(-getX(), -getY(), -getZ(), getW()); }
|
|
public default Quaternion setIdentity() { return set(0F, 0F, 0F, 1F); }
|
|
public default Quaternion multiply(Quaternion other) { return set(getX() * other.getW() + getW() * other.getX() + getY() * other.getZ() - getZ() * other.getY(), getY() * other.getW() + getW() * other.getY() + getZ() * other.getX() - getX() * other.getZ(), getZ() * other.getW() + getW() * other.getZ() + getX() * other.getY() - getY() * other.getX(), getW() * other.getW() - getX() * other.getX() - getY() * other.getY() - getZ() * other.getZ()); }
|
|
public default Quaternion scale(float scale) { return set(getX() * scale, getY() * scale, getZ() * scale, getW() * scale); }
|
|
public default Quaternion rotateX(float angle) {
|
|
angle = (float)Math.toRadians(angle);
|
|
float sin = MathUtils.sin(angle * 0.5D);
|
|
float cos = MathUtils.cos(angle * 0.5D);
|
|
return set(getW() * sin + getX() * cos, getY() * cos + getZ() * sin, getZ() * cos - getY() * sin, getW() * cos - getX() * sin);
|
|
}
|
|
|
|
public default Quaternion rotateY(float angle) {
|
|
angle = (float)Math.toRadians(angle);
|
|
float sin = MathUtils.sin(angle * 0.5D);
|
|
float cos = MathUtils.cos(angle * 0.5D);
|
|
return set(getX() * cos - getZ() * sin, getW() * sin + getY() * cos, getX() * sin + getZ() * cos, getW() * cos - getY() * sin);
|
|
}
|
|
|
|
public default Quaternion rotateZ(float angle) {
|
|
angle = (float)Math.toRadians(angle);
|
|
float sin = MathUtils.sin(angle * 0.5D);
|
|
float cos = MathUtils.cos(angle * 0.5D);
|
|
return set(getX() * cos + getY() * sin, getY() * cos - getX() * sin, getW() * sin + getZ() * cos, getW() * cos - getZ() * sin);
|
|
}
|
|
|
|
public default Quaternion set(Vec4f value) { return set(value.getX(), value.getY(), value.getZ(), value.getW()); }
|
|
public default Quaternion set(Quaternion value) { return set(value.getX(), value.getY(), value.getZ(), value.getW()); }
|
|
|
|
public default Quaternion set(Matrix4f matrix) {
|
|
float tr = matrix.get(0, 0) + matrix.get(1, 1) + matrix.get(2, 2);
|
|
if(tr >= 0.0D) {
|
|
float s = (float)Math.sqrt(tr + 1.0D);
|
|
float w = s * 0.5F;
|
|
s = 0.5F / s;
|
|
float x = (matrix.get(2, 1) - matrix.get(1, 2)) * s;
|
|
float y = (matrix.get(0, 2) - matrix.get(2, 0)) * s;
|
|
float z = (matrix.get(1, 0) - matrix.get(0, 1)) * s;
|
|
return set(x, y, z, w);
|
|
}
|
|
else {
|
|
float max = Math.max(Math.max(matrix.get(0, 0), matrix.get(1, 1)), matrix.get(2, 2));
|
|
if(max == matrix.get(0, 0)) {
|
|
float s = (float)Math.sqrt(matrix.get(0, 0) - (matrix.get(1, 1) + matrix.get(2, 2)) + 1.0D);
|
|
float x = s * 0.5F;
|
|
s = 0.5F / s;
|
|
float y = (matrix.get(0, 1) + matrix.get(1, 0)) * s;
|
|
float z = (matrix.get(2, 0) + matrix.get(0, 2)) * s;
|
|
float w = (matrix.get(2, 1) - matrix.get(1, 2)) * s;
|
|
return set(x, y, z, w);
|
|
}
|
|
else if(max == matrix.get(1, 1)) {
|
|
float s = (float)Math.sqrt(matrix.get(1, 1) - (matrix.get(2, 2) + matrix.get(0, 0)) + 1.0D);
|
|
float y = s * 0.5F;
|
|
s = 0.5F / s;
|
|
float z = (matrix.get(1, 2) + matrix.get(2, 1)) * s;
|
|
float x = (matrix.get(0, 1) + matrix.get(1, 0)) * s;
|
|
float w = (matrix.get(0, 2) - matrix.get(2, 0)) * s;
|
|
return set(x, y, z, w);
|
|
}
|
|
else {
|
|
float s = (float)Math.sqrt(matrix.get(2, 2) - (matrix.get(0, 0) + matrix.get(1, 1)) + 1.0D);
|
|
float z = s * 0.5F;
|
|
s = 0.5F / s;
|
|
float x = (matrix.get(2, 0) + matrix.get(0, 2)) * s;
|
|
float y = (matrix.get(1, 2) + matrix.get(2, 1)) * s;
|
|
float w = (matrix.get(1, 0) - matrix.get(0, 1)) * s;
|
|
return set(x, y, z, w);
|
|
}
|
|
}
|
|
}
|
|
|
|
public default Quaternion setAxisRad(float x, float y, float z, float angle) {
|
|
float sin = MathUtils.sin(angle * 0.5D);
|
|
float cos = MathUtils.cos(angle * 0.5D);
|
|
return set(x * sin, y * sin, z * sin, cos);
|
|
}
|
|
|
|
public default Quaternion setAxisDeg(float x, float y, float z, float angle) {
|
|
angle = (float)Math.toRadians(angle);
|
|
float sin = MathUtils.sin(angle * 0.5D);
|
|
float cos = MathUtils.cos(angle * 0.5D);
|
|
return set(x * sin, y * sin, z * sin, cos);
|
|
}
|
|
|
|
public Quaternion set(float x, float y, float z, float w);
|
|
public default float dot(Quaternion other) { return getX() * other.getX() + getY() * other.getY() + getZ() * other.getZ() + getW() * other.getW(); }
|
|
public default Quaternion difference(Quaternion other) { return difference(other, this); }
|
|
public default Quaternion difference(Quaternion other, Quaternion result) {
|
|
float invNorm = 1.0F / (getX() * getX() + getY() * getY() + getZ() * getZ() + getW() * getW());
|
|
float x = -getX() * invNorm;
|
|
float y = -getY() * invNorm;
|
|
float z = -getZ() * invNorm;
|
|
float w = getW() * invNorm;
|
|
return set(w * other.getX() + x * other.getW() + y * other.getZ() - z * other.getY(), w * other.getY() - x * other.getZ() + y * other.getW() + z * other.getX(), w * other.getZ() + x * other.getY() - y * other.getX() + z * other.getW(), w * other.getW() - x * other.getX() - y * other.getY() - z * other.getZ());
|
|
}
|
|
|
|
public default Quaternion lerp(Quaternion other, float progress) { return lerp(other, progress, this); }
|
|
public default Quaternion lerp(Quaternion other, float progress, Quaternion result) {
|
|
float cosom = getX() * other.getX() + getY() * other.getY() + getZ() * other.getZ() + getW() * other.getW();
|
|
float absCosom = Math.abs(cosom);
|
|
float scale1;
|
|
float scale0;
|
|
if(1.0F - absCosom > 1.0E-006F) {
|
|
float sinSqr = 1.0F - absCosom * absCosom;
|
|
float sinom = (float)(1.0D / Math.sqrt(sinSqr));
|
|
float omega = (float)Math.atan2(sinSqr * sinom, absCosom);
|
|
scale0 = MathUtils.sin((1.0D - progress) * omega) * sinom;
|
|
scale1 = MathUtils.sin(progress * omega) * sinom;
|
|
}
|
|
else {
|
|
scale0 = 1.0F - progress;
|
|
scale1 = progress;
|
|
}
|
|
scale1 = cosom >= 0.0F ? scale1 : -scale1;
|
|
return result.set(scale0 * getX() + scale1 * other.getX(), scale0 * getY() + scale1 * other.getY(), scale0 * getZ() + scale1 * other.getZ(), scale0 * getW() + scale1 * other.getW());
|
|
}
|
|
|
|
public default float length() { return (float)Math.sqrt(lengthSquared()); }
|
|
public default double lengthSquared() { return (getX() * getX()) + (getY() * getY()) + (getZ() * getZ()) + (getW() * getW()); }
|
|
public default Matrix4f asRotationMatrix() { return new Matrix4f().rotate(this); }
|
|
public default Vec4f toAxisDegreeRotation() { return toAxisDegreeRotation(Vec4f.mutable()); }
|
|
public default Vec4f toAxisDegreeRotation(Vec4f input) {
|
|
double invSqrt = 1.0D / Math.sqrt(1.0D - getW() * getW());
|
|
return input.set((float)(getX() * invSqrt), (float)(getY() * invSqrt), (float)(getZ() * invSqrt), (float)Math.toDegrees(acos(getW()) * 2F));
|
|
}
|
|
|
|
public default Vec4f toAxisRotation() { return toAxisRotation(Vec4f.mutable()); }
|
|
public default Vec4f toAxisRotation(Vec4f input) {
|
|
double invSqrt = 1.0D / Math.sqrt(1.0D - getW() * getW());
|
|
return input.set((float)(getX() * invSqrt), (float)(getY() * invSqrt), (float)(getZ() * invSqrt), (float)acos(getW()) * 2F);
|
|
}
|
|
|
|
public default Quaternion store(ByteBuffer buffer) {
|
|
buffer.putFloat(getX()).putFloat(getY()).putFloat(getZ()).putFloat(getW());
|
|
return this;
|
|
}
|
|
|
|
public default Quaternion load(ByteBuffer buffer) { return set(buffer.getFloat(), buffer.getFloat(), buffer.getFloat(), buffer.getFloat()); }
|
|
|
|
public default Quaternion store(FloatBuffer buffer) {
|
|
buffer.put(getX()).put(getY()).put(getZ()).put(getW());
|
|
return this;
|
|
}
|
|
|
|
public default Quaternion load(FloatBuffer buffer) { return set(buffer.get(), buffer.get(), buffer.get(), buffer.get()); }
|
|
|
|
public Quaternion copy();
|
|
public boolean isMutable();
|
|
public default Quaternion asMutable() { return isMutable() ? this : of(this); }
|
|
public default Quaternion asImmutable() { return isMutable() ? mutable(this) : this; }
|
|
public default Quaternion copyAsMutable() { return mutable(this); }
|
|
public default Quaternion copyAsImmutable() { return of(this); }
|
|
}
|