update euclid math extensions

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())
+	x = -x
-	setY(-y())
+	y = -y
-	setZ(-z())
+	z = -z
 	return this
 }
 
-fun Vector3f.length() = sqrt(x().toDouble() * x() + y().toDouble() * y() + z().toDouble() * z())
+operator fun Vector3f.times(v: Float): Vector3f = mul(v)
-
-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.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 {
+fun Quaternion(vec: Vector, rotation: Float, isDegrees: Boolean = false): Quaternionf {
-	return Quaternion(vec.asVector3f(), rotation, isDegrees)
+	// 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 {
+fun Vector.rotate(q: Quaternionf): Vector {
-	val quaternion = Quaternion(q)
+	// TODO: 1.19.3
-	quaternion.mul(Quaternion(x.toFloat(), y.toFloat(), z.toFloat(), 0.0f))
+	val quaternion = Quaternionf(q)
-	val quaternion1 = Quaternion(q)
+	quaternion.mul(rotateAroundThis())
-	quaternion1.conj()
+	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 {
+fun Vector.rotateAroundThis(rotation: Float, isDegrees: Boolean = false): Quaternionf {
-	return Quaternion(this, rotation, isDegrees)
+	// 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 }
 	}

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 org.joml.Matrix3f
-import com.mojang.math.Matrix4f
+import org.joml.Matrix4f
-import com.mojang.math.Vector3f
+import org.joml.Vector3f
-import com.mojang.math.Vector4f
+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.zero()
-	result.m10 = this.m10
+
-	result.m20 = this.m20
+	result.m00(this.m00)
-	result.m01 = this.m01
+	result.m10(this.m10)
-	result.m11 = this.m11
+	result.m20(this.m20)
-	result.m21 = this.m21
+	result.m01(this.m01)
-	result.m02 = this.m02
+	result.m11(this.m11)
-	result.m12 = this.m12
+	result.m21(this.m21)
-	result.m22 = this.m22
+	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) {