From c49ce136094beb4d6c676728347074b1654567b9 Mon Sep 17 00:00:00 2001 From: Thomas Gorisse Date: Tue, 25 Jan 2022 23:28:22 +0100 Subject: [PATCH] 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 --- README.md | 23 ++ .../dev/romainguy/kotlin/math/Matrix.kt | 183 +++++++++- .../dev/romainguy/kotlin/math/Quaternion.kt | 342 ++++++++++++++++++ .../dev/romainguy/kotlin/math/Vector.kt | 6 + .../dev/romainguy/kotlin/math/MatrixTest.kt | 19 +- .../romainguy/kotlin/math/QuaternionTest.kt | 46 +++ 6 files changed, 601 insertions(+), 18 deletions(-) create mode 100644 src/commonMain/kotlin/dev/romainguy/kotlin/math/Quaternion.kt create mode 100644 src/commonTest/kotlin/dev/romainguy/kotlin/math/QuaternionTest.kt diff --git a/README.md b/README.md index d346d88..65609cd 100644 --- a/README.md +++ b/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 diff --git a/src/commonMain/kotlin/dev/romainguy/kotlin/math/Matrix.kt b/src/commonMain/kotlin/dev/romainguy/kotlin/math/Matrix.kt index d669fc7..fa1f879 100644 --- a/src/commonMain/kotlin/dev/romainguy/kotlin/math/Matrix.kt +++ b/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,7 +128,7 @@ 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 @@ -132,7 +136,7 @@ data class Mat3( } 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 { diff --git a/src/commonMain/kotlin/dev/romainguy/kotlin/math/Quaternion.kt b/src/commonMain/kotlin/dev/romainguy/kotlin/math/Quaternion.kt new file mode 100644 index 0000000..1d527a7 --- /dev/null +++ b/src/commonMain/kotlin/dev/romainguy/kotlin/math/Quaternion.kt @@ -0,0 +1,342 @@ +/* + * Copyright (C) 2017 Romain Guy + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +@file:Suppress("unused") + +package dev.romainguy.kotlin.math + +import kotlin.math.* + +enum class QuaternionComponent { + X, Y, Z, W +} + +/** + * Construct Quaternion and set each value. + * The Quaternion will be normalized during construction + * Default: Identity + */ +data class Quaternion( + var x: Float = 0.0f, + var y: Float = 0.0f, + var z: Float = 0.0f, + var w: Float = 0.0f) { + + constructor(v: Float3, w: Float = 0.0f) : this(v.x, v.y, v.z, w) + constructor(v: Float4) : this(v.x, v.y, v.z, v.w) + constructor(q: Quaternion) : this(q.x, q.y, q.z, q.w) + + companion object { + /** + * Construct a Quaternion from an axis and angle in degrees + * + * @param axis Rotation direction + * @param angle Angle size in degrees + */ + fun fromAxisAngle(axis: Float3, angle: Float): Quaternion { + val r = radians(angle) + return Quaternion(sin(r * 0.5f) * normalize(axis), cos(r * 0.5f)) + } + + /** + * Construct a Quaternion from Euler angles using YPR around ZYX respectively + * + * The Euler angles are applied in ZYX order. + * i.e: a vector is first rotated about X (roll) then Y (pitch) and then Z (yaw). + * + * @param d Per axis Euler angles in degrees + */ + fun fromEuler(d: Float3): Quaternion { + val r = transform(d, ::radians) + return fromEulerZYX(r.z, r.y, r.x) + } + + /** + * Construct a Quaternion from Euler angles using YPR around ZYX respectively + * + * The Euler angles are applied in ZYX order. + * i.e: a vector is first rotated about X (roll) then Y (pitch) and then Z (yaw). + * + * @param roll about X axis in radians + * @param pitch about Y axis in radians + * @param yaw about Z axis in radians + */ + fun fromEulerZYX(yaw: Float = 0.0f, pitch: Float = 0.0f, roll: Float = 0.0f): Quaternion { + val cy = cos(yaw * 0.5f) + val sy = sin(yaw * 0.5f) + val cp = cos(pitch * 0.5f) + val sp = sin(pitch * 0.5f) + val cr = cos(roll * 0.5f) + val sr = sin(roll * 0.5f) + + return Quaternion( + sr * cp * cy - cr * sp * sy, + cr * sp * cy + sr * cp * sy, + cr * cp * sy - sr * sp * cy, + cr * cp * cy + sr * sp * sy + ) + } + } + + inline var xyz: Float3 + get() = Float3(x, y, z) + set(value) { + x = value.x + y = value.y + z = value.z + } + + inline var imaginary: Float3 + get() = xyz + set(value) { + x = value.x + y = value.y + z = value.z + } + + inline var real: Float + get() = w + set(value) { + w = value + } + + inline var xyzw: Float4 + get() = Float4(x, y, z, w) + set(value) { + x = value.x + y = value.y + z = value.z + w = value.w + } + + operator fun get(index: QuaternionComponent) = when (index) { + QuaternionComponent.X -> x + QuaternionComponent.Y -> y + QuaternionComponent.Z -> z + QuaternionComponent.W -> w + } + + operator fun get( + index1: QuaternionComponent, + index2: QuaternionComponent, + index3: QuaternionComponent): Float3 { + return Float3(get(index1), get(index2), get(index3)) + } + + operator fun get( + index1: QuaternionComponent, + index2: QuaternionComponent, + index3: QuaternionComponent, + index4: QuaternionComponent): Quaternion { + return Quaternion(get(index1), get(index2), get(index3), get(index4)) + } + + operator fun get(index: Int) = when (index) { + 0 -> x + 1 -> y + 2 -> z + 3 -> w + else -> throw IllegalArgumentException("index must be in 0..3") + } + + operator fun get(index1: Int, index2: Int, index3: Int): Float3 { + return Float3(get(index1), get(index2), get(index3)) + } + + operator fun get(index1: Int, index2: Int, index3: Int, index4: Int): Quaternion { + return Quaternion(get(index1), get(index2), get(index3), get(index4)) + } + + inline operator fun invoke(index: Int) = get(index - 1) + + operator fun set(index: Int, v: Float) = when (index) { + 0 -> x = v + 1 -> y = v + 2 -> z = v + 3 -> w = v + else -> throw IllegalArgumentException("index must be in 0..3") + } + + operator fun set(index1: Int, index2: Int, v: Float) { + set(index1, v) + set(index2, v) + } + + operator fun set(index1: Int, index2: Int, index3: Int, v: Float) { + set(index1, v) + set(index2, v) + set(index3, v) + } + + operator fun set(index1: Int, index2: Int, index3: Int, index4: Int, v: Float) { + set(index1, v) + set(index2, v) + set(index3, v) + set(index4, v) + } + + operator fun set(index: QuaternionComponent, v: Float) = when (index) { + QuaternionComponent.X -> x = v + QuaternionComponent.Y -> y = v + QuaternionComponent.Z -> z = v + QuaternionComponent.W -> w = v + } + + operator fun set(index1: QuaternionComponent, index2: QuaternionComponent, v: Float) { + set(index1, v) + set(index2, v) + } + + operator fun set( + index1: QuaternionComponent, index2: QuaternionComponent, index3: QuaternionComponent, v: Float) { + set(index1, v) + set(index2, v) + set(index3, v) + } + + operator fun set( + index1: QuaternionComponent, index2: QuaternionComponent, + index3: QuaternionComponent, index4: QuaternionComponent, v: Float) { + set(index1, v) + set(index2, v) + set(index3, v) + set(index4, v) + } + + operator fun unaryMinus() = Quaternion(-x, -y, -z, -w) + + inline operator fun plus(v: Float) = Quaternion(x + v, y + v, z + v, w + v) + inline operator fun minus(v: Float) = Quaternion(x - v, y - v, z - v, w - v) + inline operator fun times(v: Float) = Quaternion(x * v, y * v, z * v, w * v) + inline operator fun div(v: Float) = Quaternion(x / v, y / v, z / v, w / v) + + inline operator fun times(v: Float3) = (this * Quaternion(v, 0.0f) * inverse(this)).xyz + + inline operator fun plus(q: Quaternion) = Quaternion(x + q.x, y + q.y, z + q.z, w + q.w) + inline operator fun minus(q: Quaternion) = Quaternion(x - q.x, y - q.y, z - q.z, w - q.w) + inline operator fun times(q: Quaternion) = Quaternion( + w * q.x + x * q.w + y * q.z - z * q.y, + w * q.y - x * q.z + y * q.w + z * q.x, + w * q.z + x * q.y - y * q.x + z * q.w, + w * q.w - x * q.x - y * q.y - z * q.z) + + inline fun transform(block: (Float) -> Float): Quaternion { + x = block(x) + y = block(y) + z = block(z) + w = block(w) + return this + } + + fun toEulerAngles() = eulerAngles(this) + + fun toMatrix() = rotation(this) + + fun toFloatArray() = floatArrayOf(x, y, z, w) +} + +inline operator fun Float.plus(q: Quaternion) = Quaternion(this + q.x, this + q.y, this + q.z, this + q.w) +inline operator fun Float.minus(q: Quaternion) = Quaternion(this - q.x, this - q.y, this - q.z, this - q.w) +inline operator fun Float.times(q: Quaternion) = Quaternion(this * q.x, this * q.y, this * q.z, this * q.w) +inline operator fun Float.div(q: Quaternion) = Quaternion(this / q.x, this / q.y, this / q.z, this / q.w) + +inline fun abs(q: Quaternion) = Quaternion(abs(q.x), abs(q.y), abs(q.z), abs(q.w)) +inline fun length(q: Quaternion) = sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w) +inline fun length2(q: Quaternion) = q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w +inline fun dot(a: Quaternion, b: Quaternion) = a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w + +/** + * Rescale the Quaternion to the unit length + */ +fun normalize(q: Quaternion): Quaternion { + val l = 1.0f / length(q) + return Quaternion(q.x * l, q.y * l, q.z * l, q.w * l) +} + +fun conjugate(q: Quaternion): Quaternion = Quaternion(q.w, -q.x, -q.y, -q.z) + +fun inverse(q: Quaternion): Quaternion { + val d = 1.0f / dot(q, q) + return Quaternion(q.w * d, -q.x * d, -q.y * d, -q.w * d) +} + +fun cross(a: Quaternion, b: Quaternion): Quaternion { + val m = a * b + return Quaternion(m.x, m.y, m.z, 0.0f) +} + +/** + * Spherical linear interpolation between two given orientations + * + * If t is 0 this returns a. + * As t approaches 1 slerp may approach either b or -b (whichever is closest to a) + * If t is above 1 or below 0 the result will be extrapolated. + * @param a The beginning value + * @param b The ending value + * @param t The ratio between the two floats + * @param valueEps Prevent blowing up when slerping between two quaternions that are very near each + * other. Linear interpolation (lerp) is returned in this case. + * + * @return Interpolated value between the two floats + */ +fun slerp(a: Quaternion, b: Quaternion, t: Float, valueEps: Float = 0.0000000001f): Quaternion { + // could also be computed as: pow(q * inverse(p), t) * p; + val d = dot(a, b) + val absd = abs(d) + // Prevent blowing up when slerping between two quaternions that are very near each other. + if ((1.0f - absd) < valueEps) { + return normalize(lerp(if (d < 0.0f) -a else a, b, t)) + } + val npq = sqrt(dot(a, a) * dot(b, b)) // ||p|| * ||q|| + val acos = acos(clamp(absd / npq, -1.0f, 1.0f)) + val acos0 = acos * (1.0f - t) + val acos1 = acos * t + val sina = sin(acos) + if (sina < valueEps) { + return normalize(lerp(a, b, t)) + } + val isina = 1.0f / sina + val s0 = sin(acos0) * isina + val s1 = sin(acos1) * isina + // ensure we're taking the "short" side + return normalize(s0 * a + (if (d < 0.0f) -s1 else (s1)) * b) +} + +fun lerp(a: Quaternion, b: Quaternion, t: Float): Quaternion { + return ((1 - t) * a) + (t * b) +} + +fun nlerp(a: Quaternion, b: Quaternion, t: Float): Quaternion { + return normalize(lerp(a, b, t)) +} + +/** + * Convert a Quaternion to Euler angles using YPR around ZYX respectively + * + * The Euler angles are applied in ZYX order + */ +fun eulerAngles(q: Quaternion): Float3 { + val nq = normalize(q) + return Float3( + // roll (x-axis rotation) + degrees(atan2(2.0f * (nq.y * nq.z + nq.w * nq.x), + nq.w * nq.w - nq.x * nq.x - nq.y * nq.y + nq.z * nq.z)), + // pitch (y-axis rotation) + degrees(asin(-2.0f * (nq.x * nq.z - nq.w * nq.y))), + // yaw (z-axis rotation) + degrees(atan2(2.0f * (nq.x * nq.y + nq.w * nq.z), + nq.w * nq.w + nq.x * nq.x - nq.y * nq.y - nq.z * nq.z))) +} \ No newline at end of file diff --git a/src/commonMain/kotlin/dev/romainguy/kotlin/math/Vector.kt b/src/commonMain/kotlin/dev/romainguy/kotlin/math/Vector.kt index ceb8d13..90a9664 100644 --- a/src/commonMain/kotlin/dev/romainguy/kotlin/math/Vector.kt +++ b/src/commonMain/kotlin/dev/romainguy/kotlin/math/Vector.kt @@ -135,6 +135,8 @@ data class Float2(var x: Float = 0.0f, var y: Float = 0.0f) { y = block(y) return this } + + fun toFloatArray() = floatArrayOf(x, y) } data class Float3(var x: Float = 0.0f, var y: Float = 0.0f, var z: Float = 0.0f) { @@ -306,6 +308,8 @@ data class Float3(var x: Float = 0.0f, var y: Float = 0.0f, var z: Float = 0.0f) z = block(z) return this } + + fun toFloatArray() = floatArrayOf(x, y, z) } data class Float4( @@ -553,6 +557,8 @@ data class Float4( w = block(w) return this } + + fun toFloatArray() = floatArrayOf(x, y, z, w) } inline operator fun Float.plus(v: Float2) = Float2(this + v.x, this + v.y) diff --git a/src/commonTest/kotlin/dev/romainguy/kotlin/math/MatrixTest.kt b/src/commonTest/kotlin/dev/romainguy/kotlin/math/MatrixTest.kt index 1c836c7..8ebe936 100644 --- a/src/commonTest/kotlin/dev/romainguy/kotlin/math/MatrixTest.kt +++ b/src/commonTest/kotlin/dev/romainguy/kotlin/math/MatrixTest.kt @@ -249,9 +249,9 @@ class MatrixTest { fun rotationFloat3() { assertArrayEquals( Mat4( - Float4(0.998f, 0.0523f, -0.0348f, 0f), + Float4(0.9980f, 0.0523f, -0.0349f, 0f), Float4(-0.0517f, 0.9985f, 0.0174f, 0f), - Float4(0.0357f, -0.0156f, 0.9992f, 0f), + Float4(0.0358f, -0.0156f, 0.9992f, 0f), Float4(0f, 0f, 0f, 1f) ).toFloatArray(), rotation(Float3(1f, 2f, 3f)).toFloatArray() @@ -284,6 +284,19 @@ class MatrixTest { ) } + @Test + fun rotationQuaternion() { + assertArrayEquals( + Mat4( + Float4(-0.7333f, -0.1333f, 0.6667f, 0f), + Float4(0.66667f, -0.3333f, 0.6667f, 0f), + Float4(0.1333f, 0.93333f, 0.3333f, 0f), + Float4(0f, 0f, 0f, 1f) + ).toFloatArray(), + rotation(Quaternion(1f, 2f, 3f, 1f)).toFloatArray() + ) + } + @Test fun normal() { assertArrayEquals( @@ -382,7 +395,7 @@ class MatrixTest { Float4(4f, 8f, 12f, 16f) ) - private fun assertArrayEquals(expected: FloatArray, actual: FloatArray, delta: Float = 0.001f) { + internal fun assertArrayEquals(expected: FloatArray, actual: FloatArray, delta: Float = 0.001f) { assertEquals(expected.size, actual.size) assertTrue( expected.zip(actual).all { (a, b) -> (a - b).absoluteValue < delta }, diff --git a/src/commonTest/kotlin/dev/romainguy/kotlin/math/QuaternionTest.kt b/src/commonTest/kotlin/dev/romainguy/kotlin/math/QuaternionTest.kt new file mode 100644 index 0000000..f4b1e4d --- /dev/null +++ b/src/commonTest/kotlin/dev/romainguy/kotlin/math/QuaternionTest.kt @@ -0,0 +1,46 @@ +/* + * Copyright (C) 2017 Romain Guy + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +package dev.romainguy.kotlin.math + +import kotlin.test.Test + +class QuaternionTest { + + @Test + fun fromAxisAngle() { + MatrixTest.assertArrayEquals( + Quaternion(0.0093f, 0.0186f, 0.0280f, 0.9994f).toFloatArray(), + Quaternion.fromAxisAngle(Float3(1.0f, 2.0f, 3.0f), 4.0f).toFloatArray() + ) + } + + @Test + fun fromEuler() { + MatrixTest.assertArrayEquals( + Quaternion(0.0083f, 0.0177f, 0.0263f, 0.9995f).toFloatArray(), + Quaternion.fromEuler(Float3(1.0f, 2.0f, 3.0f)).toFloatArray() + ) + } + + @Test + fun toEuler() { + MatrixTest.assertArrayEquals( + Float3(45.0f, 19.4712f, 81.8699f).toFloatArray(), + eulerAngles(Quaternion(1.0f, 2.0f, 3.0f, 4.0f)).toFloatArray() + ) + } +} \ No newline at end of file