1 /*
2  * Copyright 2024 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 androidx.xr.runtime.math
18 
19 /**
20  * Represents an immutable rigid transformation from one coordinate space to another.
21  *
22  * @property translation the translation component of this pose.
23  * @property rotation the rotation component of this pose.
24  */
25 public class Pose
26 @JvmOverloads
27 constructor(
28     public val translation: Vector3 = Vector3(),
29     public val rotation: Quaternion = Quaternion(),
30 ) {
31 
32     /** Returns a pose that performs the opposite translation. */
33     public val inverse: Pose
34         get() = invert()
35 
36     /** The up vector in the local coordinate system. */
37     public inline val up: Vector3
38         get() = rotation * Vector3.Up
39 
40     /** The down vector in the local coordinate system. */
41     public inline val down: Vector3
42         get() = rotation * Vector3.Down
43 
44     /** The left vector in the local coordinate system. */
45     public inline val left: Vector3
46         get() = rotation * Vector3.Left
47 
48     /** The right vector in the local coordinate system. */
49     public inline val right: Vector3
50         get() = rotation * Vector3.Right
51 
52     /** The forward vector in the local coordinate system. */
53     public inline val forward: Vector3
54         get() = rotation * Vector3.Forward
55 
56     /** The backward vector in the local coordinate system. */
57     public inline val backward: Vector3
58         get() = rotation * Vector3.Backward
59 
60     /** Creates a new pose with the same values as the [other] pose. */
61     public constructor(other: Pose) : this(other.translation, other.rotation)
62 
63     /** Returns the result of composing [this] with [other]. */
composenull64     public infix fun compose(other: Pose): Pose =
65         Pose(rotation * other.translation + this.translation, rotation * other.rotation)
66 
67     /** Returns a pose that performs the opposite transformation. */
68     private fun invert(): Pose {
69         val outRotation = rotation.inverse
70         val outTranslation = -(outRotation * translation)
71 
72         return Pose(outTranslation, outRotation)
73     }
74 
75     /** Translates this pose by the given [translation]. */
translatenull76     public fun translate(translation: Vector3): Pose =
77         Pose(this.translation + translation, this.rotation)
78 
79     /** Rotates this pose by the given [rotation]. */
80     public fun rotate(rotation: Quaternion): Pose = Pose(this.translation, this.rotation * rotation)
81 
82     /**
83      * Transforms the provided point by the pose by applying both the rotation and the translation
84      * components of the pose. This is because a point represents a specific location in space. It
85      * needs to account for the position, scale and orientation of the space it is in.
86      */
87     public infix fun transformPoint(point: Vector3): Vector3 = rotation * point + translation
88 
89     /**
90      * Transforms the provided vector by the pose by only applying the rotation component of the
91      * pose. This is because a vector represents a direction and magnitude, not a specific location.
92      * It only needs to account for the scale and orientation of the space it is in since it has no
93      * position.
94      */
95     public infix fun transformVector(vector: Vector3): Vector3 = rotation * vector
96 
97     /** Returns a copy of the pose. */
98     @JvmOverloads
99     public fun copy(
100         translation: Vector3 = this.translation,
101         rotation: Quaternion = this.rotation,
102     ): Pose = Pose(translation, rotation)
103 
104     /** Returns true if this pose is equal to the [other]. */
105     override fun equals(other: Any?): Boolean {
106         if (this === other) return true
107         if (other !is Pose) return false
108 
109         return this.translation == other.translation && this.rotation == other.rotation
110     }
111 
hashCodenull112     override fun hashCode(): Int = 31 * translation.hashCode() + rotation.hashCode()
113 
114     override fun toString(): String = "Pose{\n\tTranslation=$translation\n\tRotation=$rotation\n}"
115 
116     public companion object {
117         /** Returns a new pose using the identity rotation. */
118         @JvmField public val Identity: Pose = Pose()
119 
120         /**
121          * Returns a new pose oriented to look at [target] from [eye] position with [up] as the up
122          * vector.
123          *
124          * @param eye the position from which to look at [target].
125          * @param target the target position to look at.
126          * @param up a vector indicating the general "up" direction.
127          * @return the pose oriented to look at [target] from [eye] position with [up] as the up
128          *   vector.
129          */
130         @JvmStatic
131         @JvmOverloads
132         public fun fromLookAt(eye: Vector3, target: Vector3, up: Vector3 = Vector3.Up): Pose {
133             val forward = (target - eye).toNormalized()
134             val rotation = Quaternion.fromLookTowards(forward, up)
135 
136             return Pose(eye, rotation)
137         }
138 
139         /** Returns the distance between the two poses. */
140         @JvmStatic
141         public fun distance(lhs: Pose, rhs: Pose): Float =
142             Vector3.Companion.distance(lhs.translation, rhs.translation)
143 
144         /**
145          * Returns a new pose that is linearly interpolated between [start] and [end] using the
146          * interpolation amount [ratio]. The position is [lerped][Vector3.lerp], but the rotation
147          * will be [slerped][Quaternion.slerp] if the angles are far apart.
148          *
149          * If [ratio] is outside of the range `[0, 1]`, the returned pose will be extrapolated.
150          */
151         @JvmStatic
152         public fun lerp(start: Pose, end: Pose, ratio: Float): Pose {
153             val interpolatedPosition =
154                 Vector3.Companion.lerp(start.translation, end.translation, ratio)
155 
156             val interpolatedRotation =
157                 if (start.rotation.dot(end.rotation) < 0.9995f) { // Check if angle is large
158                     Quaternion.Companion.slerp(start.rotation, end.rotation, ratio)
159                 } else {
160                     // If the angle is small, lerp can be used for efficiency.
161                     // Note: This assumes both quaternions are normalized.
162                     Quaternion.Companion.lerp(start.rotation, end.rotation, ratio).toNormalized()
163                 }
164 
165             return Pose(interpolatedPosition, interpolatedRotation)
166         }
167     }
168 }
169