Skip to content

Commit

Permalink
Add Quaternion Rotation (#33)
Browse files Browse the repository at this point in the history 
* 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
  • Loading branch information
@ThomasGorisse
ThomasGorisse authored Jan 25, 2022
1 parent ec6e582 commit c49ce13
Show file tree
Hide file tree
Showing 6 changed files with 601 additions and 18 deletions.
23 changes: 23 additions & 0 deletions README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
183 changes: 168 additions & 15 deletions src/commonMain/kotlin/dev/romainguy/kotlin/math/Matrix.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)) {
Expand All @@ -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")
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand All @@ -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
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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
Expand All @@ -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 {
Expand Down
Loading

0 comments on commit c49ce13

Please sign in to comment.