Tanoda
KabschGraspedPose.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.Collections;
10using System.Collections.Generic;
11using UnityEngine;
12
13namespace Leap.Unity.Interaction {
14
25 public const int NUM_FINGERS = 5;
26 public const int NUM_BONES = 4;
27
28 public enum SolveMethod {
29 SixDegreeSolve,
30 PivotAroundOrigin
31 }
32 private SolveMethod _solveMethod;
33
34 private InteractionBehaviour _intObj;
35 private KabschSolver _kabsch;
36 private List<Vector3> _points, _refPoints;
37
38 private Vector3 _controllerCentroid, _objectCentroid;
39 private float _manipulatorCount;
40
41 private Dictionary<InteractionController, PosePointCollection> _controllerToPoints;
42
43 public KabschGraspedPose(InteractionBehaviour interactionObj) {
44 _intObj = interactionObj;
45
46 _kabsch = new KabschSolver();
47 _points = new List<Vector3>(20); _refPoints = new List<Vector3>(20);
48 _controllerToPoints = new Dictionary<InteractionController, PosePointCollection>();
49 }
50
51 public void AddController(InteractionController controller) {
52 var newPoints = PosePointCollection.Create(_intObj.rigidbody.position,
53 _intObj.rigidbody.rotation);
54 _controllerToPoints[controller] = newPoints;
55
56 for (int i = 0; i < controller.graspManipulatorPoints.Count; i++) {
57 Vector3 manipulatorPosition = controller.graspManipulatorPoints[i];
58
59 newPoints.SetWorldPosition(i, manipulatorPosition);
60 }
61 }
62
63 public void RemoveController(InteractionController controller) {
64 var collection = _controllerToPoints[controller];
65 _controllerToPoints.Remove(controller);
66
67 // Return the collection to the pool so it can be re-used.
68 PosePointCollection.Return(collection);
69 }
70
71 public void ClearControllers() {
72 foreach (var controllerPointsPair in _controllerToPoints) {
73 PosePointCollection.Return(controllerPointsPair.Value);
74 }
75 _controllerToPoints.Clear();
76 }
77
78 public void GetGraspedPosition(out Vector3 newPosition, out Quaternion newRotation) {
79 _points.Clear(); _refPoints.Clear();
80 Vector3 bodyPosition = _intObj.rigidbody.position;
81 Quaternion bodyRotation = _intObj.rigidbody.rotation;
82 Matrix4x4 it = Matrix4x4.TRS(bodyPosition, bodyRotation, Vector3.one);
83
84 _controllerCentroid = Vector3.zero; _objectCentroid = Vector3.zero; _manipulatorCount = 0f;
85
86 foreach (var controllerPointPair in _controllerToPoints) {
87 InteractionController controller = controllerPointPair.Key;
88 PosePointCollection points = _controllerToPoints[controller];
89
90 for (int i = 0; i < controller.graspManipulatorPoints.Count; i++) {
91
92 Vector3 originalManipulatorPos = points.GetLocalPosition(i);
93 Vector3 currentManipulatorPos = controller.graspManipulatorPoints[i];
94
95 // Perform the solve such that the objects' positions are matched to the new
96 // manipulator positions.
97 Vector3 point1 = (it.MultiplyPoint3x4(originalManipulatorPos) - bodyPosition);
98 Vector3 point2 = (currentManipulatorPos - bodyPosition);
99
100 if (_intObj.isPositionLocked) {
101 // Only rotate the object, pivoting around its origin.
102 _solveMethod = SolveMethod.PivotAroundOrigin;
103 _objectCentroid += point1;
104 _controllerCentroid += point2;
105 _manipulatorCount += 1F;
106 }
107 else {
108 // Do normal Kabsch solve.
109 _solveMethod = SolveMethod.SixDegreeSolve;
110 _points.Add(point1); _refPoints.Add(point2);
111 }
112 }
113 }
114
115 Matrix4x4 kabschTransform = PerformSolve(bodyPosition);
116
117 newPosition = bodyPosition + kabschTransform.GetVector3();
118 newRotation = kabschTransform.GetQuaternion() * bodyRotation;
119 }
120
121 private Matrix4x4 PerformSolve(Vector3 position) {
122 switch (_solveMethod) {
123 case SolveMethod.SixDegreeSolve:
124 return _kabsch.SolveKabsch(_points, _refPoints);
125
126 case SolveMethod.PivotAroundOrigin:
127 _objectCentroid /= _manipulatorCount;
128 _controllerCentroid /= _manipulatorCount;
129 if (!_objectCentroid.Equals(_controllerCentroid)) {
130 return Matrix4x4.TRS(Vector3.zero, Quaternion.FromToRotation(_objectCentroid, _controllerCentroid), Vector3.one);
131 }
132 else {
133 return Matrix4x4.identity;
134 }
135 default:
136 return _kabsch.SolveKabsch(_points, _refPoints);
137 }
138 }
139
140 protected class PosePointCollection {
141 // Without a pool, you might end up with 2 instances per object
142 // With a pool, likely there will only ever be 2 instances!
143 private static Stack<PosePointCollection> _posePointCollectionPool = new Stack<PosePointCollection>();
144
145 private List<Vector3> _localPositions;
146 private Matrix4x4 _inverseTransformMatrix;
147
148 public static PosePointCollection Create(Vector3 position,
149 Quaternion rotation) {
150 PosePointCollection collection;
151 if (_posePointCollectionPool.Count != 0) {
152 collection = _posePointCollectionPool.Pop();
153 } else {
154 collection = new PosePointCollection();
155 }
156
157 collection.Initialize(position, rotation);
158 return collection;
159 }
160
161 public static void Return(PosePointCollection posePointCollection) {
162 _posePointCollectionPool.Push(posePointCollection);
163 }
164
165 private PosePointCollection() {
166 _localPositions = new List<Vector3>();
167 }
168
169 private void Initialize(Vector3 position, Quaternion rotation) {
170 _inverseTransformMatrix = Matrix4x4.TRS(position, rotation, Vector3.one).inverse;
171 }
172
173 public void SetWorldPosition(int index, Vector3 worldPosition) {
174 Vector3 localPosition = _inverseTransformMatrix.MultiplyPoint3x4(worldPosition);
175
176 if (index > _localPositions.Count) {
177 Debug.LogError("SetWorldPosition requires setting indices in order from 0.");
178 }
179
180 if (_localPositions.Count == index) {
181 _localPositions.Add(localPosition);
182 }
183 else {
184 _localPositions[index] = localPosition;
185 }
186 }
187
188 public Vector3 GetLocalPosition(int index) {
189 return _localPositions[index];
190 }
191 }
192
193 }
194
195}
UnityEngine.Debug Debug
Definition: TanodaServer.cs:19
InteractionBehaviours are components that enable GameObjects to interact with interaction controllers...
Rigidbody rigidbody
The Rigidbody associated with this interaction object.
bool isPositionLocked
Returns whether the InteractionBehaviour has its position fully locked by its Rigidbody settings or b...
abstract List< Vector3 > graspManipulatorPoints
Gets the points of the controller to add to the calculation to determine how held objects should move...
static PosePointCollection Create(Vector3 position, Quaternion rotation)
static void Return(PosePointCollection posePointCollection)
This pose handler is the default implementation of IGraspedPoseHandler provided by the Interaction En...
void ClearControllers()
Called e.g. if the InteractionBehaviour is set not to move while being grasped; this should clear any...
void AddController(InteractionController controller)
Called when a new InteractionController begins grasping a certain object. The controller or Leap hand...
void RemoveController(InteractionController controller)
Called when an InteractionController stops grasping a certain object; the controller should no longer...
KabschGraspedPose(InteractionBehaviour interactionObj)
void GetGraspedPosition(out Vector3 newPosition, out Quaternion newRotation)
Calculate the best holding position and orientation given the current state of all InteractionControl...
Matrix4x4 SolveKabsch(List< Vector3 > inPoints, List< Vector3 > refPoints, int optimalRotationIterations=9, bool solveScale=false)
Definition: KabschSolver.cs:20
An IGraspedPoseHandler specifies where an object grasped by a Leap hand or controller (or multiple ha...