Tanoda
VectorHand.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.Splines;
12
14
19 public interface IInterpolable<T> {
20 T CopyFrom(T toCopy);
21 bool FillLerped(T from, T to, float t);
22 bool FillSplined(T a, T b, T c, T d, float t);
23 }
24
35 [Serializable]
36 public class VectorHand : IInterpolable<VectorHand> {
37
38 #region Data
39
40 public const int NUM_JOINT_POSITIONS = 25;
41
42 public bool isLeft;
43 public Vector3 palmPos;
44 public Quaternion palmRot;
45 public Pose palmPose { get { return new Pose(palmPos, palmRot); }}
46
47 [SerializeField]
48 private Vector3[] _backingJointPositions;
49 public Vector3[] jointPositions {
50 get {
51 if (_backingJointPositions == null ||
52 _backingJointPositions.Length != NUM_JOINT_POSITIONS)
53 {
54 _backingJointPositions = new Vector3[NUM_JOINT_POSITIONS];
55 }
56 return _backingJointPositions;
57 }
58 }
59
60 #endregion
61
62 public VectorHand() { }
63
70 public VectorHand(Hand hand) : this() {
71 Encode(hand);
72 }
73
78 if (h != null) {
80 for(int i = 0; i < jointPositions.Length; i++)
81 _backingJointPositions[i] = h.jointPositions[i];
82 }
83 return this;
84 }
85
86 #region Hand Encoding
87
88 public void Encode(Hand fromHand) {
89 isLeft = fromHand.IsLeft;
90 palmPos = fromHand.PalmPosition.ToVector3();
91 palmRot = fromHand.Rotation.ToQuaternion();
92
93 int boneIdx = 0;
94 for (int i = 0; i < 5; i++) {
95 Vector3 baseMetacarpal = ToLocal(
96 fromHand.Fingers[i].bones[0].PrevJoint.ToVector3(), palmPos, palmRot);
97 jointPositions[boneIdx++] = baseMetacarpal;
98 for (int j = 0; j < 4; j++) {
99 Vector3 joint = ToLocal(
100 fromHand.Fingers[i].bones[j].NextJoint.ToVector3(), palmPos, palmRot);
101 jointPositions[boneIdx++] = joint;
102 }
103 }
104 }
105
106 // TODO: DELETEME
107 public static Vector3 tweakWristPosition = new Vector3(0f, -0.015f, -0.065f);
108
109 public void Decode(Hand intoHand) {
110 int boneIdx = 0;
111 Vector3 prevJoint = Vector3.zero;
112 Vector3 nextJoint = Vector3.zero;
113 Quaternion boneRot = Quaternion.identity;
114
115 // Fill fingers.
116 for (int fingerIdx = 0; fingerIdx < 5; fingerIdx++) {
117 for (int jointIdx = 0; jointIdx < 4; jointIdx++) {
118 boneIdx = fingerIdx * 4 + jointIdx;
119 prevJoint = jointPositions[fingerIdx * 5 + jointIdx];
120 nextJoint = jointPositions[fingerIdx * 5 + jointIdx + 1];
121
122 if ((nextJoint - prevJoint).normalized == Vector3.zero) {
123 // Thumb "metacarpal" slot is an identity bone.
124 boneRot = Quaternion.identity;
125 }
126 else {
127 boneRot = Quaternion.LookRotation(
128 (nextJoint - prevJoint).normalized,
129 Vector3.Cross((nextJoint - prevJoint).normalized,
130 (fingerIdx == 0 ?
131 (isLeft ? -Vector3.up : Vector3.up)
132 : Vector3.right)));
133 }
134
135 // Convert to world space from palm space.
136 nextJoint = ToWorld(nextJoint, palmPos, palmRot);
137 prevJoint = ToWorld(prevJoint, palmPos, palmRot);
138 boneRot = palmRot * boneRot;
139
140 intoHand.GetBone(boneIdx).Fill(
141 prevJoint: prevJoint.ToVector(),
142 nextJoint: nextJoint.ToVector(),
143 center: ((nextJoint + prevJoint) / 2f).ToVector(),
144 direction: (palmRot * Vector3.forward).ToVector(),
145 length: (prevJoint - nextJoint).magnitude,
146 width: 0.01f,
147 type: (Bone.BoneType)jointIdx,
148 rotation: boneRot.ToLeapQuaternion());
149 }
150 intoHand.Fingers[fingerIdx].Fill(
151 frameId: -1,
152 handId: (isLeft ? 0 : 1),
153 fingerId: fingerIdx,
154 timeVisible: 10f,// Time.time, <- This is unused and main thread only
155 tipPosition: nextJoint.ToVector(),
156 direction: (boneRot * Vector3.forward).ToVector(),
157 width: 1f,
158 length: 1f,
159 isExtended: true,
160 type: (Finger.FingerType)fingerIdx);
161 }
162
163 // Fill arm data.
164 intoHand.Arm.Fill(ToWorld(new Vector3(0f, 0f, -0.3f), palmPos, palmRot).ToVector(),
165 ToWorld(new Vector3(0f, 0f, -0.055f), palmPos, palmRot).ToVector(),
166 ToWorld(new Vector3(0f, 0f, -0.125f), palmPos, palmRot).ToVector(),
167 Vector.Zero,
168 0.3f,
169 0.05f,
170 (palmRot).ToLeapQuaternion());
171
172 // Finally, fill hand data.
173 var palmPose = new Pose(palmPos, palmRot);
174 // var wristPos = ToWorld(new Vector3(0f, -0.015f, -0.065f), palmPos, palmRot);
175 var wristPos = (palmPose * tweakWristPosition).position;
176 intoHand.Fill(
177 frameID: -1,
178 id: (isLeft ? 0 : 1),
179 confidence: 1f,
180 grabStrength: 0.5f,
181 grabAngle: 100f,
182 pinchStrength: 0.5f,
183 pinchDistance: 50f,
184 palmWidth: 0.085f,
185 isLeft: isLeft,
186 timeVisible: 1f,
187 fingers: null /* already uploaded finger data */,
188 palmPosition: palmPos.ToVector(),
189 stabilizedPalmPosition: palmPos.ToVector(),
190 palmVelocity: Vector3.zero.ToVector(),
191 palmNormal: (palmRot * Vector3.down).ToVector(),
192 rotation: (palmRot.ToLeapQuaternion()),
193 direction: (palmRot * Vector3.forward).ToVector(),
194 wristPosition: wristPos.ToVector()
195 );
196
197 // TODO: DELETEME
198 // var sphere = new Geometry.Sphere(radius: 0.008f);
199 // var drawer = HyperMegaStuff.HyperMegaLines.drawer;
200 // drawer.color = LeapColor.cerulean;
201 // sphere.WithCenter(wristPos).DrawLines(drawer.DrawLine);
202
203 // sphere.radius = 0.007f;
204 // drawer.color = LeapColor.white;
205 // foreach (var point in jointPositions) {
206 // sphere.WithCenter((palmPose * point).position).DrawLines(drawer.DrawLine);
207 // }
208 }
209
210 #endregion
211
212 #region Byte Encoding & Decoding
213
222 public int numBytesRequired { get { return 86; } }
223 public const int NUM_BYTES = 86;
224
229 public void ReadBytes(byte[] bytes, int offset = 0) {
230 ReadBytes(bytes, ref offset);
231 }
232
237 public void ReadBytes(byte[] bytes, ref int offset) {
238 if (bytes.Length - offset < numBytesRequired) {
239 throw new System.IndexOutOfRangeException(
240 "Not enough room to read bytes for VectorHand encoding starting at offset "
241 + offset + " for array of size " + bytes + "; need at least "
242 + numBytesRequired + " bytes from the offset position.");
243 }
244
245 // Chirality.
246 isLeft = bytes[offset++] == 0x00;
247
248 // Palm position and rotation.
249 for (int i = 0; i < 3; i++) {
250 palmPos[i] = Convert.ToSingle(
251 BitConverterNonAlloc.ToInt16(bytes, ref offset))
252 / 4096f;
253 }
254 palmRot = Utils.DecompressBytesToQuat(bytes, ref offset);
255
256 // Palm-local bone joint positions.
257 for (int i = 0; i < NUM_JOINT_POSITIONS; i++) {
258 for (int j = 0; j < 3; j++) {
259 jointPositions[i][j] = VectorHandExtensions.ByteToFloat(bytes[offset++]);
260 }
261 }
262 }
263
271 public void FillBytes(byte[] bytesToFill, ref int offset) {
272 if (_backingJointPositions == null) {
273 throw new System.InvalidOperationException(
274 "Joint positions array is null. You must fill a VectorHand with data before "
275 + "you can use it to fill byte representations.");
276 }
277
278 if (bytesToFill.Length - offset < numBytesRequired) {
279 throw new System.IndexOutOfRangeException(
280 "Not enough room to fill bytes for VectorHand encoding starting at offset "
281 + offset + " for array of size " + bytesToFill.Length + "; need at least "
282 + numBytesRequired + " bytes from the offset position.");
283 }
284
285 // Chirality.
286 bytesToFill[offset++] = isLeft ? (byte)0x00 : (byte)0x01;
287
288 // Palm position, each component compressed
289 for (int i = 0; i < 3; i++) {
290 BitConverterNonAlloc.GetBytes(Convert.ToInt16(palmPos[i] * 4096f),
291 bytesToFill,
292 ref offset);
293 }
294
295 // Palm rotation.
296 Utils.CompressQuatToBytes(palmRot, bytesToFill, ref offset);
297
298 // Joint positions.
299 for (int j = 0; j < NUM_JOINT_POSITIONS; j++) {
300 for (int i = 0; i < 3; i++) {
301 bytesToFill[offset++] =
302 VectorHandExtensions.FloatToByte(jointPositions[j][i]);
303 }
304 }
305 }
306
313 public void FillBytes(byte[] bytesToFill) {
314 int unusedOffset = 0;
315 FillBytes(bytesToFill, ref unusedOffset);
316 }
317
318
323 public void ReadBytes(byte[] bytes, ref int offset, Hand intoHand) {
324 ReadBytes(bytes, ref offset);
325 Decode(intoHand);
326 }
327
333 public void FillBytes(byte[] bytes, ref int offset, Hand fromHand) {
334 if (fromHand == null) {
335 for (int i = offset; i < offset + NUM_BYTES; i++) {
336 bytes[i] = 0;
337 }
338 }
339 else {
340 Encode(fromHand);
341 FillBytes(bytes, ref offset);
342 }
343 }
344
345 [ThreadStatic]
346 private static VectorHand s_backingCachedVectorHand;
347 private static VectorHand s_cachedVectorHand {
348 get {
349 if (s_backingCachedVectorHand == null) {
350 s_backingCachedVectorHand = new VectorHand();
351 }
352 return s_backingCachedVectorHand;
353 }
354 }
355 [ThreadStatic]
356 private static Vector3[] s_backingJointsBuffer =
357 new Vector3[NUM_JOINT_POSITIONS];
358 private static Vector3[] s_jointsBuffer {
359 get {
360 if (s_backingJointsBuffer == null) {
361 s_backingJointsBuffer = new Vector3[NUM_JOINT_POSITIONS];
362 }
363 return s_backingJointsBuffer;
364 }
365 }
366
372 public static void StaticFillBytes(byte[] bytes, Hand fromHand) {
373 StaticFillBytes(bytes, 0, fromHand);
374 }
375
381 public static void StaticFillBytes(byte[] bytes, int offset, Hand fromHand) {
382 StaticFillBytes(bytes, ref offset, fromHand);
383 }
384
390 public static void StaticFillBytes(byte[] bytes, ref int offset,
391 Hand fromHand) {
392 s_cachedVectorHand._backingJointPositions = s_jointsBuffer;
393 s_cachedVectorHand.FillBytes(bytes, ref offset, fromHand);
394 }
395
396 #endregion
397
398 #region Utility
399
404 public static Vector3 ToWorld(Vector3 localPoint,
405 Vector3 localOrigin, Quaternion localRot) {
406 return (localRot * localPoint) + localOrigin;
407 }
408
413 public static Vector3 ToLocal(Vector3 worldPoint,
414 Vector3 localOrigin, Quaternion localRot) {
415 return Quaternion.Inverse(localRot) * (worldPoint - localOrigin);
416 }
417
425 public bool FillLerped(VectorHand a, VectorHand b, float t) {
426 if (a == null || b == null) return false;
427 if (a.isLeft != b.isLeft) {
428 throw new System.Exception("VectorHands must be interpolated with the " +
429 "same chirality.");
430 }
431 isLeft = a.isLeft;
432 palmPos = Vector3.LerpUnclamped(a.palmPos, b.palmPos, t);
433 palmRot = Quaternion.SlerpUnclamped(a.palmRot, b.palmRot, t);
434 for (int i = 0; i < jointPositions.Length; i++) {
435 jointPositions[i] = Vector3.LerpUnclamped(a.jointPositions[i],
436 b.jointPositions[i], t);
437 }
438 return true;
439 }
440
448 public bool FillSplined(VectorHand a, VectorHand b, VectorHand c, VectorHand d, float t) {
449 if (a == null || b == null || c == null || d == null) {
450 return false;
451 }
452 if (b.isLeft != c.isLeft) {
453 throw new System.Exception("VectorHands must be interpolated with the " +
454 "same chirality.");
455 }
456 isLeft = a.isLeft;
457 palmPos = CatmullRom.ToCHS(
458 a.palmPos, b.palmPos, c.palmPos, d.palmPos, false).PositionAt(t);
459 palmRot = Quaternion.SlerpUnclamped(b.palmRot, c.palmRot, t);
460 //Quaternion splines are not as robust
461 //CatmullRom.ToQuaternionCHS(
462 // a.palmRot, b.palmRot, c.palmRot, d.palmRot, false).RotationAt(t);
463
464 for (int i = 0; i < jointPositions.Length; i++) {
465 jointPositions[i] = CatmullRom.ToCHS(
466 a.jointPositions[i],
467 b.jointPositions[i],
468 c.jointPositions[i],
469 d.jointPositions[i], false).PositionAt(t);
470 }
471 return true;
472 }
473
474 #endregion
475
476 }
477
478 #region Utility Extension Methods
479
480 public static class VectorHandExtensions {
481
482 #region VectorHand Instance API
483
484 //public static void FillBytes(this VectorHand vectorHand, )
485
486 #endregion
487
488 #region Utilities
489
495 public static Bone GetBone(this Hand hand, int boneIdx) {
496 return hand.Fingers[boneIdx / 4].bones[boneIdx % 4];
497 }
498
502 public static byte FloatToByte(float inFloat, float movementRange = 0.3f) {
503 float clamped = Mathf.Clamp(inFloat, -movementRange / 2f, movementRange / 2f);
504 clamped += movementRange / 2f;
505 clamped /= movementRange;
506 clamped *= 255f;
507 clamped = Mathf.Floor(clamped);
508 return (byte)clamped;
509 }
510
514 public static float ByteToFloat(byte inByte, float movementRange = 0.3f) {
515 float clamped = (float)inByte;
516 clamped /= 255f;
517 clamped *= movementRange;
518 clamped -= movementRange / 2f;
519 return clamped;
520 }
521
522 #endregion
523
524 }
525
526 #endregion
527
528}
The Bone class represents a tracked bone.
Definition: Bone.cs:26
BoneType
Enumerates the type of bones.
Definition: Bone.cs:168
The Finger class represents a tracked finger.
Definition: Finger.cs:20
FingerType
Enumerates the names of the fingers.
Definition: Finger.cs:167
The Hand class reports the physical characteristics of a detected hand.
Definition: Hand.cs:26
bool IsLeft
Identifies whether this Hand is a left hand.
Definition: Hand.cs:296
Arm Arm
The arm to which this hand is attached.
Definition: Hand.cs:311
Vector PalmPosition
The center position of the palm.
Definition: Hand.cs:165
List< Finger > Fingers
The list of Finger objects detected in this frame that are attached to this hand, given in order from...
Definition: Hand.cs:159
LeapQuaternion Rotation
The rotation of the hand as a quaternion.
Definition: Hand.cs:211
A Vector-based encoding of a Leap Hand.
Definition: VectorHand.cs:36
void FillBytes(byte[] bytesToFill, ref int offset)
Fills the provided byte array with a compressed, 86-byte form of this VectorHand, starting at the pro...
Definition: VectorHand.cs:271
void ReadBytes(byte[] bytes, ref int offset)
Fills this VectorHand with data read from the provided byte array, starting at the provided offset.
Definition: VectorHand.cs:237
void FillBytes(byte[] bytes, ref int offset, Hand fromHand)
Shortcut for encoding a Leap hand into a VectorHand representation and compressing it immediately int...
Definition: VectorHand.cs:333
int numBytesRequired
The number of bytes required to encode a VectorHand into its byte representation. The byte representa...
Definition: VectorHand.cs:222
static void StaticFillBytes(byte[] bytes, Hand fromHand)
Fills bytes using a thread-safe (ThreadStatic) cached VectorHand to encode the provided Hand....
Definition: VectorHand.cs:372
static Vector3 ToLocal(Vector3 worldPoint, Vector3 localOrigin, Quaternion localRot)
Converts a world-space point to a local-space point given the local space's origin and rotation.
Definition: VectorHand.cs:413
static void StaticFillBytes(byte[] bytes, ref int offset, Hand fromHand)
Fills bytes at the argument offset using a thread-safe (ThreadStatic) cached VectorHand to encode the...
Definition: VectorHand.cs:390
void ReadBytes(byte[] bytes, ref int offset, Hand intoHand)
Shortcut for reading a VectorHand-encoded byte representation of a Leap hand and decoding it immediat...
Definition: VectorHand.cs:323
static Vector3 tweakWristPosition
Definition: VectorHand.cs:107
void Encode(Hand fromHand)
Definition: VectorHand.cs:88
VectorHand CopyFrom(VectorHand h)
Copies a VectorHand from another VectorHand
Definition: VectorHand.cs:77
void ReadBytes(byte[] bytes, int offset=0)
Fills this VectorHand with data read from the provided byte array, starting at the provided offset.
Definition: VectorHand.cs:229
static void StaticFillBytes(byte[] bytes, int offset, Hand fromHand)
Fills bytes at the argument offset using a thread-safe (ThreadStatic) cached VectorHand to encode the...
Definition: VectorHand.cs:381
VectorHand(Hand hand)
Constructs a VectorHand representation from a Leap hand. This allocates a vector array for the encode...
Definition: VectorHand.cs:70
bool FillLerped(VectorHand a, VectorHand b, float t)
Fills the ref-argument VectorHand with interpolated data between the two other VectorHands,...
Definition: VectorHand.cs:425
void Decode(Hand intoHand)
Definition: VectorHand.cs:109
static Vector3 ToWorld(Vector3 localPoint, Vector3 localOrigin, Quaternion localRot)
Converts a local-space point to a world-space point given the local space's origin and rotation.
Definition: VectorHand.cs:404
bool FillSplined(VectorHand a, VectorHand b, VectorHand c, VectorHand d, float t)
Fills the ref-argument VectorHand with interpolated data between the 4 other VectorHands,...
Definition: VectorHand.cs:448
void FillBytes(byte[] bytesToFill)
Fills the provided byte array with a compressed, 86-byte form of this VectorHand.
Definition: VectorHand.cs:313
An interface that signifies this class can interpolate via the standard techniques
Definition: VectorHand.cs:19
bool FillSplined(T a, T b, T c, T d, float t)
bool FillLerped(T from, T to, float t)
A position and rotation. You can multiply two poses; this acts like Matrix4x4 multiplication,...
Definition: Pose.cs:21
The Vector struct represents a three-component mathematical vector or point such as a direction or po...
Definition: Vector.cs:36
static readonly Vector Zero
The zero vector: (0, 0, 0)
Definition: Vector.cs:313