Merge pull request #2 from dwursteisen/fix-rotation

Fix rotation

build.gradle.kts

@@ -6,24 +6,33 @@ plugins {
     id("maven-publish")
     id("org.jlleitschuh.gradle.ktlint") version "9.2.1"
     id("com.jfrog.bintray") version "1.8.5"
+    id("io.github.gradle-nexus.publish-plugin") version "1.0.0"
+    id("org.jetbrains.dokka") version "1.4.20"
 }
 
-group = "com.github.dwursteisen.kotlin-math"
+group = "com.github.minigdx"
 version = project.properties["version"] ?: "1.0-SNAPSHOT"
 
 if (version == "unspecified") {
     version = "1.0-SNAPSHOT"
 }
 
 repositories {
+    mavenCentral()
     jcenter()
-    maven(url = "https://dl.bintray.com/kotlin/kotlin-eap")
+}
+
+val javadocJar = tasks.register<Jar>("javadocJar") {
+    dependsOn(tasks.getByName("dokkaHtml"))
+    archiveClassifier.set("javadoc")
+    from(project.buildDir.resolve("dokka"))
 }
 
 publishing {
     publications {
         create<MavenPublication>("maven") {
             from(components["kotlin"])
+            artifact(javadocJar.get())
         }
     }
 }
@@ -134,6 +143,12 @@ val bintrayKey = if (project.hasProperty("bintray_key")) {
     System.getProperty("BINTRAY_KEY")
 }
 
