relative_pose.py

import maya.api.OpenMaya as om2

def relative_matrix(matrix, old_com_matrix, new_com_matrix,
                    forwardVector=om2.MVector(1, 0, 0), upVector=om2.MVector(0, 1, 0)):
    """
    Returns a new modified matrix relatively matching a previous matrix.

    Args:
        matrix(om2.MMatrix): The matrix to modify.
        old_com_matrix(om2.MMatrix): The previous center of mass matrix.
        new_com_matrix(om2.MMatrix): The current center of mass matrix.
        forwardVector(om2.MVector): The local forward direction of the center of mass node.
        upVector(om2.MVector): A direction perpendicular to the facing plane. The default is world up.

    Returns:
        o2.MMatrix: The relative matrix.
    """

    # Convert matrices to transforms, this allows us to get or set individual components of the matrix
    new_com_transform = om2.MTransformationMatrix(new_com_matrix)
    old_com_transform = om2.MTransformationMatrix(old_com_matrix)

    # Apply relative translation to center of mass
    offset_translation = old_com_transform.translation(om2.MSpace.kWorld) - new_com_transform.translation(om2.MSpace.kWorld)
    new_com_transform.translateBy(offset_translation, om2.MSpace.kWorld)

    def getFacing(matrix):
        """ Returns the local facing direction of a matrix based on a forward and up vector. """
        localForwardVector = forwardVector * matrix
        return upVector ^ (upVector ^ localForwardVector)  # ^ is the cross product operator

    # Apply relative rotation to center of mass
    offset_rotation = getFacing(new_com_matrix).rotateTo(getFacing(old_com_matrix))
    new_com_transform.rotateBy(offset_rotation, om2.MSpace.kWorld)

    # Get the offset from the center of mass to the new matrix
    offset_matrix = matrix * new_com_matrix.inverse()

    # Return the new center of mass position offset by with offset matrix
    return offset_matrix * new_com_transform.asMatrix()