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;
}
