public void ClearControllers()
{
foreach (var controllerPointsPair in _controllerToPoints)
{
PosePointCollection.Return(controllerPointsPair.Value);
}
_controllerToPoints.Clear();
}
public void RemoveController(InteractionController controller)
{
var collection = _controllerToPoints[controller];
_controllerToPoints.Remove(controller);
// Return the collection to the pool so it can be re-used.
PosePointCollection.Return(collection);
}
public void AddController(InteractionController controller)
{
var newPoints = PosePointCollection.Create(_intObj.rigidbody.position,
_intObj.rigidbody.rotation);
_controllerToPoints[controller] = newPoints;
for (int i = 0; i < controller.graspManipulatorPoints.Count; i++)
{
Vector3 manipulatorPosition = controller.graspManipulatorPoints[i];
newPoints.SetWorldPosition(i, manipulatorPosition);
}
}
public void GetGraspedPosition(out Vector3 newPosition, out Quaternion newRotation)
{
_points.Clear(); _refPoints.Clear();
Vector3 bodyPosition = _intObj.rigidbody.position;
Quaternion bodyRotation = _intObj.rigidbody.rotation;
Matrix4x4 it = Matrix4x4.TRS(bodyPosition, bodyRotation, Vector3.one);
_controllerCentroid = Vector3.zero; _objectCentroid = Vector3.zero; _manipulatorCount = 0f;
foreach (var controllerPointPair in _controllerToPoints)
{
InteractionController controller = controllerPointPair.Key;
PosePointCollection points = _controllerToPoints[controller];
for (int i = 0; i < controller.graspManipulatorPoints.Count; i++)
{
Vector3 originalManipulatorPos = points.GetLocalPosition(i);
Vector3 currentManipulatorPos = controller.graspManipulatorPoints[i];
// Perform the solve such that the objects' positions are matched to the new
// manipulator positions.
Vector3 point1 = (it.MultiplyPoint3x4(originalManipulatorPos) - bodyPosition);
Vector3 point2 = (currentManipulatorPos - bodyPosition);
if (_intObj.isPositionLocked)
{
// Only rotate the object, pivoting around its origin.
_solveMethod = SolveMethod.PivotAroundOrigin;
_objectCentroid += point1;
_controllerCentroid += point2;
_manipulatorCount += 1F;
}
else
{
// Do normal Kabsch solve.
_solveMethod = SolveMethod.SixDegreeSolve;
_points.Add(point1); _refPoints.Add(point2);
}
}
}
Matrix4x4 kabschTransform = PerformSolve(bodyPosition);
newPosition = bodyPosition + kabschTransform.GetVector3();
newRotation = kabschTransform.GetQuaternion() * bodyRotation;
}
public static PosePointCollection Create(Vector3 position,
Quaternion rotation)
{
PosePointCollection collection;
if (_posePointCollectionPool.Count != 0)
{
collection = _posePointCollectionPool.Pop();
}
else
{
collection = new PosePointCollection();
}
collection.Initialize(position, rotation);
return(collection);
}
public static void Return(PosePointCollection posePointCollection)
{
_posePointCollectionPool.Push(posePointCollection);
}
