1 /* 2 * Copyright (C) 2023 The Android Open Source Project 3 * 4 * Licensed under the Apache License, Version 2.0 (the "License"); 5 * you may not use this file except in compliance with the License. 6 * You may obtain a copy of the License at 7 * 8 * http://www.apache.org/licenses/LICENSE-2.0 9 * 10 * Unless required by applicable law or agreed to in writing, software 11 * distributed under the License is distributed on an "AS IS" BASIS, 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 * See the License for the specific language governing permissions and 14 * limitations under the License. 15 */ 16 17 package com.google.android.torus.math 18 19 import com.google.android.torus.math.MathUtils.DEG_TO_RAD 20 import com.google.android.torus.math.MathUtils.RAD_TO_DEG 21 import java.util.* 22 import kotlin.math.* 23 24 /** 25 * A unit quaternion representing a rotation. 26 */ 27 class RotationQuaternion { 28 companion object { 29 /** 30 * Creates a rotation quaternion from a quaternion. 31 * 32 * @param w The w value of a quaternion. 33 * @param x The x value of a quaternion. 34 * @param y The y value of a quaternion. 35 * @param z The z value of a quaternion. 36 */ 37 @JvmStatic fromQuaternionnull38 fun fromQuaternion(w: Double, x: Double, y: Double, z: Double): RotationQuaternion { 39 val rotation = 2.0 * atan2(sqrt(x * x + y * y + z * z), w) * RAD_TO_DEG 40 val direction = Vector3(x.toFloat(), y.toFloat(), z.toFloat()).toNormalized() 41 return RotationQuaternion(rotation, direction) 42 } 43 44 /** 45 * Creates a rotation quaternion from some Euler angles (ZYX sequence). 46 * 47 * @param eulerAngles The Euler rotation angles around each axis, in degrees. 48 */ 49 @JvmStatic fromEulernull50 fun fromEuler(eulerAngles: Vector3): RotationQuaternion { 51 return fromEuler(eulerAngles.x, eulerAngles.y, eulerAngles.z) 52 } 53 54 /** 55 * Creates a rotation quaternion from some Euler angles (ZYX sequence). 56 * 57 * @param rotationX The Euler rotation angle around the X axis, in degrees. 58 * @param rotationY The Euler rotation angle around the Y axis, in degrees. 59 * @param rotationZ The Euler rotation angle around the Z axis, in degrees. 60 */ 61 @JvmStatic fromEulernull62 fun fromEuler(rotationX: Float, rotationY: Float, rotationZ: Float): RotationQuaternion { 63 val halfDegToRad = 0.5 * DEG_TO_RAD 64 val cy = cos(rotationZ * halfDegToRad) 65 val sy = sin(rotationZ * halfDegToRad) 66 val cp = cos(rotationY * halfDegToRad) 67 val sp = sin(rotationY * halfDegToRad) 68 val cr = cos(rotationX * halfDegToRad) 69 val sr = sin(rotationX * halfDegToRad) 70 71 val w = cr * cp * cy + sr * sp * sy 72 val x = sr * cp * cy - cr * sp * sy 73 val y = cr * sp * cy + sr * cp * sy 74 val z = cr * cp * sy - sr * sp * cy 75 76 return fromQuaternion(w, x, y, z) 77 } 78 } 79 80 private val w: Double 81 private val x: Double 82 private val y: Double 83 private val z: Double 84 val direction: Vector3 85 val angle: Double 86 87 /** 88 * Creates a unit quaternion representing a rotation. 89 * 90 * @param angle The angle of rotation around [direction] vector, in degrees. The rotation is 91 * counterclockwise (if the [direction] vector is pointing at the point of sight). 92 * @param direction The angle of rotation, in degrees. 93 */ 94 constructor(angle: Double, direction: Vector3) { 95 this.direction = direction.toNormalized() 96 this.angle = angle 97 98 val angleRad = angle * DEG_TO_RAD 99 val sinAngle = sin(angleRad) 100 w = cos(angleRad) 101 x = sinAngle * this.direction.x 102 y = sinAngle * this.direction.y 103 z = sinAngle * this.direction.z 104 } 105 106 /** 107 * Creates a identity rotation quaternion, with the direction pointing to the X axis. 108 */ 109 constructor() : this(0.0, Vector3.X_AXIS) 110 111 /** 112 * Creates a rotation quaternion from another rotation quaternion. 113 */ 114 constructor(rotationQuaternion: RotationQuaternion) : this( 115 rotationQuaternion.angle, 116 rotationQuaternion.direction 117 ) 118 119 /** 120 * Returns a [Vector3] representing a quaternion as some Rotation Euler Angles (ZYX sequence). 121 * 122 * @return A [Vector3] containing the Euler rotation angle around each axis, in degrees. 123 */ toEulerAnglesnull124 fun toEulerAngles(): Vector3 { 125 val angleX = atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y)) 126 127 val tmp = 2 * (w * y - z * x) 128 val angleY = if (abs(tmp) >= 1) { 129 tmp.sign * PI / 2.0 130 } else { 131 asin(tmp) 132 } 133 134 val angleZ = atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z)) 135 136 return Vector3( 137 (angleX * RAD_TO_DEG).toFloat(), 138 (angleY * RAD_TO_DEG).toFloat(), 139 (angleZ * RAD_TO_DEG).toFloat() 140 ) 141 } 142 143 /** 144 * Inverts the rotation quaternion (q^-1). 145 * 146 * @return The new inverted quaternion. 147 */ inversenull148 fun inverse(): RotationQuaternion { 149 return fromQuaternion(w, -x, -y, -z) 150 } 151 152 /** 153 * Applies the current rotation quaternion to a [Vector3]. 154 * 155 * @param vector the [Vector3] that will be rotated. 156 * 157 * @return the rotated [vector]. 158 */ applyRotationTonull159 fun applyRotationTo(vector: Vector3): Vector3 { 160 return (this * (fromQuaternion( 161 0.0, 162 vector.x.toDouble(), 163 vector.y.toDouble(), 164 vector.z.toDouble() 165 ) * this.inverse())).direction * vector.length() 166 } 167 timesnull168 operator fun times(q: RotationQuaternion): RotationQuaternion { 169 return fromQuaternion( 170 w * q.w - x * q.x - y * q.y - z * q.z, 171 w * q.x + x * q.w + y * q.z - z * q.y, 172 w * q.y - x * q.z + y * q.w + z * q.x, 173 w * q.z + x * q.y - y * q.x + z * q.w 174 ) 175 } 176 equalsnull177 override fun equals(other: Any?): Boolean { 178 if (this === other) return true 179 if (javaClass != other?.javaClass) return false 180 181 other as RotationQuaternion 182 183 if (direction != other.direction) return false 184 if (angle != other.angle) return false 185 186 return true 187 } 188 hashCodenull189 override fun hashCode(): Int { 190 var result = direction.hashCode() 191 result = 31 * result + angle.hashCode() 192 return result 193 } 194 toStringnull195 override fun toString(): String { 196 return "Angle: ${angle}º, Direction: $direction" 197 } 198 }