scipy 1.16.2__cp314-cp314t-win_arm64.whl

scipy/linalg/_decomp_cholesky.py

@@ -0,0 +1,413 @@
+"""Cholesky decomposition functions."""
+
+import numpy as np
+from numpy import asarray_chkfinite, asarray, atleast_2d, empty_like
+
+# Local imports
+from scipy._lib._util import _apply_over_batch
+from ._misc import LinAlgError, _datacopied
+from .lapack import get_lapack_funcs
+
+__all__ = ['cholesky', 'cho_factor', 'cho_solve', 'cholesky_banded',
+           'cho_solve_banded']
+
+
+def _cholesky(a, lower=False, overwrite_a=False, clean=True,
+              check_finite=True):
+    """Common code for cholesky() and cho_factor()."""
+
+    a1 = asarray_chkfinite(a) if check_finite else asarray(a)
+    a1 = atleast_2d(a1)
+
+    # Dimension check
+    if a1.ndim != 2:
+        raise ValueError(f'Input array needs to be 2D but received a {a1.ndim}d-array.')
+    # Squareness check
+    if a1.shape[0] != a1.shape[1]:
+        raise ValueError('Input array is expected to be square but has '
+                         f'the shape: {a1.shape}.')
+
+    # Quick return for square empty array
+    if a1.size == 0:
+        dt = cholesky(np.eye(1, dtype=a1.dtype)).dtype
+        return empty_like(a1, dtype=dt), lower
+
+    overwrite_a = overwrite_a or _datacopied(a1, a)
+    potrf, = get_lapack_funcs(('potrf',), (a1,))
+    c, info = potrf(a1, lower=lower, overwrite_a=overwrite_a, clean=clean)
+    if info > 0:
+        raise LinAlgError(
+            f"{info}-th leading minor of the array is not positive definite"
+        )
+    if info < 0:
+        raise ValueError(
+            f'LAPACK reported an illegal value in {-info}-th argument '
+            f'on entry to "POTRF".'
+        )
+    return c, lower
+
+
+@_apply_over_batch(('a', 2))
+def cholesky(a, lower=False, overwrite_a=False, check_finite=True):
+    """
+    Compute the Cholesky decomposition of a matrix.
+
+    Returns the Cholesky decomposition, :math:`A = L L^*` or
+    :math:`A = U^* U` of a Hermitian positive-definite matrix A.
+
+    Parameters
+    ----------
+    a : (M, M) array_like
+        Matrix to be decomposed
+    lower : bool, optional
+        Whether to compute the upper- or lower-triangular Cholesky
+        factorization. During decomposition, only the selected half of the
+        matrix is referenced. Default is upper-triangular.
+    overwrite_a : bool, optional
+        Whether to overwrite data in `a` (may improve performance).
+    check_finite : bool, optional
+        Whether to check that the entire input matrix contains only finite numbers.
+        Disabling may give a performance gain, but may result in problems
+        (crashes, non-termination) if the inputs do contain infinities or NaNs.
+
+    Returns
+    -------
+    c : (M, M) ndarray
+        Upper- or lower-triangular Cholesky factor of `a`.
+
+    Raises
+    ------
+    LinAlgError : if decomposition fails.
+
+    Notes
+    -----
+    During the finiteness check (if selected), the entire matrix `a` is
+    checked. During decomposition, `a` is assumed to be symmetric or Hermitian
+    (as applicable), and only the half selected by option `lower` is referenced.
+    Consequently, if `a` is asymmetric/non-Hermitian, `cholesky` may still
+    succeed if the symmetric/Hermitian matrix represented by the selected half
+    is positive definite, yet it may fail if an element in the other half is
+    non-finite.
+
+    Examples
+    --------
+    >>> import numpy as np
+    >>> from scipy.linalg import cholesky
+    >>> a = np.array([[1,-2j],[2j,5]])
+    >>> L = cholesky(a, lower=True)
+    >>> L
+    array([[ 1.+0.j,  0.+0.j],
+           [ 0.+2.j,  1.+0.j]])
+    >>> L @ L.T.conj()
+    array([[ 1.+0.j,  0.-2.j],
+           [ 0.+2.j,  5.+0.j]])
+
+    """
+    c, lower = _cholesky(a, lower=lower, overwrite_a=overwrite_a, clean=True,
+                         check_finite=check_finite)
+    return c
+
+
+@_apply_over_batch(("a", 2))
+def cho_factor(a, lower=False, overwrite_a=False, check_finite=True):
+    """
+    Compute the Cholesky decomposition of a matrix, to use in cho_solve
+
+    Returns a matrix containing the Cholesky decomposition,
+    ``A = L L*`` or ``A = U* U`` of a Hermitian positive-definite matrix `a`.
+    The return value can be directly used as the first parameter to cho_solve.
+
+    .. warning::
+        The returned matrix also contains random data in the entries not
+        used by the Cholesky decomposition. If you need to zero these
+        entries, use the function `cholesky` instead.
+
+    Parameters
+    ----------
+    a : (M, M) array_like
+        Matrix to be decomposed
+    lower : bool, optional
+        Whether to compute the upper or lower triangular Cholesky factorization.
+        During decomposition, only the selected half of the matrix is referenced.
+        (Default: upper-triangular)
+    overwrite_a : bool, optional
+        Whether to overwrite data in a (may improve performance)
+    check_finite : bool, optional
+        Whether to check that the entire input matrix contains only finite numbers.
+        Disabling may give a performance gain, but may result in problems
+        (crashes, non-termination) if the inputs do contain infinities or NaNs.
+
+    Returns
+    -------
+    c : (M, M) ndarray
+        Matrix whose upper or lower triangle contains the Cholesky factor
+        of `a`. Other parts of the matrix contain random data.
+    lower : bool
+        Flag indicating whether the factor is in the lower or upper triangle
+
+    Raises
+    ------
+    LinAlgError
+        Raised if decomposition fails.
+
+    See Also
+    --------
+    cho_solve : Solve a linear set equations using the Cholesky factorization
+                of a matrix.
+
+    Notes
+    -----
+    During the finiteness check (if selected), the entire matrix `a` is
+    checked. During decomposition, `a` is assumed to be symmetric or Hermitian
+    (as applicable), and only the half selected by option `lower` is referenced.
+    Consequently, if `a` is asymmetric/non-Hermitian, `cholesky` may still
+    succeed if the symmetric/Hermitian matrix represented by the selected half
+    is positive definite, yet it may fail if an element in the other half is
+    non-finite.
+
+    Examples
+    --------
+    >>> import numpy as np
+    >>> from scipy.linalg import cho_factor
+    >>> A = np.array([[9, 3, 1, 5], [3, 7, 5, 1], [1, 5, 9, 2], [5, 1, 2, 6]])
+    >>> c, low = cho_factor(A)
+    >>> c
+    array([[3.        , 1.        , 0.33333333, 1.66666667],
+           [3.        , 2.44948974, 1.90515869, -0.27216553],
+           [1.        , 5.        , 2.29330749, 0.8559528 ],
+           [5.        , 1.        , 2.        , 1.55418563]])
+    >>> np.allclose(np.triu(c).T @ np. triu(c) - A, np.zeros((4, 4)))
+    True
+
+    """
+    c, lower = _cholesky(a, lower=lower, overwrite_a=overwrite_a, clean=False,
+                         check_finite=check_finite)
+    return c, lower
+
+
+def cho_solve(c_and_lower, b, overwrite_b=False, check_finite=True):
+    """Solve the linear equations A x = b, given the Cholesky factorization of A.
+
+    Parameters
+    ----------
+    (c, lower) : tuple, (array, bool)
+        Cholesky factorization of a, as given by cho_factor
+    b : array
+        Right-hand side
+    overwrite_b : bool, optional
+        Whether to overwrite data in b (may improve performance)
+    check_finite : bool, optional
+        Whether to check that the input matrices contain only finite numbers.
+        Disabling may give a performance gain, but may result in problems
+        (crashes, non-termination) if the inputs do contain infinities or NaNs.
+
+    Returns
+    -------
+    x : array
+        The solution to the system A x = b
+
+    See Also
+    --------
+    cho_factor : Cholesky factorization of a matrix
+
+    Examples
+    --------
+    >>> import numpy as np
+    >>> from scipy.linalg import cho_factor, cho_solve
+    >>> A = np.array([[9, 3, 1, 5], [3, 7, 5, 1], [1, 5, 9, 2], [5, 1, 2, 6]])
+    >>> c, low = cho_factor(A)
+    >>> x = cho_solve((c, low), [1, 1, 1, 1])
+    >>> np.allclose(A @ x - [1, 1, 1, 1], np.zeros(4))
+    True
+
+    """
+    c, lower = c_and_lower
+    return _cho_solve(c, b, lower, overwrite_b=overwrite_b, check_finite=check_finite)
+
+
+@_apply_over_batch(('c', 2), ('b', '1|2'))
+def _cho_solve(c, b, lower, overwrite_b, check_finite):
+    if check_finite:
+        b1 = asarray_chkfinite(b)
+        c = asarray_chkfinite(c)
+    else:
+        b1 = asarray(b)
+        c = asarray(c)
+
+    if c.ndim != 2 or c.shape[0] != c.shape[1]:
+        raise ValueError("The factored matrix c is not square.")
+    if c.shape[1] != b1.shape[0]:
+        raise ValueError(f"incompatible dimensions ({c.shape} and {b1.shape})")
+
+    # accommodate empty arrays
+    if b1.size == 0:
+        dt = cho_solve((np.eye(2, dtype=b1.dtype), True),
+                        np.ones(2, dtype=c.dtype)).dtype
+        return empty_like(b1, dtype=dt)
+
+    overwrite_b = overwrite_b or _datacopied(b1, b)
+
+    potrs, = get_lapack_funcs(('potrs',), (c, b1))
+    x, info = potrs(c, b1, lower=lower, overwrite_b=overwrite_b)
+    if info != 0:
+        raise ValueError(f'illegal value in {-info}th argument of internal potrs')
+    return x
+
+
+@_apply_over_batch(("ab", 2))
+def cholesky_banded(ab, overwrite_ab=False, lower=False, check_finite=True):
+    """
+    Cholesky decompose a banded Hermitian positive-definite matrix
+
+    The matrix a is stored in ab either in lower-diagonal or upper-
+    diagonal ordered form::
+
+        ab[u + i - j, j] == a[i,j]        (if upper form; i <= j)
+        ab[    i - j, j] == a[i,j]        (if lower form; i >= j)
+
+    Example of ab (shape of a is (6,6), u=2)::
+
+        upper form:
+        *   *   a02 a13 a24 a35
+        *   a01 a12 a23 a34 a45
+        a00 a11 a22 a33 a44 a55
+
+        lower form:
+        a00 a11 a22 a33 a44 a55
+        a10 a21 a32 a43 a54 *
+        a20 a31 a42 a53 *   *
+
+    Parameters
+    ----------
+    ab : (u + 1, M) array_like
+        Banded matrix
+    overwrite_ab : bool, optional
+        Discard data in ab (may enhance performance)
+    lower : bool, optional
+        Is the matrix in the lower form. (Default is upper form)
+    check_finite : bool, optional
+        Whether to check that the input matrix contains only finite numbers.
+        Disabling may give a performance gain, but may result in problems
+        (crashes, non-termination) if the inputs do contain infinities or NaNs.
+
+    Returns
+    -------
+    c : (u + 1, M) ndarray
+        Cholesky factorization of a, in the same banded format as ab
+
+    See Also
+    --------
+    cho_solve_banded :
+        Solve a linear set equations, given the Cholesky factorization
+        of a banded Hermitian.
+
+    Examples
+    --------
+    >>> import numpy as np
+    >>> from scipy.linalg import cholesky_banded
+    >>> from numpy import allclose, zeros, diag
+    >>> Ab = np.array([[0, 0, 1j, 2, 3j], [0, -1, -2, 3, 4], [9, 8, 7, 6, 9]])
+    >>> A = np.diag(Ab[0,2:], k=2) + np.diag(Ab[1,1:], k=1)
+    >>> A = A + A.conj().T + np.diag(Ab[2, :])
+    >>> c = cholesky_banded(Ab)
+    >>> C = np.diag(c[0, 2:], k=2) + np.diag(c[1, 1:], k=1) + np.diag(c[2, :])
+    >>> np.allclose(C.conj().T @ C - A, np.zeros((5, 5)))
+    True
+
+    """
+    if check_finite:
+        ab = asarray_chkfinite(ab)
+    else:
+        ab = asarray(ab)
+
+    # accommodate square empty matrices
+    if ab.size == 0:
+        dt = cholesky_banded(np.array([[0, 0], [1, 1]], dtype=ab.dtype)).dtype
+        return empty_like(ab, dtype=dt)
+
+    pbtrf, = get_lapack_funcs(('pbtrf',), (ab,))
+    c, info = pbtrf(ab, lower=lower, overwrite_ab=overwrite_ab)
+    if info > 0:
+        raise LinAlgError(f"{info}-th leading minor not positive definite")
+    if info < 0:
+        raise ValueError(f'illegal value in {info}-th argument of internal pbtrf')
+    return c
+
+
+def cho_solve_banded(cb_and_lower, b, overwrite_b=False, check_finite=True):
+    """
+    Solve the linear equations ``A x = b``, given the Cholesky factorization of
+    the banded Hermitian ``A``.
+
+    Parameters
+    ----------
+    (cb, lower) : tuple, (ndarray, bool)
+        `cb` is the Cholesky factorization of A, as given by cholesky_banded.
+        `lower` must be the same value that was given to cholesky_banded.
+    b : array_like
+        Right-hand side
+    overwrite_b : bool, optional
+        If True, the function will overwrite the values in `b`.
+    check_finite : bool, optional
+        Whether to check that the input matrices contain only finite numbers.
+        Disabling may give a performance gain, but may result in problems
+        (crashes, non-termination) if the inputs do contain infinities or NaNs.
+
+    Returns
+    -------
+    x : array
+        The solution to the system A x = b
+
+    See Also
+    --------
+    cholesky_banded : Cholesky factorization of a banded matrix
+
+    Notes
+    -----
+
+    .. versionadded:: 0.8.0
+
+    Examples
+    --------
+    >>> import numpy as np
+    >>> from scipy.linalg import cholesky_banded, cho_solve_banded
+    >>> Ab = np.array([[0, 0, 1j, 2, 3j], [0, -1, -2, 3, 4], [9, 8, 7, 6, 9]])
+    >>> A = np.diag(Ab[0,2:], k=2) + np.diag(Ab[1,1:], k=1)
+    >>> A = A + A.conj().T + np.diag(Ab[2, :])
+    >>> c = cholesky_banded(Ab)
+    >>> x = cho_solve_banded((c, False), np.ones(5))
+    >>> np.allclose(A @ x - np.ones(5), np.zeros(5))
+    True
+
+    """
+    (cb, lower) = cb_and_lower
+    return _cho_solve_banded(cb, b, lower, overwrite_b=overwrite_b,
+                             check_finite=check_finite)
+
+
+@_apply_over_batch(('cb', 2), ('b', '1|2'))
+def _cho_solve_banded(cb, b, lower, overwrite_b, check_finite):
+    if check_finite:
+        cb = asarray_chkfinite(cb)
+        b = asarray_chkfinite(b)
+    else:
+        cb = asarray(cb)
+        b = asarray(b)
+
+    # Validate shapes.
+    if cb.shape[-1] != b.shape[0]:
+        raise ValueError("shapes of cb and b are not compatible.")
+
+    # accommodate empty arrays
+    if b.size == 0:
+        m = cholesky_banded(np.array([[0, 0], [1, 1]], dtype=cb.dtype))
+        dt = cho_solve_banded((m, True), np.ones(2, dtype=b.dtype)).dtype
+        return empty_like(b, dtype=dt)
+
+    pbtrs, = get_lapack_funcs(('pbtrs',), (cb, b))
+    x, info = pbtrs(cb, b, lower=lower, overwrite_b=overwrite_b)
+    if info > 0:
+        raise LinAlgError(f"{info}th leading minor not positive definite")
+    if info < 0:
+        raise ValueError(f'illegal value in {-info}th argument of internal pbtrs')
+    return x

