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