Tanoda
Pose.cs
Go to the documentation of this file.
1/******************************************************************************
2 * Copyright (C) Ultraleap, Inc. 2011-2020. *
3 * *
4 * Use subject to the terms of the Apache License 2.0 available at *
5 * http://www.apache.org/licenses/LICENSE-2.0, or another agreement *
6 * between Ultraleap and you, your company or other organization. *
7 ******************************************************************************/
8
9using System;
10using UnityEngine;
11using Leap.Unity.Encoding;
12using Leap.Unity.Splines;
13
14namespace Leap.Unity {
15
20 [System.Serializable]
21 public struct Pose : IEquatable<Pose>, IInterpolable<Pose> {
22
23 public Vector3 position;
24 public Quaternion rotation;
25
26 public Pose(Vector3 position)
27 : this(position, Quaternion.identity) { }
28 public Pose(Quaternion rotation)
29 : this(Vector3.zero, rotation) { }
30 public Pose(Vector3 position, Quaternion rotation) {
31 this.position = position;
32 this.rotation = rotation;
33 }
34
35 public static readonly Pose identity = new Pose(Vector3.zero, Quaternion.identity);
36
37 public Pose inverse {
38 get {
39 Quaternion invQ = Quaternion.Inverse(this.rotation);
40 return new Pose(invQ * -this.position, invQ.ToNormalized()); // Normalize
41 }
42 }
43
48 public Matrix4x4 matrix {
49 get {
50 this.rotation = this.rotation.ToNormalized(); // Normalize
51 return Matrix4x4.TRS(this.position, this.rotation, Vector3.one);
52 }
53 }
54
59 public static Pose operator *(Pose A, Pose B) {
60 return new Pose(A.position + (A.rotation * B.position),
61 A.rotation * B.rotation);
62 }
63
70 public static Pose operator +(Pose A, Pose B) {
71 return new Pose(A.position + B.position,
72 A.rotation * B.rotation);
73 }
74
79 public static Pose operator *(Pose pose, Vector3 localPosition) {
80 return new Pose(pose.position + pose.rotation * localPosition,
81 pose.rotation);
82 }
83
84 public static Pose operator *(Pose pose, Quaternion localRotation) {
85 return pose * new Pose(Vector3.zero, localRotation);
86 }
87
88 public static Pose operator *(Quaternion parentRotation, Pose localPose) {
89 return new Pose(Vector3.zero, parentRotation) * localPose;
90 }
91
93 public static Pose operator *(Matrix4x4 matrix, Pose localPose) {
94 return new Pose(matrix.MultiplyPoint3x4(localPose.position),
95 matrix.rotation * localPose.rotation);
96 }
97
98 public bool ApproxEquals(Pose other) {
99 return position.ApproxEquals(other.position) && rotation.ApproxEquals(other.rotation);
100 }
101
107 public static Pose Lerp(Pose a, Pose b, float t) {
108 if (t >= 1f) return b;
109 if (t <= 0f) return a;
110 return new Pose(Vector3.Lerp(a.position, b.position, t),
111 Quaternion.Lerp(Quaternion.Slerp(a.rotation, b.rotation, t), Quaternion.identity, 0f));
112 }
113
118 public static Pose LerpUnclamped(Pose a, Pose b, float t) {
119 return new Pose(Vector3.LerpUnclamped(a.position, b.position, t),
120 Quaternion.SlerpUnclamped(a.rotation, b.rotation, t));
121 }
122
127 public static Pose LerpUnclampedTimed(Pose a, float aTime,
128 Pose b, float bTime,
129 float extrapolateTime) {
130 return LerpUnclamped(a, b, extrapolateTime.MapUnclamped(aTime, bTime, 0f, 1f));
131 }
132
133 public override string ToString() {
134 return "[Pose | Position: " + this.position.ToString()
135 + ", Rotation: " + this.rotation.ToString() + "]";
136 }
137
138 public string ToString(string format) {
139 return "[Pose | Position: " + this.position.ToString(format)
140 + ", Rotation: " + this.rotation.ToString(format) + "]";
141 }
142
143 public override bool Equals(object obj) {
144 if (!(obj is Pose)) return false;
145 else return this.Equals((Pose)obj);
146 }
147 public bool Equals(Pose other) {
148 return other.position == this.position &&
149 other.rotation == this.rotation;
150 }
151
152 public override int GetHashCode() {
153 return new Hash() {
154 position,
156 };
157 }
158
159 public static bool operator ==(Pose a, Pose b) {
160 return a.Equals(b);
161 }
162
163 public static bool operator !=(Pose a, Pose b) {
164 return !(a.Equals(b));
165 }
166
167
168 // IInterpolable Implementation
169 public Pose CopyFrom(Pose h) {
170 position = h.position;
171 rotation = h.rotation;
172 return this;
173 }
174 public bool FillLerped(Pose a, Pose b, float t) {
175 this = LerpUnclamped(a, b, t);
176 return true;
177 }
178 public bool FillSplined(Pose a, Pose b, Pose c, Pose d, float t) {
179 position = CatmullRom.ToCHS(a.position, b.position, c.position, d.position,
180 centripetal: false).PositionAt(t);
181 rotation = Quaternion.SlerpUnclamped(b.rotation, c.rotation, t);
182 return true;
183 }
184
185 }
186
187 public static class PoseExtensions {
188
192 public static Pose ToLocalPose(this Transform t) {
193 return new Pose(t.localPosition, t.localRotation);
194 }
195
199 public static Pose ToPose(this Transform t) {
200 return new Pose(t.position, t.rotation);
201 }
202
206 public static Pose GetPose(this Transform t) {
207 return new Pose(t.position, t.rotation);
208 }
209
213 public static Pose ToWorldPose(this Transform t) {
214 return t.ToPose();
215 }
216
221 public static void SetLocalPose(this Transform t, Pose localPose) {
222 t.localPosition = localPose.position;
223 t.localRotation = localPose.rotation;
224 }
229 public static void SetPose(this Transform t, Pose worldPose) {
230 t.position = worldPose.position;
231 t.rotation = worldPose.rotation;
232 }
233
238 public static void SetWorldPose(this Transform t, Pose worldPose) {
239 t.SetPose(worldPose);
240 }
241
245 public static Pose GetPose(this Matrix4x4 m) {
246 return new Pose(position: m.MultiplyPoint3x4(Vector3.zero),
247 rotation: m.GetQuaternion());
248 // return new Pose(m.GetColumn(3),
249 // m.GetColumn(2) == m.GetColumn(1) ? Quaternion.identity
250 // : Quaternion.LookRotation(
251 // m.GetColumn(2),
252 // m.GetColumn(1)));
253 }
254
259 public static Pose WithRotation(this Pose pose, Quaternion newRotation) {
260 return new Pose(pose.position, newRotation);
261 }
262
267 public static Pose WithPosition(this Pose pose, Vector3 newPosition) {
268 return new Pose(newPosition, pose.rotation);
269 }
270
271 public const float EPSILON = 0.0001f;
272
273 public static bool ApproxEquals(this Vector3 v0, Vector3 v1) {
274 return (v0 - v1).magnitude < EPSILON;
275 }
276
277 public static bool ApproxEquals(this Quaternion q0, Quaternion q1) {
278 return (q0.ToAngleAxisVector() - q1.ToAngleAxisVector()).magnitude < EPSILON;
279 }
280
281 }
282
283}
An interface that signifies this class can interpolate via the standard techniques
Definition: VectorHand.cs:19
A position and rotation. You can multiply two poses; this acts like Matrix4x4 multiplication,...
Definition: Pose.cs:21
static bool operator==(Pose a, Pose b)
Definition: Pose.cs:159
Pose inverse
Definition: Pose.cs:37
bool FillSplined(Pose a, Pose b, Pose c, Pose d, float t)
Definition: Pose.cs:178
static Pose operator+(Pose A, Pose B)
Returns the accumulation of the two poses: The positions summed, and with rotation A....
Definition: Pose.cs:70
Pose(Quaternion rotation)
Definition: Pose.cs:28
static Pose LerpUnclampedTimed(Pose a, float aTime, Pose b, float bTime, float extrapolateTime)
As LerpUnclamped, but extrapolates using time values for a and b, and a target time at which to deter...
Definition: Pose.cs:127
Pose CopyFrom(Pose h)
Definition: Pose.cs:169
bool FillLerped(Pose a, Pose b, float t)
Definition: Pose.cs:174
override string ToString()
Definition: Pose.cs:133
static Pose LerpUnclamped(Pose a, Pose b, float t)
As Lerp, but doesn't clamp t between 0 and 1. Values above one extrapolate forwards beyond b,...
Definition: Pose.cs:118
Pose(Vector3 position, Quaternion rotation)
Definition: Pose.cs:30
Quaternion rotation
Definition: Pose.cs:24
Matrix4x4 matrix
Returns a Matrix4x4 corresponding to this pose's translation and rotation, with unit scale.
Definition: Pose.cs:48
Pose(Vector3 position)
Definition: Pose.cs:26
static Pose operator*(Pose A, Pose B)
Returns Pose B transformed by Pose A, like a transform hierarchy with A as the parent of B.
Definition: Pose.cs:59
override int GetHashCode()
Definition: Pose.cs:152
string ToString(string format)
Definition: Pose.cs:138
bool Equals(Pose other)
Definition: Pose.cs:147
bool ApproxEquals(Pose other)
Definition: Pose.cs:98
static Pose Lerp(Pose a, Pose b, float t)
Returns a pose interpolated (Lerp for position, Slerp, NOT Lerp for rotation) between a and b by t fr...
Definition: Pose.cs:107
override bool Equals(object obj)
Definition: Pose.cs:143
static readonly Pose identity
Definition: Pose.cs:35
Vector3 position
Definition: Pose.cs:23
static bool operator!=(Pose a, Pose b)
Definition: Pose.cs:163