scipy/linalg/_decomp_cossin.py

@@ -0,0 +1,236 @@
+from collections.abc import Iterable
+import numpy as np
+
+from scipy._lib._util import _asarray_validated, _apply_over_batch
+from scipy.linalg import block_diag, LinAlgError
+from .lapack import _compute_lwork, get_lapack_funcs
+
+__all__ = ['cossin']
+
+
+def cossin(X, p=None, q=None, separate=False,
+           swap_sign=False, compute_u=True, compute_vh=True):
+    """
+    Compute the cosine-sine (CS) decomposition of an orthogonal/unitary matrix.
+
+    X is an ``(m, m)`` orthogonal/unitary matrix, partitioned as the following
+    where upper left block has the shape of ``(p, q)``::
+
+                                   ┌                   ┐
+                                   │ I  0  0 │ 0  0  0 │
+        ┌           ┐   ┌         ┐│ 0  C  0 │ 0 -S  0 │┌         ┐*
+        │ X11 │ X12 │   │ U1 │    ││ 0  0  0 │ 0  0 -I ││ V1 │    │
+        │ ────┼──── │ = │────┼────││─────────┼─────────││────┼────│
+        │ X21 │ X22 │   │    │ U2 ││ 0  0  0 │ I  0  0 ││    │ V2 │
+        └           ┘   └         ┘│ 0  S  0 │ 0  C  0 │└         ┘
+                                   │ 0  0  I │ 0  0  0 │
+                                   └                   ┘
+
+    ``U1``, ``U2``, ``V1``, ``V2`` are square orthogonal/unitary matrices of
+    dimensions ``(p,p)``, ``(m-p,m-p)``, ``(q,q)``, and ``(m-q,m-q)``
+    respectively, and ``C`` and ``S`` are ``(r, r)`` nonnegative diagonal
+    matrices satisfying ``C^2 + S^2 = I`` where ``r = min(p, m-p, q, m-q)``.
+
+    Moreover, the rank of the identity matrices are ``min(p, q) - r``,
+    ``min(p, m - q) - r``, ``min(m - p, q) - r``, and ``min(m - p, m - q) - r``
+    respectively.
+
+    X can be supplied either by itself and block specifications p, q or its
+    subblocks in an iterable from which the shapes would be derived. See the
+    examples below.
+
+    Parameters
+    ----------
+    X : array_like, iterable
+        complex unitary or real orthogonal matrix to be decomposed, or iterable
+        of subblocks ``X11``, ``X12``, ``X21``, ``X22``, when ``p``, ``q`` are
+        omitted.
+    p : int, optional
+        Number of rows of the upper left block ``X11``, used only when X is
+        given as an array.
+    q : int, optional
+        Number of columns of the upper left block ``X11``, used only when X is
+        given as an array.
+    separate : bool, optional
+        if ``True``, the low level components are returned instead of the
+        matrix factors, i.e. ``(u1,u2)``, ``theta``, ``(v1h,v2h)`` instead of
+        ``u``, ``cs``, ``vh``.
+    swap_sign : bool, optional
+        if ``True``, the ``-S``, ``-I`` block will be the bottom left,
+        otherwise (by default) they will be in the upper right block.
+    compute_u : bool, optional
+        if ``False``, ``u`` won't be computed and an empty array is returned.
+    compute_vh : bool, optional
+        if ``False``, ``vh`` won't be computed and an empty array is returned.
+
+    Returns
+    -------
+    u : ndarray
+        When ``compute_u=True``, contains the block diagonal orthogonal/unitary
+        matrix consisting of the blocks ``U1`` (``p`` x ``p``) and ``U2``
+        (``m-p`` x ``m-p``) orthogonal/unitary matrices. If ``separate=True``,
+        this contains the tuple of ``(U1, U2)``.
+    cs : ndarray
+        The cosine-sine factor with the structure described above.
+         If ``separate=True``, this contains the ``theta`` array containing the
+         angles in radians.
+    vh : ndarray
+        When ``compute_vh=True`, contains the block diagonal orthogonal/unitary
+        matrix consisting of the blocks ``V1H`` (``q`` x ``q``) and ``V2H``
+        (``m-q`` x ``m-q``) orthogonal/unitary matrices. If ``separate=True``,
+        this contains the tuple of ``(V1H, V2H)``.
+
+    Notes
+    -----
+    The documentation is written assuming array arguments are of specified
+    "core" shapes. However, array argument(s) of this function may have additional
+    "batch" dimensions prepended to the core shape. In this case, the array is treated
+    as a batch of lower-dimensional slices; see :ref:`linalg_batch` for details.
+
+    References
+    ----------
+    .. [1] Brian D. Sutton. Computing the complete CS decomposition. Numer.
+           Algorithms, 50(1):33-65, 2009.
+
+    Examples
+    --------
+    >>> import numpy as np
+    >>> from scipy.linalg import cossin
+    >>> from scipy.stats import unitary_group
+    >>> x = unitary_group.rvs(4)
+    >>> u, cs, vdh = cossin(x, p=2, q=2)
+    >>> np.allclose(x, u @ cs @ vdh)
+    True
+
+    Same can be entered via subblocks without the need of ``p`` and ``q``. Also
+    let's skip the computation of ``u``
+
+    >>> ue, cs, vdh = cossin((x[:2, :2], x[:2, 2:], x[2:, :2], x[2:, 2:]),
+    ...                      compute_u=False)
+    >>> print(ue)
+    []
+    >>> np.allclose(x, u @ cs @ vdh)
+    True
+
+    """
+
+    if p or q:
+        p = 1 if p is None else int(p)
+        q = 1 if q is None else int(q)
+        X = _asarray_validated(X, check_finite=True)
+        if not np.equal(*X.shape[-2:]):
+            raise ValueError("Cosine Sine decomposition only supports square"
+                             f" matrices, got {X.shape[-2:]}")
+        m = X.shape[-2]
+        if p >= m or p <= 0:
+            raise ValueError(f"invalid p={p}, 0<p<{X.shape[-2]} must hold")
+        if q >= m or q <= 0:
+            raise ValueError(f"invalid q={q}, 0<q<{X.shape[-2]} must hold")
+
+        x11, x12, x21, x22 = (X[..., :p, :q], X[..., :p, q:],
+                              X[..., p:, :q], X[..., p:, q:])
+    elif not isinstance(X, Iterable):
+        raise ValueError("When p and q are None, X must be an Iterable"
+                         " containing the subblocks of X")
+    else:
+        if len(X) != 4:
+            raise ValueError("When p and q are None, exactly four arrays"
+                             f" should be in X, got {len(X)}")
+        x11, x12, x21, x22 = (np.atleast_2d(x) for x in X)
+
+    return _cossin(x11, x12, x21, x22, separate=separate, swap_sign=swap_sign,
+                   compute_u=compute_u, compute_vh=compute_vh)
+
+
+@_apply_over_batch(('x11', 2), ('x12', 2), ('x21', 2), ('x22', 2))
+def _cossin(x11, x12, x21, x22, separate, swap_sign, compute_u, compute_vh):
+
+    for name, block in zip(["x11", "x12", "x21", "x22"],
+                           [x11, x12, x21, x22]):
+        if block.shape[1] == 0:
+            raise ValueError(f"{name} can't be empty")
+    p, q = x11.shape
+    mmp, mmq = x22.shape
+
+    if x12.shape != (p, mmq):
+        raise ValueError(f"Invalid x12 dimensions: desired {(p, mmq)}, "
+                         f"got {x12.shape}")
+
+    if x21.shape != (mmp, q):
+        raise ValueError(f"Invalid x21 dimensions: desired {(mmp, q)}, "
+                         f"got {x21.shape}")
+
+    if p + mmp != q + mmq:
+        raise ValueError("The subblocks have compatible sizes but "
+                         "don't form a square array (instead they form a"
+                          f" {p + mmp}x{q + mmq} array). This might be "
+                          "due to missing p, q arguments.")
+
+    m = p + mmp
+
+    cplx = any([np.iscomplexobj(x) for x in [x11, x12, x21, x22]])
+    driver = "uncsd" if cplx else "orcsd"
+    csd, csd_lwork = get_lapack_funcs([driver, driver + "_lwork"],
+                                      [x11, x12, x21, x22])
+    lwork = _compute_lwork(csd_lwork, m=m, p=p, q=q)
+    lwork_args = ({'lwork': lwork[0], 'lrwork': lwork[1]} if cplx else
+                  {'lwork': lwork})
+    *_, theta, u1, u2, v1h, v2h, info = csd(x11=x11, x12=x12, x21=x21, x22=x22,
+                                            compute_u1=compute_u,
+                                            compute_u2=compute_u,
+                                            compute_v1t=compute_vh,
+                                            compute_v2t=compute_vh,
+                                            trans=False, signs=swap_sign,
+                                            **lwork_args)
+
+    method_name = csd.typecode + driver
+    if info < 0:
+        raise ValueError(f'illegal value in argument {-info} '
+                         f'of internal {method_name}')
+    if info > 0:
+        raise LinAlgError(f"{method_name} did not converge: {info}")
+
+    if separate:
+        return (u1, u2), theta, (v1h, v2h)
+
+    U = block_diag(u1, u2)
+    VDH = block_diag(v1h, v2h)
+
+    # Construct the middle factor CS
+    c = np.diag(np.cos(theta))
+    s = np.diag(np.sin(theta))
+    r = min(p, q, m - p, m - q)
+    n11 = min(p, q) - r
+    n12 = min(p, m - q) - r
+    n21 = min(m - p, q) - r
+    n22 = min(m - p, m - q) - r
+    Id = np.eye(np.max([n11, n12, n21, n22, r]), dtype=theta.dtype)
+    CS = np.zeros((m, m), dtype=theta.dtype)
+
+    CS[:n11, :n11] = Id[:n11, :n11]
+
+    xs = n11 + r
+    xe = n11 + r + n12
+    ys = n11 + n21 + n22 + 2 * r
+    ye = n11 + n21 + n22 + 2 * r + n12
+    CS[xs: xe, ys:ye] = Id[:n12, :n12] if swap_sign else -Id[:n12, :n12]
+
+    xs = p + n22 + r
+    xe = p + n22 + r + + n21
+    ys = n11 + r
+    ye = n11 + r + n21
+    CS[xs:xe, ys:ye] = -Id[:n21, :n21] if swap_sign else Id[:n21, :n21]
+
+    CS[p:p + n22, q:q + n22] = Id[:n22, :n22]
+    CS[n11:n11 + r, n11:n11 + r] = c
+    CS[p + n22:p + n22 + r, n11 + r + n21 + n22:2 * r + n11 + n21 + n22] = c
+
+    xs = n11
+    xe = n11 + r
+    ys = n11 + n21 + n22 + r
+    ye = n11 + n21 + n22 + 2 * r
+    CS[xs:xe, ys:ye] = s if swap_sign else -s
+
+    CS[p + n22:p + n22 + r, n11:n11 + r] = -s if swap_sign else s
+
+    return U, CS, VDH