1
0

SteamVR_Skeleton_Poser.cs 22 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564
  1. //======= Copyright (c) Valve Corporation, All rights reserved. ===============
  2. using System;
  3. using System.Collections;
  4. using UnityEngine;
  5. using Valve.VR;
  6. using System.Collections.Generic;
  7. using System.Linq;
  8. namespace Valve.VR
  9. {
  10. public class SteamVR_Skeleton_Poser : MonoBehaviour
  11. {
  12. #region Editor Storage
  13. public bool poseEditorExpanded = true;
  14. public bool blendEditorExpanded = true;
  15. public string[] poseNames;
  16. #endregion
  17. public GameObject overridePreviewLeftHandPrefab;
  18. public GameObject overridePreviewRightHandPrefab;
  19. public SteamVR_Skeleton_Pose skeletonMainPose;
  20. public List<SteamVR_Skeleton_Pose> skeletonAdditionalPoses = new List<SteamVR_Skeleton_Pose>();
  21. [SerializeField]
  22. protected bool showLeftPreview = false;
  23. [SerializeField]
  24. protected bool showRightPreview = true; //show the right hand by default
  25. [SerializeField]
  26. protected GameObject previewLeftInstance;
  27. [SerializeField]
  28. protected GameObject previewRightInstance;
  29. [SerializeField]
  30. protected int previewPoseSelection = 0;
  31. public int blendPoseCount { get { return blendPoses.Length; } }
  32. public List<PoseBlendingBehaviour> blendingBehaviours = new List<PoseBlendingBehaviour>();
  33. public SteamVR_Skeleton_PoseSnapshot blendedSnapshotL;
  34. public SteamVR_Skeleton_PoseSnapshot blendedSnapshotR;
  35. private SkeletonBlendablePose[] blendPoses;
  36. private int boneCount;
  37. private bool poseUpdatedThisFrame;
  38. public float scale;
  39. protected void Awake()
  40. {
  41. if (previewLeftInstance != null)
  42. DestroyImmediate(previewLeftInstance);
  43. if (previewRightInstance != null)
  44. DestroyImmediate(previewRightInstance);
  45. blendPoses = new SkeletonBlendablePose[skeletonAdditionalPoses.Count + 1];
  46. for (int i = 0; i < blendPoseCount; i++)
  47. {
  48. blendPoses[i] = new SkeletonBlendablePose(GetPoseByIndex(i));
  49. blendPoses[i].PoseToSnapshots();
  50. }
  51. boneCount = skeletonMainPose.leftHand.bonePositions.Length;
  52. // NOTE: Is there a better way to get the bone count? idk
  53. blendedSnapshotL = new SteamVR_Skeleton_PoseSnapshot(boneCount, SteamVR_Input_Sources.LeftHand);
  54. blendedSnapshotR = new SteamVR_Skeleton_PoseSnapshot(boneCount, SteamVR_Input_Sources.RightHand);
  55. }
  56. /// <summary>
  57. /// Set the blending value of a blendingBehaviour. Works best on Manual type behaviours.
  58. /// </summary>
  59. public void SetBlendingBehaviourValue(string behaviourName, float value)
  60. {
  61. PoseBlendingBehaviour behaviour = FindBlendingBehaviour(behaviourName);
  62. if (behaviour != null)
  63. {
  64. behaviour.value = value;
  65. if (behaviour.type != PoseBlendingBehaviour.BlenderTypes.Manual)
  66. {
  67. Debug.LogWarning("[SteamVR] Blending Behaviour: " + behaviourName + " is not a manual behaviour. Its value will likely be overriden.", this);
  68. }
  69. }
  70. }
  71. /// <summary>
  72. /// Get the blending value of a blendingBehaviour.
  73. /// </summary>
  74. public float GetBlendingBehaviourValue(string behaviourName)
  75. {
  76. PoseBlendingBehaviour behaviour = FindBlendingBehaviour(behaviourName);
  77. if (behaviour != null)
  78. {
  79. return behaviour.value;
  80. }
  81. return 0;
  82. }
  83. /// <summary>
  84. /// Enable or disable a blending behaviour.
  85. /// </summary>
  86. public void SetBlendingBehaviourEnabled(string behaviourName, bool value)
  87. {
  88. PoseBlendingBehaviour behaviour = FindBlendingBehaviour(behaviourName);
  89. if (behaviour != null)
  90. {
  91. behaviour.enabled = value;
  92. }
  93. }
  94. /// <summary>
  95. /// Check if a blending behaviour is enabled.
  96. /// </summary>
  97. /// <param name="behaviourName"></param>
  98. /// <returns></returns>
  99. public bool GetBlendingBehaviourEnabled(string behaviourName)
  100. {
  101. PoseBlendingBehaviour behaviour = FindBlendingBehaviour(behaviourName);
  102. if (behaviour != null)
  103. {
  104. return behaviour.enabled;
  105. }
  106. return false;
  107. }
  108. /// <summary>
  109. /// Get a blending behaviour by name.
  110. /// </summary>
  111. public PoseBlendingBehaviour GetBlendingBehaviour(string behaviourName)
  112. {
  113. return FindBlendingBehaviour(behaviourName);
  114. }
  115. protected PoseBlendingBehaviour FindBlendingBehaviour(string behaviourName, bool throwErrors = true)
  116. {
  117. PoseBlendingBehaviour behaviour = blendingBehaviours.Find(b => b.name == behaviourName);
  118. if (behaviour == null)
  119. {
  120. if (throwErrors)
  121. Debug.LogError("[SteamVR] Blending Behaviour: " + behaviourName + " not found on Skeleton Poser: " + gameObject.name, this);
  122. return null;
  123. }
  124. return behaviour;
  125. }
  126. public SteamVR_Skeleton_Pose GetPoseByIndex(int index)
  127. {
  128. if (index == 0) { return skeletonMainPose; }
  129. else { return skeletonAdditionalPoses[index - 1]; }
  130. }
  131. private SteamVR_Skeleton_PoseSnapshot GetHandSnapshot(SteamVR_Input_Sources inputSource)
  132. {
  133. if (inputSource == SteamVR_Input_Sources.LeftHand)
  134. return blendedSnapshotL;
  135. else
  136. return blendedSnapshotR;
  137. }
  138. /// <summary>
  139. /// Retrieve the final animated pose, to be applied to a hand skeleton
  140. /// </summary>
  141. /// <param name="forAction">The skeleton action you want to blend between</param>
  142. /// <param name="handType">If this is for the left or right hand</param>
  143. public SteamVR_Skeleton_PoseSnapshot GetBlendedPose(SteamVR_Action_Skeleton skeletonAction, SteamVR_Input_Sources handType)
  144. {
  145. UpdatePose(skeletonAction, handType);
  146. return GetHandSnapshot(handType);
  147. }
  148. /// <summary>
  149. /// Retrieve the final animated pose, to be applied to a hand skeleton
  150. /// </summary>
  151. /// <param name="skeletonBehaviour">The skeleton behaviour you want to get the action/input source from to blend between</param>
  152. public SteamVR_Skeleton_PoseSnapshot GetBlendedPose(SteamVR_Behaviour_Skeleton skeletonBehaviour)
  153. {
  154. return GetBlendedPose(skeletonBehaviour.skeletonAction, skeletonBehaviour.inputSource);
  155. }
  156. /// <summary>
  157. /// Updates all pose animation and blending. Can be called from different places without performance concerns, as it will only let itself run once per frame.
  158. /// </summary>
  159. public void UpdatePose(SteamVR_Action_Skeleton skeletonAction, SteamVR_Input_Sources inputSource)
  160. {
  161. // only allow this function to run once per frame
  162. if (poseUpdatedThisFrame) return;
  163. poseUpdatedThisFrame = true;
  164. if (skeletonAction.activeBinding)
  165. {
  166. // always do additive animation on main pose
  167. blendPoses[0].UpdateAdditiveAnimation(skeletonAction, inputSource);
  168. }
  169. //copy from main pose as a base
  170. SteamVR_Skeleton_PoseSnapshot snap = GetHandSnapshot(inputSource);
  171. snap.CopyFrom(blendPoses[0].GetHandSnapshot(inputSource));
  172. ApplyBlenderBehaviours(skeletonAction, inputSource, snap);
  173. if (inputSource == SteamVR_Input_Sources.RightHand)
  174. blendedSnapshotR = snap;
  175. if (inputSource == SteamVR_Input_Sources.LeftHand)
  176. blendedSnapshotL = snap;
  177. }
  178. protected void ApplyBlenderBehaviours(SteamVR_Action_Skeleton skeletonAction, SteamVR_Input_Sources inputSource, SteamVR_Skeleton_PoseSnapshot snapshot)
  179. {
  180. // apply blending for each behaviour
  181. for (int behaviourIndex = 0; behaviourIndex < blendingBehaviours.Count; behaviourIndex++)
  182. {
  183. blendingBehaviours[behaviourIndex].Update(Time.deltaTime, inputSource);
  184. // if disabled or very low influence, skip for perf
  185. if (blendingBehaviours[behaviourIndex].enabled && blendingBehaviours[behaviourIndex].influence * blendingBehaviours[behaviourIndex].value > 0.01f)
  186. {
  187. if (blendingBehaviours[behaviourIndex].pose != 0 && skeletonAction.activeBinding)
  188. {
  189. // update additive animation only as needed
  190. blendPoses[blendingBehaviours[behaviourIndex].pose].UpdateAdditiveAnimation(skeletonAction, inputSource);
  191. }
  192. blendingBehaviours[behaviourIndex].ApplyBlending(snapshot, blendPoses, inputSource);
  193. }
  194. }
  195. }
  196. protected void LateUpdate()
  197. {
  198. // let the pose be updated again the next frame
  199. poseUpdatedThisFrame = false;
  200. }
  201. /// <summary>Weighted average of n vector3s</summary>
  202. protected Vector3 BlendVectors(Vector3[] vectors, float[] weights)
  203. {
  204. Vector3 blendedVector = Vector3.zero;
  205. for (int i = 0; i < vectors.Length; i++)
  206. {
  207. blendedVector += vectors[i] * weights[i];
  208. }
  209. return blendedVector;
  210. }
  211. /// <summary>Weighted average of n quaternions</summary>
  212. protected Quaternion BlendQuaternions(Quaternion[] quaternions, float[] weights)
  213. {
  214. Quaternion outquat = Quaternion.identity;
  215. for (int i = 0; i < quaternions.Length; i++)
  216. {
  217. outquat *= Quaternion.Slerp(Quaternion.identity, quaternions[i], weights[i]);
  218. }
  219. return outquat;
  220. }
  221. /// <summary>
  222. /// A SkeletonBlendablePose holds a reference to a Skeleton_Pose scriptableObject, and also contains some helper functions.
  223. /// Also handles pose-specific animation like additive finger motion.
  224. /// </summary>
  225. public class SkeletonBlendablePose
  226. {
  227. public SteamVR_Skeleton_Pose pose;
  228. public SteamVR_Skeleton_PoseSnapshot snapshotR;
  229. public SteamVR_Skeleton_PoseSnapshot snapshotL;
  230. /// <summary>
  231. /// Get the snapshot of this pose with effects such as additive finger animation applied.
  232. /// </summary>
  233. public SteamVR_Skeleton_PoseSnapshot GetHandSnapshot(SteamVR_Input_Sources inputSource)
  234. {
  235. if (inputSource == SteamVR_Input_Sources.LeftHand)
  236. {
  237. return snapshotL;
  238. }
  239. else
  240. {
  241. return snapshotR;
  242. }
  243. }
  244. public void UpdateAdditiveAnimation(SteamVR_Action_Skeleton skeletonAction, SteamVR_Input_Sources inputSource)
  245. {
  246. if (skeletonAction.GetSkeletalTrackingLevel() == EVRSkeletalTrackingLevel.VRSkeletalTracking_Estimated)
  247. {
  248. //do not apply additive animation on low fidelity controllers, eg. Vive Wands and Touch
  249. return;
  250. }
  251. SteamVR_Skeleton_PoseSnapshot snapshot = GetHandSnapshot(inputSource);
  252. SteamVR_Skeleton_Pose_Hand poseHand = pose.GetHand(inputSource);
  253. for (int boneIndex = 0; boneIndex < snapshotL.bonePositions.Length; boneIndex++)
  254. {
  255. int fingerIndex = SteamVR_Skeleton_JointIndexes.GetFingerForBone(boneIndex);
  256. SteamVR_Skeleton_FingerExtensionTypes extensionType = poseHand.GetMovementTypeForBone(boneIndex);
  257. if (extensionType == SteamVR_Skeleton_FingerExtensionTypes.Free)
  258. {
  259. snapshot.bonePositions[boneIndex] = skeletonAction.bonePositions[boneIndex];
  260. snapshot.boneRotations[boneIndex] = skeletonAction.boneRotations[boneIndex];
  261. }
  262. if (extensionType == SteamVR_Skeleton_FingerExtensionTypes.Extend)
  263. {
  264. // lerp to open pose by fingercurl
  265. snapshot.bonePositions[boneIndex] = Vector3.Lerp(poseHand.bonePositions[boneIndex], skeletonAction.bonePositions[boneIndex], 1 - skeletonAction.fingerCurls[fingerIndex]);
  266. snapshot.boneRotations[boneIndex] = Quaternion.Lerp(poseHand.boneRotations[boneIndex], skeletonAction.boneRotations[boneIndex], 1 - skeletonAction.fingerCurls[fingerIndex]);
  267. }
  268. if (extensionType == SteamVR_Skeleton_FingerExtensionTypes.Contract)
  269. {
  270. // lerp to closed pose by fingercurl
  271. snapshot.bonePositions[boneIndex] = Vector3.Lerp(poseHand.bonePositions[boneIndex], skeletonAction.bonePositions[boneIndex], skeletonAction.fingerCurls[fingerIndex]);
  272. snapshot.boneRotations[boneIndex] = Quaternion.Lerp(poseHand.boneRotations[boneIndex], skeletonAction.boneRotations[boneIndex], skeletonAction.fingerCurls[fingerIndex]);
  273. }
  274. }
  275. }
  276. /// <summary>
  277. /// Init based on an existing Skeleton_Pose
  278. /// </summary>
  279. public SkeletonBlendablePose(SteamVR_Skeleton_Pose p)
  280. {
  281. pose = p;
  282. snapshotR = new SteamVR_Skeleton_PoseSnapshot(p.rightHand.bonePositions.Length, SteamVR_Input_Sources.RightHand);
  283. snapshotL = new SteamVR_Skeleton_PoseSnapshot(p.leftHand.bonePositions.Length, SteamVR_Input_Sources.LeftHand);
  284. }
  285. /// <summary>
  286. /// Copy the base pose into the snapshots.
  287. /// </summary>
  288. public void PoseToSnapshots()
  289. {
  290. snapshotR.position = pose.rightHand.position;
  291. snapshotR.rotation = pose.rightHand.rotation;
  292. pose.rightHand.bonePositions.CopyTo(snapshotR.bonePositions, 0);
  293. pose.rightHand.boneRotations.CopyTo(snapshotR.boneRotations, 0);
  294. snapshotL.position = pose.leftHand.position;
  295. snapshotL.rotation = pose.leftHand.rotation;
  296. pose.leftHand.bonePositions.CopyTo(snapshotL.bonePositions, 0);
  297. pose.leftHand.boneRotations.CopyTo(snapshotL.boneRotations, 0);
  298. }
  299. public SkeletonBlendablePose() { }
  300. }
  301. /// <summary>
  302. /// A filter applied to the base pose. Blends to a secondary pose by a certain weight. Can be masked per-finger
  303. /// </summary>
  304. [System.Serializable]
  305. public class PoseBlendingBehaviour
  306. {
  307. public string name;
  308. public bool enabled = true;
  309. public float influence = 1;
  310. public int pose = 1;
  311. public float value = 0;
  312. public SteamVR_Action_Single action_single;
  313. public SteamVR_Action_Boolean action_bool;
  314. public float smoothingSpeed = 0;
  315. public BlenderTypes type;
  316. public bool useMask;
  317. public SteamVR_Skeleton_HandMask mask = new SteamVR_Skeleton_HandMask();
  318. public bool previewEnabled;
  319. /// <summary>
  320. /// Performs smoothing based on deltaTime parameter.
  321. /// </summary>
  322. public void Update(float deltaTime, SteamVR_Input_Sources inputSource)
  323. {
  324. if (type == BlenderTypes.AnalogAction)
  325. {
  326. if (smoothingSpeed == 0)
  327. value = action_single.GetAxis(inputSource);
  328. else
  329. value = Mathf.Lerp(value, action_single.GetAxis(inputSource), deltaTime * smoothingSpeed);
  330. }
  331. if (type == BlenderTypes.BooleanAction)
  332. {
  333. if (smoothingSpeed == 0)
  334. value = action_bool.GetState(inputSource) ? 1 : 0;
  335. else
  336. value = Mathf.Lerp(value, action_bool.GetState(inputSource) ? 1 : 0, deltaTime * smoothingSpeed);
  337. }
  338. }
  339. /// <summary>
  340. /// Apply blending to this behaviour's pose to an existing snapshot.
  341. /// </summary>
  342. /// <param name="snapshot">Snapshot to modify</param>
  343. /// <param name="blendPoses">List of blend poses to get the target pose</param>
  344. /// <param name="inputSource">Which hand to receive input from</param>
  345. public void ApplyBlending(SteamVR_Skeleton_PoseSnapshot snapshot, SkeletonBlendablePose[] blendPoses, SteamVR_Input_Sources inputSource)
  346. {
  347. SteamVR_Skeleton_PoseSnapshot targetSnapshot = blendPoses[pose].GetHandSnapshot(inputSource);
  348. if (mask.GetFinger(0) || useMask == false)
  349. {
  350. snapshot.position = Vector3.Lerp(snapshot.position, targetSnapshot.position, influence * value);
  351. snapshot.rotation = Quaternion.Slerp(snapshot.rotation, targetSnapshot.rotation, influence * value);
  352. }
  353. for (int boneIndex = 0; boneIndex < snapshot.bonePositions.Length; boneIndex++)
  354. {
  355. // verify the current finger is enabled in the mask, or if no mask is used.
  356. if (mask.GetFinger(SteamVR_Skeleton_JointIndexes.GetFingerForBone(boneIndex) + 1) || useMask == false)
  357. {
  358. snapshot.bonePositions[boneIndex] = Vector3.Lerp(snapshot.bonePositions[boneIndex], targetSnapshot.bonePositions[boneIndex], influence * value);
  359. snapshot.boneRotations[boneIndex] = Quaternion.Slerp(snapshot.boneRotations[boneIndex], targetSnapshot.boneRotations[boneIndex], influence * value);
  360. }
  361. }
  362. }
  363. public PoseBlendingBehaviour()
  364. {
  365. enabled = true;
  366. influence = 1;
  367. }
  368. public enum BlenderTypes
  369. {
  370. Manual, AnalogAction, BooleanAction
  371. }
  372. }
  373. //this is broken
  374. public Vector3 GetTargetHandPosition(SteamVR_Behaviour_Skeleton hand, Transform origin)
  375. {
  376. Vector3 oldOrigin = origin.position;
  377. Quaternion oldHand = hand.transform.rotation;
  378. hand.transform.rotation = GetBlendedPose(hand).rotation;
  379. origin.position = hand.transform.TransformPoint(GetBlendedPose(hand).position);
  380. Vector3 offset = origin.InverseTransformPoint(hand.transform.position);
  381. origin.position = oldOrigin;
  382. hand.transform.rotation = oldHand;
  383. return origin.TransformPoint(offset);
  384. }
  385. public Quaternion GetTargetHandRotation(SteamVR_Behaviour_Skeleton hand, Transform origin)
  386. {
  387. Quaternion oldOrigin = origin.rotation;
  388. origin.rotation = hand.transform.rotation * GetBlendedPose(hand).rotation;
  389. Quaternion offsetRot = Quaternion.Inverse(origin.rotation) * hand.transform.rotation;
  390. origin.rotation = oldOrigin;
  391. return origin.rotation * offsetRot;
  392. }
  393. }
  394. /// <summary>
  395. /// PoseSnapshots hold a skeleton pose for one hand, as well as storing which hand they contain.
  396. /// They have several functions for combining BlendablePoses.
  397. /// </summary>
  398. public class SteamVR_Skeleton_PoseSnapshot
  399. {
  400. public SteamVR_Input_Sources inputSource;
  401. public Vector3 position;
  402. public Quaternion rotation;
  403. public Vector3[] bonePositions;
  404. public Quaternion[] boneRotations;
  405. public SteamVR_Skeleton_PoseSnapshot(int boneCount, SteamVR_Input_Sources source)
  406. {
  407. inputSource = source;
  408. bonePositions = new Vector3[boneCount];
  409. boneRotations = new Quaternion[boneCount];
  410. position = Vector3.zero;
  411. rotation = Quaternion.identity;
  412. }
  413. /// <summary>
  414. /// Perform a deep copy from one poseSnapshot to another.
  415. /// </summary>
  416. public void CopyFrom(SteamVR_Skeleton_PoseSnapshot source)
  417. {
  418. inputSource = source.inputSource;
  419. position = source.position;
  420. rotation = source.rotation;
  421. for (int i = 0; i < bonePositions.Length; i++)
  422. {
  423. bonePositions[i] = source.bonePositions[i];
  424. boneRotations[i] = source.boneRotations[i];
  425. }
  426. }
  427. }
  428. /// <summary>
  429. /// Simple mask for fingers
  430. /// </summary>
  431. [System.Serializable]
  432. public class SteamVR_Skeleton_HandMask
  433. {
  434. public bool palm;
  435. public bool thumb;
  436. public bool index;
  437. public bool middle;
  438. public bool ring;
  439. public bool pinky;
  440. public bool[] values = new bool[6];
  441. public void SetFinger(int i, bool value)
  442. {
  443. values[i] = value;
  444. Apply();
  445. }
  446. public bool GetFinger(int i)
  447. {
  448. return values[i];
  449. }
  450. public SteamVR_Skeleton_HandMask()
  451. {
  452. values = new bool[6];
  453. Reset();
  454. }
  455. /// <summary>
  456. /// All elements on
  457. /// </summary>
  458. public void Reset()
  459. {
  460. values = new bool[6];
  461. for (int i = 0; i < 6; i++)
  462. {
  463. values[i] = true;
  464. }
  465. Apply();
  466. }
  467. protected void Apply()
  468. {
  469. palm = values[0];
  470. thumb = values[1];
  471. index = values[2];
  472. middle = values[3];
  473. ring = values[4];
  474. pinky = values[5];
  475. }
  476. public static readonly SteamVR_Skeleton_HandMask fullMask = new SteamVR_Skeleton_HandMask();
  477. };
  478. }