217 lines
9.7 KiB
Java
217 lines
9.7 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);}
|
|
}
|