9using System.Collections;
10using System.Collections.Generic;
36 private List<Vector3> _points, _refPoints;
38 private Vector3 _controllerCentroid, _objectCentroid;
39 private float _manipulatorCount;
41 private Dictionary<InteractionController, PosePointCollection> _controllerToPoints;
44 _intObj = interactionObj;
47 _points =
new List<Vector3>(20); _refPoints =
new List<Vector3>(20);
48 _controllerToPoints =
new Dictionary<InteractionController, PosePointCollection>();
54 _controllerToPoints[controller] = newPoints;
59 newPoints.SetWorldPosition(i, manipulatorPosition);
64 var collection = _controllerToPoints[controller];
65 _controllerToPoints.Remove(controller);
72 foreach (var controllerPointsPair
in _controllerToPoints) {
75 _controllerToPoints.Clear();
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);
84 _controllerCentroid = Vector3.zero; _objectCentroid = Vector3.zero; _manipulatorCount = 0f;
86 foreach (var controllerPointPair
in _controllerToPoints) {
97 Vector3 point1 = (it.MultiplyPoint3x4(originalManipulatorPos) - bodyPosition);
98 Vector3 point2 = (currentManipulatorPos - bodyPosition);
103 _objectCentroid += point1;
104 _controllerCentroid += point2;
105 _manipulatorCount += 1F;
110 _points.Add(point1); _refPoints.Add(point2);
115 Matrix4x4 kabschTransform = PerformSolve(bodyPosition);
117 newPosition = bodyPosition + kabschTransform.GetVector3();
118 newRotation = kabschTransform.GetQuaternion() * bodyRotation;
121 private Matrix4x4 PerformSolve(Vector3 position) {
122 switch (_solveMethod) {
127 _objectCentroid /= _manipulatorCount;
128 _controllerCentroid /= _manipulatorCount;
129 if (!_objectCentroid.Equals(_controllerCentroid)) {
130 return Matrix4x4.TRS(Vector3.zero, Quaternion.FromToRotation(_objectCentroid, _controllerCentroid), Vector3.one);
133 return Matrix4x4.identity;
143 private static Stack<PosePointCollection> _posePointCollectionPool =
new Stack<PosePointCollection>();
145 private List<Vector3> _localPositions;
146 private Matrix4x4 _inverseTransformMatrix;
149 Quaternion rotation) {
151 if (_posePointCollectionPool.Count != 0) {
152 collection = _posePointCollectionPool.Pop();
157 collection.Initialize(position, rotation);
162 _posePointCollectionPool.Push(posePointCollection);
166 _localPositions =
new List<Vector3>();
169 private void Initialize(Vector3 position, Quaternion rotation) {
170 _inverseTransformMatrix = Matrix4x4.TRS(position, rotation, Vector3.one).inverse;
174 Vector3 localPosition = _inverseTransformMatrix.MultiplyPoint3x4(worldPosition);
176 if (index > _localPositions.Count) {
177 Debug.LogError(
"SetWorldPosition requires setting indices in order from 0.");
180 if (_localPositions.Count == index) {
181 _localPositions.Add(localPosition);
184 _localPositions[index] = localPosition;
189 return _localPositions[index];
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)
Vector3 GetLocalPosition(int index)
static void Return(PosePointCollection posePointCollection)
void SetWorldPosition(int index, Vector3 worldPosition)
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)
An IGraspedPoseHandler specifies where an object grasped by a Leap hand or controller (or multiple ha...