Add Quaternion Rotation (#33)

* Add Quaternion Rotation

* Remove unnecessary Mat4 allocation

* Move and rename quaternion Mat4 extraction

* Documentation

* Quaternion dedicated data class

* Fix Quaternion operator wrong return type

* Handle multiple Euler rotations orders

Author: ThomasGorisse

README.md

@@ -187,6 +187,29 @@ You can also use the invoke operator to access elements in row-major mode with
 v = myMat4(2, 3) // equivalent to myMat4[2, 1]
 ```
 
+## Rotation types
+
+Construct a Euler Angle Rotation Matrix using per axis angles in degrees
+
+```kotlin
+rotationMatrix = rotation(d = Float3(y = 90.0f)) // rotation of 90° around y axis
+```
+
+Construct a Euler Angle Rotation Matrix using axis direction and angle in degree
+
+```kotlin
+rotationMatrix = rotation(axis = Float3(y = 1.0f), angle = 90.0f) // rotation of 90° around y axis
+```
+
+Construct a Quaternion Rotation Matrix following the Hamilton convention.
+Assume the destination and local coordinate spaces are initially aligned, and the local coordinate
+space is then rotated counter-clockwise about a unit-length axis, k, by an angle, theta.
+
+```kotlin
+rotationMatrix = rotation(quaternion = Float4(y = 1.0f, w = 1.0f)) // rotation of 90° around y axis
+```
+
+
 ## Scalar APIs
 
 The file `Scalar.kt` contains various helper methods to use common math operations

src/commonMain/kotlin/dev/romainguy/kotlin/math/Matrix.kt

@@ -24,6 +24,10 @@ enum class MatrixColumn {
     X, Y, Z, W
 }
 
+enum class RotationsOrder {
+    XYZ, XZY, YXZ, YZX, ZXY, ZYX
+}
+
 data class Mat2(
         var x: Float2 = Float2(x = 1.0f),
         var y: Float2 = Float2(y = 1.0f)) {
@@ -41,14 +45,14 @@ data class Mat2(
         fun identity() = Mat2()
     }
 
-    operator fun get(column: Int) = when(column) {
+    operator fun get(column: Int) = when (column) {
         0 -> x
         1 -> y
         else -> throw IllegalArgumentException("column must be in 0..1")
     }
     operator fun get(column: Int, row: Int) = get(column)[row]
 
-    operator fun get(column: MatrixColumn) = when(column) {
+    operator fun get(column: MatrixColumn) = when (column) {
         MatrixColumn.X -> x
         MatrixColumn.Y -> y
         else -> throw IllegalArgumentException("column must be X or Y")
@@ -124,15 +128,15 @@ data class Mat3(
         fun identity() = Mat3()
     }
 
-    operator fun get(column: Int) = when(column) {
+    operator fun get(column: Int) = when (column) {
         0 -> x
         1 -> y
         2 -> z
         else -> throw IllegalArgumentException("column must be in 0..2")
     }
     operator fun get(column: Int, row: Int) = get(column)[row]
 
-    operator fun get(column: MatrixColumn) = when(column) {
+    operator fun get(column: MatrixColumn) = when (column) {
         MatrixColumn.X -> x
         MatrixColumn.Y -> y
         MatrixColumn.Z -> z
@@ -263,7 +267,7 @@ data class Mat4(
     inline val upperLeft: Mat3
         get() = Mat3(x.xyz, y.xyz, z.xyz)
 
-    operator fun get(column: Int) = when(column) {
+    operator fun get(column: Int) = when (column) {
         0 -> x
         1 -> y
         2 -> z
@@ -272,7 +276,7 @@ data class Mat4(
     }
     operator fun get(column: Int, row: Int) = get(column)[row]
 
-    operator fun get(column: MatrixColumn) = when(column) {
+    operator fun get(column: MatrixColumn) = when (column) {
         MatrixColumn.X -> x
         MatrixColumn.Y -> y
         MatrixColumn.Z -> z
@@ -333,6 +337,8 @@ data class Mat4(
             x.w * v.x + y.w * v.y + z.w * v.z+ w.w * v.w
     )
 
+    fun toQuaternion() = quaternion(this)
+
     fun toFloatArray() = floatArrayOf(
             x.x, y.x, z.x, w.x,
             x.y, y.y, z.y, w.y,
@@ -468,18 +474,90 @@ fun translation(t: Float3) = Mat4(w = Float4(t, 1.0f))
 fun translation(m: Mat4) = translation(m.translation)
 
 fun rotation(m: Mat4) = Mat4(normalize(m.right), normalize(m.up), normalize(m.forward))
-fun rotation(d: Float3): Mat4 {
+
+/**
+ * Construct a rotation matrix from Euler angles using YPR around a specified order
+ *
+ * Uses intrinsic Tait-Bryan angles. This means that rotations are performed with respect to the
+ * local coordinate system.
+ * That is, for order 'XYZ', the rotation is first around the X axis (which is the same as the
+ * world-X axis), then around local-Y (which may now be different from the world Y-axis),
+ * then local-Z (which may be different from the world Z-axis)
+ *
+ * @param d Per axis Euler angles in degrees
+ * Yaw, pitch, roll (YPR) are taken accordingly to the rotations order input.
+ * @param order The order in which to apply rotations.
+ * Default is [RotationsOrder.ZYX] which means that the object will first be rotated around its Z
+ * axis, then its Y axis and finally its X axis.
+ *
+ * @return The rotation matrix
+ */
+fun rotation(d: Float3, order: RotationsOrder = RotationsOrder.ZYX): Mat4 {
     val r = transform(d, ::radians)
-    val c = transform(r) { x -> cos(x) }
-    val s = transform(r) { x -> sin(x) }
+    return when(order) {
+        RotationsOrder.XZY -> rotation(r.x, r.z, r.y)
+        RotationsOrder.XYZ -> rotation(r.x, r.y, r.z)
+        RotationsOrder.YXZ -> rotation(r.y, r.x, r.z)
+        RotationsOrder.YZX -> rotation(r.y, r.z, r.x)
+        RotationsOrder.ZYX -> rotation(r.z, r.y, r.x)
+        RotationsOrder.ZXY -> rotation(r.z, r.x, r.y)
+    }
+}
 
-    return Mat4.of(
-            c.y * c.z, -c.x * s.z + s.x * s.y * c.z, s.x * s.z + c.x * s.y * c.z, 0.0f,
-            c.y * s.z, c.x * c.z + s.x * s.y * s.z, -s.x * c.z + c.x * s.y * s.z, 0.0f,
-            -s.y, s.x * c.y, c.x * c.y, 0.0f,
-            0.0f, 0.0f, 0.0f, 1.0f
-    )
+/**
+ * Construct a rotation matrix from Euler yaw, pitch, roll around a specified order.
+ *
+ * @param roll about 1st rotation axis in radians. Z in case of ZYX order
+ * @param pitch about 2nd rotation axis in radians. Y in case of ZYX order
+ * @param yaw about 3rd rotation axis in radians. X in case of ZYX order
+ * @param order The order in which to apply rotations.
+ * Default is [RotationsOrder.ZYX] which means that the object will first be rotated around its Z
+ * axis, then its Y axis and finally its X axis.
+ *
+ * @return The rotation matrix
+ */
+fun rotation(yaw: Float = 0.0f, pitch: Float = 0.0f, roll: Float = 0.0f, order: RotationsOrder = RotationsOrder.ZYX): Mat4 {
+    val c1 = cos(yaw)
+    val s1 = sin(yaw)
+    val c2 = cos(pitch)
+    val s2 = sin(pitch)
+    val c3 = cos(roll)
+    val s3 = sin(roll)
+
+    return when (order) {
+        RotationsOrder.XZY -> Mat4.of(
+                c2 * c3, -s2, c2 * s3, 0.0f,
+                s1 * s3 + c1 * c3 * s2, c1 * c2, c1 * s2 * s3 - c3 * s1, 0.0f,
+                c3 * s1 * s2 - c1 * s3, c2 * s1, c1 * c3 + s1 * s2 * s3, 0.0f,
+                0.0f, 0.0f, 0.0f, 1.0f)
+        RotationsOrder.XYZ -> Mat4.of(
+                c2 * c3, -c2 * s3, s2, 0.0f,
+                c1 * s3 + c3 * s1 * s2, c1 * c3 - s1 * s2 * s3, -c2 * s1, 0.0f,
+                s1 * s3 - c1 * c3 * s2, c3 * s1 + c1 * s2 * s3, c1 * c2, 0.0f,
+                0.0f, 0.0f, 0.0f, 1.0f)
+        RotationsOrder.YXZ -> Mat4.of(
+                c1 * c3 + s1 * s2 * s3, c3 * s1 * s2 - c1 * s3, c2 * s1, 0.0f,
+                c2 * s3, c2 * c3, -s2, 0.0f,
+                c1 * s2 * s3 - c3 * s1, c1 * c3 * s2 + s1 * s3, c1 * c2, 0.0f,
+                0.0f, 0.0f, 0.0f, 1.0f)
+        RotationsOrder.YZX -> Mat4.of(
+                c1 * c2, s1 * s3 - c1 * c3 * s2, c3 * s1 + c1 * s2 * s3, 0.0f,
+                s2, c2 * c3, -c2 * s3, 0.0f,
+                -c2 * s1, c1 * s3 + c3 * s1 * s2, c1 * c3 - s1 * s2 * s3, 0.0f,
+                0.0f, 0.0f, 0.0f, 1.0f)
+        RotationsOrder.ZYX -> Mat4.of(
+                c1 * c2, c1 * s2 * s3 - c3 * s1, s1 * s3 + c1 * c3 * s2, 0.0f,
+                c2 * s1, c1 * c3 + s1 * s2 * s3, c3 * s1 * s2 - c1 * s3, 0.0f,
+                -s2, c2 * s3, c2 * c3, 0.0f,
+                0.0f, 0.0f, 0.0f, 1.0f)
+        RotationsOrder.ZXY -> Mat4.of(
+                c1 * c3 - s1 * s2 * s3, -c2 * s1, c1 * s3 + c3 * s1 * s2, 0.0f,
+                c3 * s1 + c1 * s2 * s3, c1 * c2, s1 * s3 - c1 * c3 * s2, 0.0f,
+                -c2 * s3, s2, c2 * c3, 0.0f,
+                0.0f, 0.0f, 0.0f, 1.0f)
+    }
 }
+
 fun rotation(axis: Float3, angle: Float): Mat4 {
     val x = axis.x
     val y = axis.y
@@ -498,6 +576,81 @@ fun rotation(axis: Float3, angle: Float): Mat4 {
     )
 }
 
+/**
+ * Construct a Quaternion Rotation Matrix following the Hamilton convention
+ *
+ * Assume the destination and local coordinate spaces are initially aligned, and the local
+ * coordinate space is then rotated counter-clockwise about a unit-length axis, k, by an angle,
+ * theta.
+ */
+fun rotation(quaternion: Quaternion): Mat4 {
+    val n = normalize(quaternion)
+    return Mat4(
+            Float4(
+                1.0f - 2.0f * (n.y * n.y + n.z * n.z),
+                2.0f * (n.x * n.y - n.z * n.w),
+                2.0f * (n.x * n.z + n.y * n.w)
+            ),
+            Float4(
+                2.0f * (n.x * n.y + n.z * n.w),
+                1.0f - 2.0f * (n.x * n.x + n.z * n.z),
+                2.0f * (n.y * n.z - n.x * n.w)
+            ),
+            Float4(
+                2.0f * (n.x * n.z - n.y * n.w),
+                2.0f * (n.y * n.z + n.x * n.w),
+                1.0f - 2.0f * (n.x * n.x + n.y * n.y)
+            )
+    )
+}
+
+/**
+ * Extract Quaternion rotation from a Matrix
+ */
+fun quaternion(m: Mat4): Quaternion {
+    val trace = m.x.x + m.y.y + m.z.z
+    return normalize(
+        when {
+            trace > 0 -> {
+                val s = sqrt(trace + 1.0f) * 2.0f
+                Quaternion(
+                    (m.z.y - m.y.z) / s,
+                    (m.x.z - m.z.x) / s,
+                    (m.y.x - m.x.y) / s,
+                    0.25f * s
+                )
+            }
+            m.x.x > m.y.y && m.x.x > m.z.z -> {
+                val s = sqrt(1.0f + m.x.x - m.y.y - m.z.z) * 2.0f
+                Quaternion(
+                    0.25f * s,
+                    (m.x.y + m.y.x) / s,
+                    (m.x.z + m.z.x) / s,
+                    (m.z.y - m.y.z) / s
+                )
+            }
+            m.y.y > m.z.z -> {
+                val s = sqrt(1.0f + m.y.y - m.x.x - m.z.z) * 2.0f
+                Quaternion(
+                    (m.x.y + m.y.x) / s,
+                    0.25f * s,
+                    (m.y.z + m.z.y) / s,
+                    (m.x.z - m.z.x) / s
+                )
+            }
+            else -> {
+                val s = sqrt(1.0f + m.z.z - m.x.x - m.y.y) * 2.0f
+                Quaternion(
+                    (m.y.x - m.x.y) / s,
+                    (m.x.z + m.z.x) / s,
+                    (m.y.z + m.z.y) / s,
+                    0.25f * s
+                )
+            }
+        }
+    )
+}
+
 fun normal(m: Mat4) = scale(1.0f / Float3(length2(m.right), length2(m.up), length2(m.forward))) * m
 
 fun lookAt(eye: Float3, target: Float3, up: Float3 = Float3(z = 1.0f)): Mat4 {