+nexusPublishing {
+    repositories {
+        sonatype()
+    }
+}
+
 configure<com.jfrog.bintray.gradle.BintrayExtension> {
     user = properties.getProperty("bintray.user") ?: bintrayUser
     key = properties.getProperty("bintray.key") ?: bintrayKey

settings.gradle.kts

@@ -3,5 +3,6 @@ pluginManagement {
     repositories {
         maven(url = "https://dl.bintray.com/kotlin/kotlin-eap")
         gradlePluginPortal()
+        jcenter()
     }
 }

src/commonMain/kotlin/com/curiouscreature/kotlin/math/Quaternion.kt

@@ -3,7 +3,9 @@ package com.curiouscreature.kotlin.math
 import kotlin.math.abs
 import kotlin.math.asin
 import kotlin.math.atan2
+import kotlin.math.cos
 import kotlin.math.sign
+import kotlin.math.sin
 import kotlin.math.sqrt
 
 data class Quaternion(val x: Float, val y: Float, val z: Float, val w: Float) {
@@ -40,6 +42,21 @@ data class Quaternion(val x: Float, val y: Float, val z: Float, val w: Float) {
         return Float3(roll, pitch, yaw)
     }
 
+    /** Multiplies this quaternion with another one in the form of this = this * other
+     *
+     * @param other Quaternion to multiply with
+     * @return This quaternion for chaining
+     */
+    fun mul(other: Quaternion): Quaternion {
+        val newX: Float = this.w * other.x + this.x * other.w + this.y * other.z - this.z * other.y
+        val newY: Float = this.w * other.y + this.y * other.w + this.z * other.x - this.x * other.z
+        val newZ: Float = this.w * other.z + this.z * other.w + this.x * other.y - this.y * other.x
+        val newW: Float = this.w * other.w - this.x * other.x - this.y * other.y - this.z * other.z
+        return Quaternion(newX, newY, newZ, newW)
+    }
+
+    operator fun Quaternion.times(other: Quaternion): Quaternion = this.mul(other)
+
     override fun toString(): String {
         return "($x $y $z $w)"
     }
@@ -50,40 +67,68 @@ data class Quaternion(val x: Float, val y: Float, val z: Float, val w: Float) {
             return Quaternion(0f, 0f, 0f, 1f)
         }
 
-        // https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
-        fun from(m: Mat4): Quaternion {
+        /**
+         * Construct a quaternion using a [Mat4]
+         */
+        fun from(m2: Mat4): Quaternion {
+            // https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
+            // The code is a conversion from the `setFromAxis` method from LibGDX.
+            val m = transpose(m2)
             val tr = m.x.x + m.y.y + m.z.z
 
             return if (tr > 0) {
-                val s = sqrt(tr + 1f) * 2f // S=4*qw
-                val qw = 0.25f * s
-                val qx = (m.z.y - m.y.z) / s
-                val qy = (m.x.z - m.z.x) / s
-                val qz = (m.y.x - m.x.y) / s
+                var s = sqrt(tr + 1f)
+                val qw = s * 0.5f
+                s = 0.5f / s
+                val qx = (m.z.y - m.y.z) * s
+                val qy = (m.x.z - m.z.x) * s
+                val qz = (m.y.x - m.x.y) * s
                 Quaternion(qx, qy, qz, qw)
             } else if ((m.x.x > m.y.y) && (m.x.x > m.z.z)) {
-                val s = sqrt(1f + m.x.x - m.y.y - m.z.z) * 2; // S=4*qx
-                val qw = (m.z.y - m.y.z) / s
-                val qx = 0.25f * s
-                val qy = (m.x.y + m.y.x) / s
-                val qz = (m.x.z + m.z.x) / s
+                var s = sqrt(1f + m.x.x - m.y.y - m.z.z)
+                val qx = s * 0.5f
+                s = 0.5f / s
+                val qy = (m.y.x + m.x.y) * s
+                val qz = (m.x.z + m.z.x) * s
+                val qw = (m.z.y - m.y.z) * s
                 Quaternion(qx, qy, qz, qw)
             } else if (m.y.y > m.z.z) {
-                val s = sqrt(1.0f + m.y.y - m.x.x - m.z.z) * 2; // S=4*qy
-                val qw = (m.x.z - m.z.x) / s
-                val qx = (m.x.y + m.y.x) / s
-                val qy = 0.25f * s
-                val qz = (m.y.z + m.z.y) / s
+                var s = sqrt(1.0f + m.y.y - m.x.x - m.z.z)
+                val qy = s * 0.5f
+                s = 0.5f / s
+                val qx = (m.y.x + m.x.y) * s
+                val qz = (m.y.z + m.z.y) * s
+                val qw = (m.x.z - m.z.x) * s
                 Quaternion(qx, qy, qz, qw)
             } else {
-                val s = sqrt(1.0f + m.z.z - m.x.x - m.y.y) * 2 // S=4*qz
-                val qw = (m.y.x - m.x.y) / s
-                val qx = (m.x.z + m.z.x) / s
-                val qy = (m.y.z + m.z.y) / s
-                val qz = 0.25f * s
+                var s = sqrt(1.0f + m.z.z - m.x.x - m.y.y)
+                val qz = s * 0.5f
+                s = 0.5f / s
+                val qx = (m.x.z + m.z.x) * s
+                val qy = (m.z.y + m.y.z) * s
+                val qw = (m.y.x - m.x.y) * s
                 Quaternion(qx, qy, qz, qw)
             }
         }
+
+        /**
+         * Create a Quaternion from Eulers angle.
+         * Angles are expressed in degres.
+         */
+        fun fromEulers(x: Float, y: Float, z: Float, angle: Degrees): Quaternion {
+            var d: Float = length(Float3(x, y, z))
+            if (d == 0f) return identity()
+            d = 1f / d
+            val radians = radians(angle)
+            val l_ang: Float = if (radians < 0) TWO_PI - -radians % TWO_PI else radians % TWO_PI
+            val l_sin = sin(l_ang / 2f)
+            return normalize(Quaternion(d * x * l_sin, d * y * l_sin, d * z * l_sin, cos(l_ang / 2f)))
+        }
+
+        /**
+         * @see [fromEulers]
+         */
+        fun fromEulers(vector: Float3, angle: Degrees): Quaternion = fromEulers(vector.x, vector.y, vector.z, angle)
     }
 }
 

src/commonMain/kotlin/com/curiouscreature/kotlin/math/Vector.kt

@@ -1343,10 +1343,18 @@ data class Bool4(
     }
 }
 
+/**
+ * Interpolation between [a] and [b].
+ * [blend] is expected to be a value between 0..1.
+ *
+ * It returns a vector close to a when the blend value is close to 0.
+ * It returns a vector close to b when the blend value is close to 1.
+ *
+ */
 fun interpolate(a: Float3, b: Float3, blend: Float): Float3 {
     val x = a.x + blend * (b.x - a.x)
-    val y = a.y + blend * (b.y - a.x)
-    val z = a.z + blend * (b.z - a.x)
+    val y = a.y + blend * (b.y - a.y)
+    val z = a.z + blend * (b.z - a.z)
 
     return Float3(x, y, z)
 }

src/commonTest/kotlin/com/curiouscreature/kotlin/math/MatrixTest.kt

