diff --git a/src/main/kotlin/ru/dbotthepony/mc/otm/core/EuclidMath.kt b/src/main/kotlin/ru/dbotthepony/mc/otm/core/EuclidMath.kt index 8b08843ea..fd4feaff2 100644 --- a/src/main/kotlin/ru/dbotthepony/mc/otm/core/EuclidMath.kt +++ b/src/main/kotlin/ru/dbotthepony/mc/otm/core/EuclidMath.kt @@ -1,15 +1,16 @@ package ru.dbotthepony.mc.otm.core -import com.mojang.math.Matrix3f -import com.mojang.math.Matrix4f -import com.mojang.math.Quaternion -import com.mojang.math.Vector3f -import it.unimi.dsi.fastutil.longs.Long2ObjectAVLTreeMap import it.unimi.dsi.fastutil.longs.Long2ObjectOpenHashMap import net.minecraft.core.BlockPos import net.minecraft.core.Direction import net.minecraft.core.Vec3i import net.minecraft.world.phys.Vec3 +import org.joml.AxisAngle4f +import org.joml.Matrix3f +import org.joml.Matrix4f +import org.joml.Quaternionf +import org.joml.Vector3f +import java.lang.Math.toDegrees import java.lang.ref.SoftReference import kotlin.math.* @@ -22,6 +23,12 @@ val VECTOR_DOWN = Vector(0.0, -1.0, 0.0) val VECTOR_RIGHT = Vector(0.0, 0.0, 1.0) val VECTOR_LEFT = Vector(0.0, 0.0, -1.0) +private const val DEGREES_TO_RADIANS = 0.017453292f + +fun toRadians(angle: Float): Float { + return angle * DEGREES_TO_RADIANS +} + fun Vector.asAngle(): Angle { val norm = normalize() return Angle(asin(norm.y), atan2(norm.x, norm.z)) @@ -33,39 +40,33 @@ fun Vector.asMutableAngle(): MutableAngle { } fun Vector3f.asAngle(): Angle { - val len = length() + val len = length().toDouble() val norm = Vector(x() / len, y() / len, z() / len) return Angle(asin(norm.y), atan2(norm.x, norm.z)) } fun Vector3f.asMutableAngle(): MutableAngle { - val len = length() + val len = length().toDouble() val norm = Vector(x() / len, y() / len, z() / len) return MutableAngle(asin(norm.y), atan2(norm.x, norm.z)) } +fun Vector3f.rotation(radians: Float): Quaternionf { + return Quaternionf(AxisAngle4f(radians, x, y, z)) +} + +fun Vector3f.rotationDegrees(degrees: Float): Quaternionf { + return rotation(toRadians(degrees)) +} + operator fun Vector3f.unaryMinus(): Vector3f { - setX(-x()) - setY(-y()) - setZ(-z()) + x = -x + y = -y + z = -z return this } -fun Vector3f.length() = sqrt(x().toDouble() * x() + y().toDouble() * y() + z().toDouble() * z()) - -operator fun Vector3f.times(v: Float): Vector3f { - setX(x() * v) - setY(y() * v) - setZ(z() * v) - return this -} - -operator fun Vector3f.div(v: Float): Vector3f { - setX(x() / v) - setY(y() / v) - setZ(z() / v) - return this -} +operator fun Vector3f.times(v: Float): Vector3f = mul(v) operator fun Vector3f.component1() = x() operator fun Vector3f.component2() = y() @@ -140,8 +141,14 @@ fun Vector.asVector3f(): Vector3f { return Vector3f(x.toFloat(), y.toFloat(), z.toFloat()) } -fun Quaternion(vec: Vector, rotation: Float, isDegrees: Boolean = false): Quaternion { - return Quaternion(vec.asVector3f(), rotation, isDegrees) +fun Quaternion(vec: Vector, rotation: Float, isDegrees: Boolean = false): Quaternionf { + // TODO: 1.19.3 + return Quaternionf(AxisAngle4f(if (isDegrees) toRadians(rotation) else rotation, vec.x.toFloat(), vec.y.toFloat(), vec.z.toFloat())) +} + +fun AxisAngle4f(vec: Vector, rotation: Float, isDegrees: Boolean = false): AxisAngle4f { + // TODO: 1.19.3 + return AxisAngle4f(if (isDegrees) toRadians(rotation) else rotation, vec.x.toFloat(), vec.y.toFloat(), vec.z.toFloat()) } fun Vector(x: Double): Vector { @@ -152,17 +159,24 @@ fun Vector(x: Double, y: Double): Vector { return Vector(x, y, 0.0) } -fun Vector.rotate(q: Quaternion): Vector { - val quaternion = Quaternion(q) - quaternion.mul(Quaternion(x.toFloat(), y.toFloat(), z.toFloat(), 0.0f)) - val quaternion1 = Quaternion(q) - quaternion1.conj() +fun Vector.rotate(q: Quaternionf): Vector { + // TODO: 1.19.3 + val quaternion = Quaternionf(q) + quaternion.mul(rotateAroundThis()) + val quaternion1 = Quaternionf(q) + quaternion1.conjugate() quaternion.mul(quaternion1) - return Vector(quaternion.i().toDouble(), quaternion.j().toDouble(), quaternion.k().toDouble()) + return Vector(quaternion.x.toDouble(), quaternion.y.toDouble(), quaternion.z.toDouble()) } -fun Vector.rotateAroundThis(rotation: Float, isDegrees: Boolean = false): Quaternion { - return Quaternion(this, rotation, isDegrees) +fun Vector.rotateAroundThis(rotation: Float, isDegrees: Boolean = false): Quaternionf { + // TODO: 1.19.3 + return Quaternionf(AxisAngle4f(if (isDegrees) toRadians(rotation) else rotation, x.toFloat(), y.toFloat(), z.toFloat())) +} + +fun Vector.rotateAroundThis(): Quaternionf { + // TODO: 1.19.3 + return Quaternionf(AxisAngle4f(0f, x.toFloat(), y.toFloat(), z.toFloat())) } fun Vector.asMatrix(): Matrix3f { @@ -197,7 +211,7 @@ interface IAngle { fun rotationX(): Matrix3f { if (roll == 0.0) { - return Matrix3f().also { it.identityFast() } + return Matrix3f().also { it.identity() } } return Matrix3f().also { @@ -214,7 +228,7 @@ interface IAngle { fun rotationZ(): Matrix3f { if (pitch == 0.0) { - return Matrix3f().also { it.identityFast() } + return Matrix3f().also { it.identity() } } return Matrix3f().also { @@ -231,7 +245,7 @@ interface IAngle { fun rotationY(): Matrix3f { if (yaw == 0.0) { - return Matrix3f().also { it.identityFast() } + return Matrix3f().also { it.identity() } } return Matrix3f().also { @@ -250,7 +264,7 @@ interface IAngle { fun rotationXYZ(): Matrix3f { if (pitch == 0.0 && yaw == 0.0 && roll == 0.0) - return Matrix3f().also { it.identityFast() } + return Matrix3f().also { it.identity() } return rotationY().also { it.mul(rotationZ()) @@ -260,7 +274,7 @@ interface IAngle { fun rotationXYZW(): Matrix4f { if (pitch == 0.0 && yaw == 0.0 && roll == 0.0) - return Matrix4f().also { it.identityFast() } + return Matrix4f().also { it.identity() } return rotationXYZ().toMatrix4f().also { it[3, 3] = 1f } } diff --git a/src/main/kotlin/ru/dbotthepony/mc/otm/core/MatrixExt.kt b/src/main/kotlin/ru/dbotthepony/mc/otm/core/MatrixExt.kt index 3e8b1d6d3..770b60ec1 100644 --- a/src/main/kotlin/ru/dbotthepony/mc/otm/core/MatrixExt.kt +++ b/src/main/kotlin/ru/dbotthepony/mc/otm/core/MatrixExt.kt @@ -3,13 +3,13 @@ package ru.dbotthepony.mc.otm.core -import com.mojang.math.Matrix3f -import com.mojang.math.Matrix4f -import com.mojang.math.Vector3f -import com.mojang.math.Vector4f +import org.joml.Matrix3f +import org.joml.Matrix4f +import org.joml.Vector3f +import org.joml.Vector4f fun Matrix4f.rotate(angle: IAngle): Matrix4f { - multiply(angle.rotationXYZW()) + mul(angle.rotationXYZW()) return this } @@ -20,164 +20,63 @@ fun Matrix4f.translate(vector: Vector) { fun Matrix4f.rotateAroundPoint(point: Vector, axis: Vector, rotation: Float, isDegrees: Boolean = false) { val p = point.asVector3f() translate(-p) - multiply(axis.rotateAroundThis(rotation, isDegrees)) + rotation(axis.rotateAroundThis(rotation, isDegrees)) // TODO: 1.19.3 translate(-p) } fun Matrix4f.rotateAroundPoint(point: Vector, axis: Vector3f, rotation: Float, isDegrees: Boolean = false) { val p = point.asVector3f() translate(-p) - multiply(if (isDegrees) axis.rotationDegrees(rotation) else axis.rotation(rotation)) + rotation(if (isDegrees) axis.rotationDegrees(rotation) else axis.rotation(rotation)) // TODO: 1.19.3 translate(-p) } fun Matrix4f.rotateAroundPoint(point: Vector, rotation: IAngle) { val p = point.asVector3f() translate(-p) - multiply(rotation.rotationXYZW()) + mul(rotation.rotationXYZW()) translate(-p) } fun Matrix4f.rotateAroundPoint(point: Vector3f, axis: Vector, rotation: Float, isDegrees: Boolean = false) { translate(-point) - multiply(axis.rotateAroundThis(rotation, isDegrees)) + rotation(axis.rotateAroundThis(rotation, isDegrees)) // TODO: 1.19.3 translate(-point) } fun Matrix4f.rotateAroundPoint(point: Vector3f, rotation: IAngle) { translate(-point) - multiply(rotation.rotationXYZW()) + mul(rotation.rotationXYZW()) translate(-point) } fun Matrix4f.rotateAroundPoint(point: Vector3f, axis: Vector3f, rotation: Float, isDegrees: Boolean = false) { translate(-point) - multiply(if (isDegrees) axis.rotationDegrees(rotation) else axis.rotation(rotation)) + rotation(if (isDegrees) axis.rotationDegrees(rotation) else axis.rotation(rotation)) // TODO: 1.19.3 translate(-point) } -operator fun Matrix4f.get(row: Int, column: Int): Float { - require(row in 0 .. 3) { "Invalid row: $row" } - require(column in 0 .. 3) { "Invalid column: $column" } - - return when (val index = row or (column shl 2)) { - 0 or (0 shl 2) -> m00 - 1 or (0 shl 2) -> m10 - 2 or (0 shl 2) -> m20 - 3 or (0 shl 2) -> m30 - 0 or (1 shl 2) -> m01 - 1 or (1 shl 2) -> m11 - 2 or (1 shl 2) -> m21 - 3 or (1 shl 2) -> m31 - 0 or (2 shl 2) -> m02 - 1 or (2 shl 2) -> m12 - 2 or (2 shl 2) -> m22 - 3 or (2 shl 2) -> m32 - 0 or (3 shl 2) -> m03 - 1 or (3 shl 2) -> m13 - 2 or (3 shl 2) -> m23 - 3 or (3 shl 2) -> m33 - else -> throw IllegalStateException("$index") - } -} - -operator fun Matrix4f.set(row: Int, column: Int, value: Float) { - require(row in 0 .. 3) { "Invalid row: $row" } - require(column in 0 .. 3) { "Invalid column: $column" } - - when (val index = row or (column shl 2)) { - 0 or (0 shl 2) -> m00 = value - 1 or (0 shl 2) -> m10 = value - 2 or (0 shl 2) -> m20 = value - 3 or (0 shl 2) -> m30 = value - 0 or (1 shl 2) -> m01 = value - 1 or (1 shl 2) -> m11 = value - 2 or (1 shl 2) -> m21 = value - 3 or (1 shl 2) -> m31 = value - 0 or (2 shl 2) -> m02 = value - 1 or (2 shl 2) -> m12 = value - 2 or (2 shl 2) -> m22 = value - 3 or (2 shl 2) -> m32 = value - 0 or (3 shl 2) -> m03 = value - 1 or (3 shl 2) -> m13 = value - 2 or (3 shl 2) -> m23 = value - 3 or (3 shl 2) -> m33 = value - else -> throw IllegalStateException("$index") - } -} - -operator fun Matrix3f.get(row: Int, column: Int): Float { - require(row in 0 .. 2) { "Invalid row: $row" } - require(column in 0 .. 2) { "Invalid column: $column" } - - return when (val index = row or (column shl 2)) { - 0 or (0 shl 2) -> m00 - 1 or (0 shl 2) -> m10 - 2 or (0 shl 2) -> m20 - 0 or (1 shl 2) -> m01 - 1 or (1 shl 2) -> m11 - 2 or (1 shl 2) -> m21 - 0 or (2 shl 2) -> m02 - 1 or (2 shl 2) -> m12 - 2 or (2 shl 2) -> m22 - else -> throw IllegalStateException("$index") - } -} - -@Suppress("EXTENSION_SHADOWED_BY_MEMBER") -operator fun Matrix3f.set(row: Int, column: Int, value: Float) { - require(row in 0 .. 2) { "Invalid row: $row" } - require(column in 0 .. 2) { "Invalid column: $column" } - - when (val index = row or (column shl 2)) { - 0 or (0 shl 2) -> m00 = value - 1 or (0 shl 2) -> m10 = value - 2 or (0 shl 2) -> m20 = value - 0 or (1 shl 2) -> m01 = value - 1 or (1 shl 2) -> m11 = value - 2 or (1 shl 2) -> m21 = value - 0 or (2 shl 2) -> m02 = value - 1 or (2 shl 2) -> m12 = value - 2 or (2 shl 2) -> m22 = value - else -> throw IllegalStateException("$index") - } -} - -fun Matrix4f.identity() = setIdentity() -fun Matrix3f.identity() = setIdentity() - -fun Matrix4f.identityFast() { - m00 = 1.0F - m11 = 1.0F - m22 = 1.0F - m33 = 1.0F -} - -fun Matrix3f.identityFast() { - m00 = 1.0F - m11 = 1.0F - m22 = 1.0F -} - fun Matrix3f.toMatrix4f(): Matrix4f { val result = Matrix4f() - result.m00 = this.m00 - result.m10 = this.m10 - result.m20 = this.m20 - result.m01 = this.m01 - result.m11 = this.m11 - result.m21 = this.m21 - result.m02 = this.m02 - result.m12 = this.m12 - result.m22 = this.m22 + result.zero() + + result.m00(this.m00) + result.m10(this.m10) + result.m20(this.m20) + result.m01(this.m01) + result.m11(this.m11) + result.m21(this.m21) + result.m02(this.m02) + result.m12(this.m12) + result.m22(this.m22) return result } var Matrix4f.translation: Vector3f get() { - return Vector3f(this[0, 3], this[1, 3], this[2, 3]) + return Vector3f(this[3, 0], this[3, 1], this[3, 2]) } set(value) { @@ -193,7 +92,7 @@ var Matrix4f.translation: Vector3f var Matrix4f.translation4: Vector4f get() { - return Vector4f(this[0, 3], this[1, 3], this[2, 3], this[3, 3]) + return Vector4f(this[3, 0], this[3, 1], this[3, 2], this[3, 3]) } set(value) {