444 lines
16 KiB
Java
444 lines
16 KiB
Java
package com.mojang.math;
|
|
|
|
import org.apache.commons.lang3.tuple.Triple;
|
|
import net.minecraft.util.Mth;
|
|
import com.mojang.datafixers.util.Pair;
|
|
|
|
public final class Matrix3f {
|
|
private static final float G;
|
|
private static final float CS;
|
|
private static final float SS;
|
|
private static final float SQ2;
|
|
protected float m00;
|
|
protected float m01;
|
|
protected float m02;
|
|
protected float m10;
|
|
protected float m11;
|
|
protected float m12;
|
|
protected float m20;
|
|
protected float m21;
|
|
protected float m22;
|
|
|
|
public Matrix3f() {
|
|
}
|
|
|
|
public Matrix3f(final Quaternion d) {
|
|
final float float3 = d.i();
|
|
final float float4 = d.j();
|
|
final float float5 = d.k();
|
|
final float float6 = d.r();
|
|
final float float7 = 2.0f * float3 * float3;
|
|
final float float8 = 2.0f * float4 * float4;
|
|
final float float9 = 2.0f * float5 * float5;
|
|
this.m00 = 1.0f - float8 - float9;
|
|
this.m11 = 1.0f - float9 - float7;
|
|
this.m22 = 1.0f - float7 - float8;
|
|
final float float10 = float3 * float4;
|
|
final float float11 = float4 * float5;
|
|
final float float12 = float5 * float3;
|
|
final float float13 = float3 * float6;
|
|
final float float14 = float4 * float6;
|
|
final float float15 = float5 * float6;
|
|
this.m10 = 2.0f * (float10 + float15);
|
|
this.m01 = 2.0f * (float10 - float15);
|
|
this.m20 = 2.0f * (float12 - float14);
|
|
this.m02 = 2.0f * (float12 + float14);
|
|
this.m21 = 2.0f * (float11 + float13);
|
|
this.m12 = 2.0f * (float11 - float13);
|
|
}
|
|
|
|
public static Matrix3f createScaleMatrix(final float float1, final float float2, final float float3) {
|
|
final Matrix3f a4 = new Matrix3f();
|
|
a4.m00 = float1;
|
|
a4.m11 = float2;
|
|
a4.m22 = float3;
|
|
return a4;
|
|
}
|
|
|
|
public Matrix3f(final Matrix4f b) {
|
|
this.m00 = b.m00;
|
|
this.m01 = b.m01;
|
|
this.m02 = b.m02;
|
|
this.m10 = b.m10;
|
|
this.m11 = b.m11;
|
|
this.m12 = b.m12;
|
|
this.m20 = b.m20;
|
|
this.m21 = b.m21;
|
|
this.m22 = b.m22;
|
|
}
|
|
|
|
public Matrix3f(final Matrix3f a) {
|
|
this.m00 = a.m00;
|
|
this.m01 = a.m01;
|
|
this.m02 = a.m02;
|
|
this.m10 = a.m10;
|
|
this.m11 = a.m11;
|
|
this.m12 = a.m12;
|
|
this.m20 = a.m20;
|
|
this.m21 = a.m21;
|
|
this.m22 = a.m22;
|
|
}
|
|
|
|
private static Pair<Float, Float> approxGivensQuat(final float float1, final float float2, final float float3) {
|
|
final float float4 = 2.0f * (float1 - float3);
|
|
final float float5 = float2;
|
|
if (Matrix3f.G * float5 * float5 < float4 * float4) {
|
|
final float float6 = Mth.fastInvSqrt(float5 * float5 + float4 * float4);
|
|
return (Pair<Float, Float>)Pair.of((float6 * float5), (float6 * float4));
|
|
}
|
|
return (Pair<Float, Float>)Pair.of(Matrix3f.SS, Matrix3f.CS);
|
|
}
|
|
|
|
private static Pair<Float, Float> qrGivensQuat(final float float1, final float float2) {
|
|
final float float3 = (float)Math.hypot(float1, float2);
|
|
float float4 = (float3 > 1.0E-6f) ? float2 : 0.0f;
|
|
float float5 = Math.abs(float1) + Math.max(float3, 1.0E-6f);
|
|
if (float1 < 0.0f) {
|
|
final float float6 = float4;
|
|
float4 = float5;
|
|
float5 = float6;
|
|
}
|
|
final float float6 = Mth.fastInvSqrt(float5 * float5 + float4 * float4);
|
|
float5 *= float6;
|
|
float4 *= float6;
|
|
return (Pair<Float, Float>)Pair.of(float4, float5);
|
|
}
|
|
|
|
private static Quaternion stepJacobi(final Matrix3f a) {
|
|
final Matrix3f a2 = new Matrix3f();
|
|
final Quaternion d3 = Quaternion.ONE.copy();
|
|
if (a.m01 * a.m01 + a.m10 * a.m10 > 1.0E-6f) {
|
|
final Pair<Float, Float> pair4 = approxGivensQuat(a.m00, 0.5f * (a.m01 + a.m10), a.m11);
|
|
final Float float5 = (Float)pair4.getFirst();
|
|
final Float float6 = (Float)pair4.getSecond();
|
|
final Quaternion d4 = new Quaternion(0.0f, 0.0f, float5, float6);
|
|
final float float7 = float6 * float6 - float5 * float5;
|
|
final float float8 = -2.0f * float5 * float6;
|
|
final float float9 = float6 * float6 + float5 * float5;
|
|
d3.mul(d4);
|
|
a2.setIdentity();
|
|
a2.m00 = float7;
|
|
a2.m11 = float7;
|
|
a2.m10 = -float8;
|
|
a2.m01 = float8;
|
|
a2.m22 = float9;
|
|
a.mul(a2);
|
|
a2.transpose();
|
|
a2.mul(a);
|
|
a.load(a2);
|
|
}
|
|
if (a.m02 * a.m02 + a.m20 * a.m20 > 1.0E-6f) {
|
|
final Pair<Float, Float> pair4 = approxGivensQuat(a.m00, 0.5f * (a.m02 + a.m20), a.m22);
|
|
final float float10 = -(float)pair4.getFirst();
|
|
final Float float6 = (Float)pair4.getSecond();
|
|
final Quaternion d4 = new Quaternion(0.0f, float10, 0.0f, float6);
|
|
final float float7 = float6 * float6 - float10 * float10;
|
|
final float float8 = -2.0f * float10 * float6;
|
|
final float float9 = float6 * float6 + float10 * float10;
|
|
d3.mul(d4);
|
|
a2.setIdentity();
|
|
a2.m00 = float7;
|
|
a2.m22 = float7;
|
|
a2.m20 = float8;
|
|
a2.m02 = -float8;
|
|
a2.m11 = float9;
|
|
a.mul(a2);
|
|
a2.transpose();
|
|
a2.mul(a);
|
|
a.load(a2);
|
|
}
|
|
if (a.m12 * a.m12 + a.m21 * a.m21 > 1.0E-6f) {
|
|
final Pair<Float, Float> pair4 = approxGivensQuat(a.m11, 0.5f * (a.m12 + a.m21), a.m22);
|
|
final Float float5 = (Float)pair4.getFirst();
|
|
final Float float6 = (Float)pair4.getSecond();
|
|
final Quaternion d4 = new Quaternion(float5, 0.0f, 0.0f, float6);
|
|
final float float7 = float6 * float6 - float5 * float5;
|
|
final float float8 = -2.0f * float5 * float6;
|
|
final float float9 = float6 * float6 + float5 * float5;
|
|
d3.mul(d4);
|
|
a2.setIdentity();
|
|
a2.m11 = float7;
|
|
a2.m22 = float7;
|
|
a2.m21 = -float8;
|
|
a2.m12 = float8;
|
|
a2.m00 = float9;
|
|
a.mul(a2);
|
|
a2.transpose();
|
|
a2.mul(a);
|
|
a.load(a2);
|
|
}
|
|
return d3;
|
|
}
|
|
|
|
public void transpose() {
|
|
float float2 = this.m01;
|
|
this.m01 = this.m10;
|
|
this.m10 = float2;
|
|
float2 = this.m02;
|
|
this.m02 = this.m20;
|
|
this.m20 = float2;
|
|
float2 = this.m12;
|
|
this.m12 = this.m21;
|
|
this.m21 = float2;
|
|
}
|
|
|
|
public Triple<Quaternion, Vector3f, Quaternion> svdDecompose() {
|
|
final Quaternion d2 = Quaternion.ONE.copy();
|
|
final Quaternion d3 = Quaternion.ONE.copy();
|
|
final Matrix3f a4 = this.copy();
|
|
a4.transpose();
|
|
a4.mul(this);
|
|
for (int integer5 = 0; integer5 < 5; ++integer5) {
|
|
d3.mul(stepJacobi(a4));
|
|
}
|
|
d3.normalize();
|
|
final Matrix3f a5 = new Matrix3f(this);
|
|
a5.mul(new Matrix3f(d3));
|
|
float float7 = 1.0f;
|
|
Pair<Float, Float> pair6 = qrGivensQuat(a5.m00, a5.m10);
|
|
final Float float8 = (Float)pair6.getFirst();
|
|
final Float float9 = (Float)pair6.getSecond();
|
|
final float float10 = float9 * float9 - float8 * float8;
|
|
final float float11 = -2.0f * float8 * float9;
|
|
final float float12 = float9 * float9 + float8 * float8;
|
|
final Quaternion d4 = new Quaternion(0.0f, 0.0f, float8, float9);
|
|
d2.mul(d4);
|
|
final Matrix3f a6 = new Matrix3f();
|
|
a6.setIdentity();
|
|
a6.m00 = float10;
|
|
a6.m11 = float10;
|
|
a6.m10 = float11;
|
|
a6.m01 = -float11;
|
|
a6.m22 = float12;
|
|
float7 *= float12;
|
|
a6.mul(a5);
|
|
pair6 = qrGivensQuat(a6.m00, a6.m20);
|
|
final float float13 = -(float)pair6.getFirst();
|
|
final Float float14 = (Float)pair6.getSecond();
|
|
final float float15 = float14 * float14 - float13 * float13;
|
|
final float float16 = -2.0f * float13 * float14;
|
|
final float float17 = float14 * float14 + float13 * float13;
|
|
final Quaternion d5 = new Quaternion(0.0f, float13, 0.0f, float14);
|
|
d2.mul(d5);
|
|
final Matrix3f a7 = new Matrix3f();
|
|
a7.setIdentity();
|
|
a7.m00 = float15;
|
|
a7.m22 = float15;
|
|
a7.m20 = -float16;
|
|
a7.m02 = float16;
|
|
a7.m11 = float17;
|
|
float7 *= float17;
|
|
a7.mul(a6);
|
|
pair6 = qrGivensQuat(a7.m11, a7.m21);
|
|
final Float float18 = (Float)pair6.getFirst();
|
|
final Float float19 = (Float)pair6.getSecond();
|
|
final float float20 = float19 * float19 - float18 * float18;
|
|
final float float21 = -2.0f * float18 * float19;
|
|
final float float22 = float19 * float19 + float18 * float18;
|
|
final Quaternion d6 = new Quaternion(float18, 0.0f, 0.0f, float19);
|
|
d2.mul(d6);
|
|
final Matrix3f a8 = new Matrix3f();
|
|
a8.setIdentity();
|
|
a8.m11 = float20;
|
|
a8.m22 = float20;
|
|
a8.m21 = float21;
|
|
a8.m12 = -float21;
|
|
a8.m00 = float22;
|
|
float7 *= float22;
|
|
a8.mul(a7);
|
|
float7 = 1.0f / float7;
|
|
d2.mul((float)Math.sqrt(float7));
|
|
final Vector3f g29 = new Vector3f(a8.m00 * float7, a8.m11 * float7, a8.m22 * float7);
|
|
return (Triple<Quaternion, Vector3f, Quaternion>)Triple.of(d2, g29, d3);
|
|
}
|
|
|
|
@Override
|
|
public boolean equals(final Object object) {
|
|
if (this == object) {
|
|
return true;
|
|
}
|
|
if (object == null || this.getClass() != object.getClass()) {
|
|
return false;
|
|
}
|
|
final Matrix3f a3 = (Matrix3f)object;
|
|
return Float.compare(a3.m00, this.m00) == 0 && Float.compare(a3.m01, this.m01) == 0 && Float.compare(a3.m02, this.m02) == 0 && Float.compare(a3.m10, this.m10) == 0 && Float.compare(a3.m11, this.m11) == 0 && Float.compare(a3.m12, this.m12) == 0 && Float.compare(a3.m20, this.m20) == 0 && Float.compare(a3.m21, this.m21) == 0 && Float.compare(a3.m22, this.m22) == 0;
|
|
}
|
|
|
|
@Override
|
|
public int hashCode() {
|
|
int integer2 = (this.m00 != 0.0f) ? Float.floatToIntBits(this.m00) : 0;
|
|
integer2 = 31 * integer2 + ((this.m01 != 0.0f) ? Float.floatToIntBits(this.m01) : 0);
|
|
integer2 = 31 * integer2 + ((this.m02 != 0.0f) ? Float.floatToIntBits(this.m02) : 0);
|
|
integer2 = 31 * integer2 + ((this.m10 != 0.0f) ? Float.floatToIntBits(this.m10) : 0);
|
|
integer2 = 31 * integer2 + ((this.m11 != 0.0f) ? Float.floatToIntBits(this.m11) : 0);
|
|
integer2 = 31 * integer2 + ((this.m12 != 0.0f) ? Float.floatToIntBits(this.m12) : 0);
|
|
integer2 = 31 * integer2 + ((this.m20 != 0.0f) ? Float.floatToIntBits(this.m20) : 0);
|
|
integer2 = 31 * integer2 + ((this.m21 != 0.0f) ? Float.floatToIntBits(this.m21) : 0);
|
|
integer2 = 31 * integer2 + ((this.m22 != 0.0f) ? Float.floatToIntBits(this.m22) : 0);
|
|
return integer2;
|
|
}
|
|
|
|
public void load(final Matrix3f a) {
|
|
this.m00 = a.m00;
|
|
this.m01 = a.m01;
|
|
this.m02 = a.m02;
|
|
this.m10 = a.m10;
|
|
this.m11 = a.m11;
|
|
this.m12 = a.m12;
|
|
this.m20 = a.m20;
|
|
this.m21 = a.m21;
|
|
this.m22 = a.m22;
|
|
}
|
|
|
|
@Override
|
|
public String toString() {
|
|
final StringBuilder stringBuilder2 = new StringBuilder();
|
|
stringBuilder2.append("Matrix3f:\n");
|
|
stringBuilder2.append(this.m00);
|
|
stringBuilder2.append(" ");
|
|
stringBuilder2.append(this.m01);
|
|
stringBuilder2.append(" ");
|
|
stringBuilder2.append(this.m02);
|
|
stringBuilder2.append("\n");
|
|
stringBuilder2.append(this.m10);
|
|
stringBuilder2.append(" ");
|
|
stringBuilder2.append(this.m11);
|
|
stringBuilder2.append(" ");
|
|
stringBuilder2.append(this.m12);
|
|
stringBuilder2.append("\n");
|
|
stringBuilder2.append(this.m20);
|
|
stringBuilder2.append(" ");
|
|
stringBuilder2.append(this.m21);
|
|
stringBuilder2.append(" ");
|
|
stringBuilder2.append(this.m22);
|
|
stringBuilder2.append("\n");
|
|
return stringBuilder2.toString();
|
|
}
|
|
|
|
public void setIdentity() {
|
|
this.m00 = 1.0f;
|
|
this.m01 = 0.0f;
|
|
this.m02 = 0.0f;
|
|
this.m10 = 0.0f;
|
|
this.m11 = 1.0f;
|
|
this.m12 = 0.0f;
|
|
this.m20 = 0.0f;
|
|
this.m21 = 0.0f;
|
|
this.m22 = 1.0f;
|
|
}
|
|
|
|
public float adjugateAndDet() {
|
|
final float float2 = this.m11 * this.m22 - this.m12 * this.m21;
|
|
final float float3 = -(this.m10 * this.m22 - this.m12 * this.m20);
|
|
final float float4 = this.m10 * this.m21 - this.m11 * this.m20;
|
|
final float float5 = -(this.m01 * this.m22 - this.m02 * this.m21);
|
|
final float float6 = this.m00 * this.m22 - this.m02 * this.m20;
|
|
final float float7 = -(this.m00 * this.m21 - this.m01 * this.m20);
|
|
final float float8 = this.m01 * this.m12 - this.m02 * this.m11;
|
|
final float float9 = -(this.m00 * this.m12 - this.m02 * this.m10);
|
|
final float float10 = this.m00 * this.m11 - this.m01 * this.m10;
|
|
final float float11 = this.m00 * float2 + this.m01 * float3 + this.m02 * float4;
|
|
this.m00 = float2;
|
|
this.m10 = float3;
|
|
this.m20 = float4;
|
|
this.m01 = float5;
|
|
this.m11 = float6;
|
|
this.m21 = float7;
|
|
this.m02 = float8;
|
|
this.m12 = float9;
|
|
this.m22 = float10;
|
|
return float11;
|
|
}
|
|
|
|
public boolean invert() {
|
|
final float float2 = this.adjugateAndDet();
|
|
if (Math.abs(float2) > 1.0E-6f) {
|
|
this.mul(float2);
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
public void set(final int integer1, final int integer2, final float float3) {
|
|
if (integer1 == 0) {
|
|
if (integer2 == 0) {
|
|
this.m00 = float3;
|
|
}
|
|
else if (integer2 == 1) {
|
|
this.m01 = float3;
|
|
}
|
|
else {
|
|
this.m02 = float3;
|
|
}
|
|
}
|
|
else if (integer1 == 1) {
|
|
if (integer2 == 0) {
|
|
this.m10 = float3;
|
|
}
|
|
else if (integer2 == 1) {
|
|
this.m11 = float3;
|
|
}
|
|
else {
|
|
this.m12 = float3;
|
|
}
|
|
}
|
|
else if (integer2 == 0) {
|
|
this.m20 = float3;
|
|
}
|
|
else if (integer2 == 1) {
|
|
this.m21 = float3;
|
|
}
|
|
else {
|
|
this.m22 = float3;
|
|
}
|
|
}
|
|
|
|
public void mul(final Matrix3f a) {
|
|
final float float3 = this.m00 * a.m00 + this.m01 * a.m10 + this.m02 * a.m20;
|
|
final float float4 = this.m00 * a.m01 + this.m01 * a.m11 + this.m02 * a.m21;
|
|
final float float5 = this.m00 * a.m02 + this.m01 * a.m12 + this.m02 * a.m22;
|
|
final float float6 = this.m10 * a.m00 + this.m11 * a.m10 + this.m12 * a.m20;
|
|
final float float7 = this.m10 * a.m01 + this.m11 * a.m11 + this.m12 * a.m21;
|
|
final float float8 = this.m10 * a.m02 + this.m11 * a.m12 + this.m12 * a.m22;
|
|
final float float9 = this.m20 * a.m00 + this.m21 * a.m10 + this.m22 * a.m20;
|
|
final float float10 = this.m20 * a.m01 + this.m21 * a.m11 + this.m22 * a.m21;
|
|
final float float11 = this.m20 * a.m02 + this.m21 * a.m12 + this.m22 * a.m22;
|
|
this.m00 = float3;
|
|
this.m01 = float4;
|
|
this.m02 = float5;
|
|
this.m10 = float6;
|
|
this.m11 = float7;
|
|
this.m12 = float8;
|
|
this.m20 = float9;
|
|
this.m21 = float10;
|
|
this.m22 = float11;
|
|
}
|
|
|
|
public void mul(final Quaternion d) {
|
|
this.mul(new Matrix3f(d));
|
|
}
|
|
|
|
public void mul(final float float1) {
|
|
this.m00 *= float1;
|
|
this.m01 *= float1;
|
|
this.m02 *= float1;
|
|
this.m10 *= float1;
|
|
this.m11 *= float1;
|
|
this.m12 *= float1;
|
|
this.m20 *= float1;
|
|
this.m21 *= float1;
|
|
this.m22 *= float1;
|
|
}
|
|
|
|
public Matrix3f copy() {
|
|
return new Matrix3f(this);
|
|
}
|
|
|
|
static {
|
|
G = 3.0f + 2.0f * (float)Math.sqrt(2.0);
|
|
CS = (float)Math.cos(0.39269908169872414);
|
|
SS = (float)Math.sin(0.39269908169872414);
|
|
SQ2 = 1.0f / (float)Math.sqrt(2.0);
|
|
}
|
|
}
|