@@ -246,19 +246,19 @@ class MatrixTest {
     @Test
     fun Mat4_rotation_by_axis_x() {
         val x = rotation(Float3(1f, 0f, 0f), 90f).rotation.x
-        assertEquals(-90f, x, delta = 0.1f)
+        assertEquals(90f, x, delta = 0.1f)
     }
 
     @Test
     fun Mat4_rotation_by_axis_y() {
         val y = rotation(Float3(0f, 1f, 0f), 90f).rotation.y
-        assertEquals(-90f, y, delta = 0.1f)
+        assertEquals(90f, y, delta = 0.1f)
     }
 
     @Test
     fun Mat4_rotation_by_axis_z() {
         val z = rotation(Float3(0f, 0f, 1f), 90f).rotation.z
-        assertEquals(-90f, z, delta = 0.1f)
+        assertEquals(90f, z, delta = 0.1f)
     }
 
     @Test

src/commonTest/kotlin/com/curiouscreature/kotlin/math/QuaternionTest.kt

@@ -24,21 +24,28 @@ class QuaternionTest {
         )
 
         assertEquals(
-            normalize(Quaternion(-0.7071068f, 0.0f, 0.0f, 0.7071067f)),
-            Quaternion.from(Mat4.identity() * rotation(Float3(1f, 0f, 0f), 90f))
+            normalize(Quaternion(0.7071068f, 0.0f, 0.0f, 0.7071067f)),
+            Quaternion.from(rotation(Float3(1f, 0f, 0f), 90f))
         )
 
         assertEquals(
-            normalize(Quaternion(0.0f, 0.0f, -0.70710677f, 0.70710677f)),
-            Quaternion.from(Mat4.identity() * rotation(Float3(0f, 0f, 1f), 90f))
+            normalize(Quaternion(0.0f, 0.0f, 0.70710677f, 0.70710677f)),
+            Quaternion.from(rotation(Float3(0f, 0f, 1f), 90f))
         )
 
         assertEquals(
-            normalize(Quaternion(-0.5f, 0.0f, -0.5f, 0.70710677f)),
-            Quaternion.from(Mat4.identity() * rotation(normalize(Float3(1f, 0f, 1f)), 90f))
+            normalize(Quaternion(0.5f, 0.0f, 0.5f, 0.70710677f)),
+            Quaternion.from(rotation(normalize(Float3(1f, 0f, 1f)), 90f))
         )
     }
 
+    @Test
+    fun fromEulers() {
+        val quaternion = Quaternion.fromEulers(1f, 0f, 0f, 90f)
+        val matrix = Quaternion.from(rotation(normalize(Float3(1f, 0f, 0f)), 90f))
+        assertEquals(matrix, quaternion)
+    }
+
     companion object {
         fun assertEquals(expected: Quaternion, actual: Quaternion) {
             assertEquals(expected.x, actual.x)

src/commonTest/kotlin/com/curiouscreature/kotlin/math/VectorTest.kt

@@ -0,0 +1,19 @@
+package com.curiouscreature.kotlin.math
+
+import kotlin.test.Test
+
+class VectorTest {
+
+    @Test
+    fun Float3_interpolation() {
+        val resultA = interpolate(Float3(0f, 1f, 2f), Float3(3f, 4f, 5f), 0f)
+        assertEquals(0f, resultA.x)
+        assertEquals(1f, resultA.y)
+        assertEquals(2f, resultA.z)
+
+        val resultB = interpolate(Float3(0f, 1f, 2f), Float3(3f, 4f, 5f), 1f)
+        assertEquals(3f, resultB.x)
+        assertEquals(4f, resultB.y)
+        assertEquals(5f, resultB.z)
+    }
+}

src/jvmTest/kotlin/com/curiouscreature/kotlin/math/LibGDXQuaternionTest.kt

@@ -4,7 +4,6 @@ import com.badlogic.gdx.math.Matrix4
 import com.badlogic.gdx.math.Quaternion as Quat
 import com.badlogic.gdx.math.Vector3
 import com.badlogic.gdx.utils.GdxNativesLoader
-import kotlin.math.abs
 import kotlin.test.Test
 
 class LibGDXQuaternionTest {
@@ -41,8 +40,8 @@ class LibGDXQuaternionTest {
 
     fun assertEquals(expected: Quat, actual: Quaternion) {
         assertArrayEquals(
-            floatArrayOf(abs(expected.x), abs(expected.y), abs(expected.z), abs(expected.w)),
-            floatArrayOf(abs(actual.x), abs(actual.y), abs(actual.z), abs(actual.w))
+            floatArrayOf(expected.x, expected.y, expected.z, expected.w),
+            floatArrayOf(actual.x, actual.y, actual.z, actual.w)
         )
     }
 }