private void InterpolationFunction(GameObject rotObj, System.Numerics.Quaternion[] rotations)
{
//turn the quaternion into a rotation matrix.
System.Numerics.Matrix4x4 rotationMatrixFirstPose = System.Numerics.Matrix4x4.CreateFromQuaternion(rotations[0]);
System.Numerics.Matrix4x4 rotationMatrixSecondPose = System.Numerics.Matrix4x4.CreateFromQuaternion(rotations[1]);
//System.Numerics.Matrix4x4 rotationMatrixThirdPose = System.Numerics.Matrix4x4.CreateFromQuaternion(rotations[2]);
//find the interpolated entry for each entry of the matrix.
System.Numerics.Matrix4x4 interpolatedMat = new System.Numerics.Matrix4x4();
Vector3 position = gameObject.transform.position;
interpolatedMat.M11 = interpolate(rotationMatrixFirstPose.M11, rotationMatrixSecondPose.M11, 0f, position);
interpolatedMat.M12 = interpolate(rotationMatrixFirstPose.M12, rotationMatrixSecondPose.M12, 0f, position);
interpolatedMat.M13 = interpolate(rotationMatrixFirstPose.M13, rotationMatrixSecondPose.M13, 0f, position);
interpolatedMat.M21 = interpolate(rotationMatrixFirstPose.M21, rotationMatrixSecondPose.M21, 0f, position);
interpolatedMat.M22 = interpolate(rotationMatrixFirstPose.M22, rotationMatrixSecondPose.M22, 0f, position);
interpolatedMat.M23 = interpolate(rotationMatrixFirstPose.M23, rotationMatrixSecondPose.M23, 0f, position);
interpolatedMat.M31 = interpolate(rotationMatrixFirstPose.M31, rotationMatrixSecondPose.M31, 0f, position);
interpolatedMat.M32 = interpolate(rotationMatrixFirstPose.M32, rotationMatrixSecondPose.M32, 0f, position);
interpolatedMat.M33 = interpolate(rotationMatrixFirstPose.M33, rotationMatrixSecondPose.M33, 0f, position);
Debug.Log(interpolatedMat.ToString());
Debug.Log("Cursor" + " " + position);
//orthonormalize the interpolated Matrix
System.Numerics.Matrix4x4 orthogonalMatrix = matrixOrthogonal(interpolatedMat);
//Convert the matrix to quaternion.
System.Numerics.Quaternion newPose = System.Numerics.Quaternion.CreateFromRotationMatrix(orthogonalMatrix);
//apply the transformation to the joints
UnityEngine.Quaternion newPoseQuaternion = new UnityEngine.Quaternion();
newPoseQuaternion.x = newPose.X;
newPoseQuaternion.y = newPose.Y;
newPoseQuaternion.z = newPose.Z;
newPoseQuaternion.w = newPose.W;
rotObj.transform.rotation = Quaternion.Lerp(rotObj.transform.rotation, newPoseQuaternion, Time.deltaTime);
}
