LCOV - code coverage report
Current view: top level - src/motion - cell_manifold_utils.F (source / functions) Coverage Total Hit
Test: CP2K Regtests (git:dbd702d) Lines: 93.7 % 623 584
Test Date: 2026-11-22 01:17:58 Functions: 100.0 % 22 22

            Line data    Source code
       1              : !--------------------------------------------------------------------------------------------------!
       2              : !   CP2K: A general program to perform molecular dynamics simulations                              !
       3              : !   Copyright 2000-2026 CP2K developers group <https://cp2k.org>                                   !
       4              : !                                                                                                  !
       5              : !   SPDX-License-Identifier: GPL-2.0-or-later                                                      !
       6              : !--------------------------------------------------------------------------------------------------!
       7              : 
       8              : ! **************************************************************************************************
       9              : !> \brief Reduced-coordinate parameterization of cell constraints for CELL_OPT.
      10              : !> \details
      11              : !> This pure-math module depends only on kinds, ieee_arithmetic, and LAPACK.
      12              : !> It has no force_env, cell_type, or input dependencies.
      13              : !>
      14              : !> Canonical cell vector:
      15              : !>   CP2K places lattice vector a along x and b in the xy plane:
      16              : !>     hmat(:, 1) = (AX,  0,  0)
      17              : !>     hmat(:, 2) = (BX, BY,  0)
      18              : !>     hmat(:, 3) = (CX, CY, CZ)
      19              : !>   The 6 upper-triangular entries form q = (AX, BX, BY, CX, CY, CZ).
      20              : !>   The 7 geometric quantities are s(q) = (a, b, c, alpha, beta, gamma, V),
      21              : !>   with angles in radians and volume V = AX * BY * CZ.
      22              : !>
      23              : !> Constraint equations and reduced coordinates:
      24              : !>   Active constraints define k equations r(q) = 0 with k x 6 Jacobian A(q) = dr/dq:
      25              : !>     1. Fixed cell components: q_i - q0_i = 0.
      26              : !>     2. Fixed geometric quantities: s_m(q) - s_m(q0) = 0.
      27              : !>     3. Equal lengths or angles: s_m(q) - s_n(q) = 0 (for example a = b).
      28              : !>     4. Linear relations: w^T (q - q0) = 0 (for example AX = BY).
      29              : !>
      30              : !>   At the initial cell q0, QR factorization splits R^6 into two fixed
      31              : !>   orthonormal bases:
      32              : !>     - Y (6 x k) spans the constrained directions row(A(q0)).
      33              : !>     - Z (6 x ndof, ndof = 6 - k) spans the free directions ker(A(q0)).
      34              : !>
      35              : !>   The optimizer works only on the reduced vector u of length ndof:
      36              : !>     - Forward map (chart_eval): q(u) = q0 + Z (u - u0) + Y w(u).
      37              : !>       Newton's method solves the k x k system r(q(u)) = 0 for w(u).
      38              : !>       Linear constraints satisfy r = 0 in zero Newton iterations.
      39              : !>     - Inverse map (chart_inv): u = u0 + Z^T (q - q0), because Z^T Y = 0.
      40              : !>     - Reduced gradient (chart_pullback_grad): g_u = (dq/du)^T g_q.
      41              : !>       Implicit differentiation gives g_u = Z^T (g_q - A(q)^T lam), where
      42              : !>       (A(q) Y)^T lam = Y^T g_q.
      43              : !>     - Unconstrained case (k = 0): Z = I and u0 = q0, so u = q and g_u = g_q
      44              : !>       with zero floating-point roundoff.
      45              : !>
      46              : !>   Sign convention: get_dg_dh returns g_q = +d(E + pres_ext*V)/dq.
      47              : !>   Routine cell_manifold_proj_pressure maps g_q to pdiff = P_constr - pres_ext.
      48              : ! **************************************************************************************************
      49              : MODULE cell_manifold_utils
      50              : 
      51              :    USE ieee_arithmetic,                 ONLY: ieee_is_finite
      52              :    USE kinds,                           ONLY: default_string_length,&
      53              :                                               dp
      54              : 
      55              :    IMPLICIT NONE
      56              :    PRIVATE
      57              : 
      58              :    ! Indices of q = (AX, BX, BY, CX, CY, CZ) and s = (a, b, c, alpha, beta, gamma, V).
      59              :    INTEGER, PARAMETER, PUBLIC :: n_slot = 6, n_geom = 7
      60              :    INTEGER, PARAMETER, PUBLIC :: slot_ax = 1, &
      61              :                                  slot_bx = 2, &
      62              :                                  slot_by = 3, &
      63              :                                  slot_cx = 4, &
      64              :                                  slot_cy = 5, &
      65              :                                  slot_cz = 6
      66              :    INTEGER, PARAMETER, PUBLIC :: geom_a = 1, &
      67              :                                  geom_b = 2, &
      68              :                                  geom_c = 3, &
      69              :                                  geom_alpha = 4, &
      70              :                                  geom_beta = 5, &
      71              :                                  geom_gamma = 6, &
      72              :                                  geom_v = 7
      73              : 
      74              :    ! Bit flags for fixed canonical components and direction/vector shorthands.
      75              :    INTEGER, PARAMETER, PUBLIC :: comp_ax = 2**0, &
      76              :                                  comp_bx = 2**1, &
      77              :                                  comp_by = 2**2, &
      78              :                                  comp_cx = 2**3, &
      79              :                                  comp_cy = 2**4, &
      80              :                                  comp_cz = 2**5
      81              :    INTEGER, PARAMETER, PUBLIC :: mask_a = comp_ax, &
      82              :                                  mask_b = comp_bx + comp_by, &
      83              :                                  mask_c = comp_cx + comp_cy + comp_cz, &
      84              :                                  mask_x = comp_ax + comp_bx + comp_cx, &
      85              :                                  mask_y = comp_bx + comp_by + comp_cy, &
      86              :                                  mask_z = mask_c, &
      87              :                                  mask_xy = comp_ax + comp_bx + comp_by + comp_cx + comp_cy, &
      88              :                                  mask_xz = comp_ax + comp_bx + comp_cx + comp_cy + comp_cz, &
      89              :                                  mask_yz = comp_bx + comp_by + comp_cx + comp_cy + comp_cz, &
      90              :                                  mask_all = comp_ax + comp_bx + comp_by + comp_cx + &
      91              :                                  comp_cy + comp_cz
      92              : 
      93              :    ! Bit flags that fix s(geom_*) to its initial value s(q0).
      94              :    ! cell_opt_types sets these internal bits for KEEP_ANGLES and KEEP_VOLUME.
      95              :    ! The CONSTRAINT parser does not expose them.
      96              :    INTEGER, PARAMETER, PUBLIC :: gfix_a = 2**6, &
      97              :                                  gfix_b = 2**7, &
      98              :                                  gfix_c = 2**8, &
      99              :                                  gfix_alpha = 2**9, &
     100              :                                  gfix_beta = 2**10, &
     101              :                                  gfix_gamma = 2**11, &
     102              :                                  gfix_v = 2**12
     103              :    INTEGER, PARAMETER, PUBLIC :: gfix_all = gfix_a + gfix_b + gfix_c + gfix_alpha + &
     104              :                                  gfix_beta + gfix_gamma + gfix_v
     105              :    INTEGER, PARAMETER, PUBLIC :: n_constraint_bits = n_slot + n_geom
     106              : 
     107              :    ! max_cell_coord keeps AX*BY*CZ well below HUGE(1.0_dp) (~1.8E308) so volume
     108              :    ! evaluation cannot trap under -ffpe-trap=overflow.
     109              :    REAL(KIND=dp), PARAMETER, PUBLIC  :: min_cell_length = 1.0E-8_dp
     110              :    REAL(KIND=dp), PARAMETER, PRIVATE :: max_cell_coord = 1.0E90_dp
     111              :    REAL(KIND=dp), PARAMETER, PRIVATE :: collinear_cos_margin = 1.0E-8_dp
     112              :    REAL(KIND=dp), PARAMETER, PRIVATE :: licq_pivot_tol = 1.0E-6_dp
     113              :    REAL(KIND=dp), PARAMETER, PRIVATE :: basis_tie_rel_tol = 1.0E-8_dp
     114              :    REAL(KIND=dp), PARAMETER, PRIVATE :: default_chart_tol = 1.0E-12_dp
     115              :    INTEGER, PARAMETER, PRIVATE       :: default_chart_max_iter = 100
     116              :    REAL(KIND=dp), PARAMETER, PRIVATE :: proj_pressure_rel_tol = 1.0E-12_dp
     117              :    REAL(KIND=dp), PARAMETER, PRIVATE :: redundancy_probe_frac = 5.0E-2_dp
     118              :    REAL(KIND=dp), PARAMETER, PRIVATE :: redundancy_chart_tol = 1.0E-13_dp
     119              :    REAL(KIND=dp), PARAMETER, PRIVATE :: redundancy_rel_tol = 1.0E-9_dp
     120              :    REAL(KIND=dp), PARAMETER, PRIVATE :: redundancy_drift_rel_tol = 1.0E-10_dp
     121              :    REAL(KIND=dp), PARAMETER, PRIVATE :: redundancy_curv_rel_tol = 5.0E-12_dp
     122              :    REAL(KIND=dp), PARAMETER, PRIVATE :: semidef_fd_step = 1.0E-3_dp
     123              :    REAL(KIND=dp), PARAMETER, PRIVATE :: semidef_fd_shrink = 0.25_dp
     124              :    INTEGER, PARAMETER, PRIVATE       :: semidef_fd_max_tries = 6
     125              :    REAL(KIND=dp), PARAMETER, PRIVATE :: semidef_lam_rel_tol = 1.0E-11_dp
     126              :    REAL(KIND=dp), PARAMETER, PRIVATE :: semidef_min_curv = 1.0E-8_dp
     127              :    REAL(KIND=dp), PARAMETER, PRIVATE :: semidef_eig_rel_tol = 1.0E-5_dp
     128              :    REAL(KIND=dp), PARAMETER, PRIVATE :: semidef_vertex_rel_tol = 1.0E-3_dp
     129              :    REAL(KIND=dp), PARAMETER, PRIVATE :: semidef_fd_noise_floor = 1.0E-11_dp
     130              :    REAL(KIND=dp), PARAMETER, PRIVATE :: semidef_vec_zero_tol = 1.0E-9_dp
     131              :    REAL(KIND=dp), PARAMETER, PRIVATE :: semidef_max_vec_tol = 1.0E-4_dp
     132              : 
     133              :    INTEGER, PARAMETER, PRIVATE :: comp_flags(n_slot) = [comp_ax, comp_bx, comp_by, comp_cx, &
     134              :                                                         comp_cy, comp_cz]
     135              : 
     136              :    ! Status codes (negative values indicate an error).
     137              :    INTEGER, PARAMETER, PUBLIC :: info_ok = 0
     138              :    INTEGER, PARAMETER, PUBLIC :: info_degenerate = -1
     139              :    INTEGER, PARAMETER, PUBLIC :: info_licq = -2
     140              :    INTEGER, PARAMETER, PUBLIC :: info_no_converge = -3
     141              :    INTEGER, PARAMETER, PUBLIC :: info_bad_input = -4
     142              : 
     143              :    !> \brief Active constraint set and fixed coordinate bases anchored at q0.
     144              :    TYPE, PUBLIC :: cell_manifold_type
     145              :       INTEGER              :: mask = 0
     146              :       REAL(KIND=dp)        :: q0(n_slot) = 0.0_dp
     147              :       REAL(KIND=dp)        :: s0(n_geom) = 0.0_dp
     148              :       INTEGER              :: k = 0
     149              :       INTEGER              :: ndof = n_slot
     150              :       INTEGER              :: slot_rows(n_slot) = 0
     151              :       INTEGER              :: n_slot_rows = 0
     152              :       INTEGER              :: geom_rows(n_geom) = 0
     153              :       INTEGER              :: n_geom_rows = 0
     154              :       ! Column i holds the unit normal w_i of the linear constraint w_i^T (q - q0) = 0.
     155              :       REAL(KIND=dp)        :: lin_w(n_slot, n_slot) = 0.0_dp
     156              :       INTEGER              :: n_lin_rows = 0
     157              :       ! Column i holds a pair [m, n] in 1..6 for the equality s_m(q) - s_n(q) = 0.
     158              :       INTEGER              :: eq_pair(2, n_geom) = 0
     159              :       INTEGER              :: n_eq_rows = 0
     160              :       ! Fixed bases set once at init: Y(:, 1:k) spans row(A(q0)),
     161              :       ! Z(:, 1:ndof) spans ker(A(q0)), and u0(1:ndof) = Z(:, 1:ndof)^T q0.
     162              :       REAL(KIND=dp)        :: Y(n_slot, n_slot) = 0.0_dp
     163              :       REAL(KIND=dp)        :: Z(n_slot, n_slot) = 0.0_dp
     164              :       REAL(KIND=dp)        :: u0(n_slot) = 0.0_dp
     165              :       REAL(KIND=dp)        :: tol = default_chart_tol
     166              :       INTEGER              :: max_iter = default_chart_max_iter
     167              :       INTEGER              :: last_iter = 0
     168              :       REAL(KIND=dp)        :: last_resid = 0.0_dp
     169              :    END TYPE cell_manifold_type
     170              : 
     171              :    PUBLIC :: cell_manifold_init, &
     172              :              cell_manifold_geometry, &
     173              :              cell_manifold_jacobian, &
     174              :              cell_manifold_residual, &
     175              :              cell_manifold_active_jacobian, &
     176              :              cell_manifold_chart_eval, &
     177              :              cell_manifold_chart_inv, &
     178              :              cell_manifold_chart_pullback_grad, &
     179              :              cell_manifold_proj_pressure, &
     180              :              cell_manifold_pack_hmat, &
     181              :              cell_manifold_unpack_hmat, &
     182              :              cell_manifold_status_label, &
     183              :              cell_manifold_is_redundant, &
     184              :              cell_manifold_pin_semidefinite
     185              : 
     186              :    EXTERNAL :: dgeqrf, dgetrf, dgetrs, dorgqr, dpotrf, dpotrs, dsyev
     187              : 
     188              : CONTAINS
     189              : 
     190              : ! **************************************************************************************************
     191              : !> \brief Record constraint targets at q0, check row independence, and build [Y, Z].
     192              : !> \param m initialized constraint state
     193              : !> \param q0 initial canonical cell components
     194              : !> \param mask bit union of comp_* and gfix_* flags
     195              : !> \param info info_ok, info_degenerate, info_licq, or info_bad_input
     196              : !> \param lin_w optional 6 x n_lin linear coupling weights, one constraint per column
     197              : !> \param eq_pair optional 2 x n_eq index pairs [m, n] in 1..6 for s_m(q) = s_n(q)
     198              : ! **************************************************************************************************
     199       185344 :    SUBROUTINE cell_manifold_init(m, q0, mask, info, lin_w, eq_pair)
     200              :       TYPE(cell_manifold_type), INTENT(OUT)              :: m
     201              :       REAL(KIND=dp), INTENT(IN)                          :: q0(n_slot)
     202              :       INTEGER, INTENT(IN)                                :: mask
     203              :       INTEGER, INTENT(OUT)                               :: info
     204              :       REAL(KIND=dp), INTENT(IN), OPTIONAL                :: lin_w(:, :)
     205              :       INTEGER, INTENT(IN), OPTIONAL                      :: eq_pair(:, :)
     206              : 
     207              :       INTEGER, PARAMETER :: &
     208              :          gfix_flags(n_geom) = [gfix_a, gfix_b, gfix_c, gfix_alpha, gfix_beta, gfix_gamma, gfix_v]
     209              : 
     210              :       INTEGER                                            :: i, n_eq, n_lin
     211              :       REAL(KIND=dp)                                      :: weight_norm
     212              : 
     213         1024 :       m%mask = mask
     214         7168 :       m%q0 = q0
     215              : 
     216         1024 :       CALL cell_manifold_geometry(q0, m%s0, info)
     217         1024 :       IF (info /= info_ok) RETURN
     218              : 
     219         1022 :       IF (IAND(mask, NOT(mask_all + gfix_all)) /= 0) THEN
     220            2 :          info = info_bad_input
     221            2 :          RETURN
     222              :       END IF
     223              : 
     224         7140 :       DO i = 1, n_slot
     225         7140 :          IF (IAND(mask, comp_flags(i)) /= 0) THEN
     226         1124 :             m%n_slot_rows = m%n_slot_rows + 1
     227         1124 :             m%slot_rows(m%n_slot_rows) = i
     228              :          END IF
     229              :       END DO
     230         8160 :       DO i = 1, n_geom
     231         8160 :          IF (IAND(mask, gfix_flags(i)) /= 0) THEN
     232          506 :             m%n_geom_rows = m%n_geom_rows + 1
     233          506 :             m%geom_rows(m%n_geom_rows) = i
     234              :          END IF
     235              :       END DO
     236              : 
     237         1020 :       IF (PRESENT(lin_w)) THEN
     238          488 :          n_lin = SIZE(lin_w, 2)
     239          488 :          IF (SIZE(lin_w, 1) /= n_slot .OR. n_lin > n_slot) THEN
     240            2 :             info = info_bad_input
     241              :             RETURN
     242              :          END IF
     243          646 :          DO i = 1, n_lin
     244              :             ! Normalize each weight vector so residual checks do not depend on caller scaling.
     245         1120 :             weight_norm = NORM2(lin_w(:, i))
     246          160 :             IF (weight_norm <= TINY(1.0_dp)) THEN
     247            0 :                info = info_bad_input
     248            0 :                RETURN
     249              :             END IF
     250         1606 :             m%lin_w(:, i) = lin_w(:, i)/weight_norm
     251              :          END DO
     252          486 :          m%n_lin_rows = n_lin
     253              :       END IF
     254              : 
     255         1018 :       IF (PRESENT(eq_pair)) THEN
     256          498 :          n_eq = SIZE(eq_pair, 2)
     257          498 :          IF (SIZE(eq_pair, 1) /= 2 .OR. n_eq > n_geom - 1) THEN
     258            0 :             info = info_bad_input
     259              :             RETURN
     260              :          END IF
     261          720 :          DO i = 1, n_eq
     262         1344 :             IF (ANY(eq_pair(:, i) < geom_a) .OR. ANY(eq_pair(:, i) > geom_gamma) .OR. &
     263              :                 eq_pair(1, i) == eq_pair(2, i)) THEN
     264            2 :                info = info_bad_input
     265            2 :                RETURN
     266              :             END IF
     267         1162 :             m%eq_pair(:, i) = eq_pair(:, i)
     268              :          END DO
     269          496 :          m%n_eq_rows = n_eq
     270              :       END IF
     271              : 
     272         1016 :       m%k = m%n_slot_rows + m%n_geom_rows + m%n_lin_rows + m%n_eq_rows
     273         1016 :       IF (m%k > n_slot) THEN
     274            2 :          info = info_licq
     275            2 :          RETURN
     276              :       END IF
     277         1014 :       m%ndof = n_slot - m%k
     278              : 
     279         1014 :       CALL build_chart_basis(m, info)
     280              : 
     281              :    END SUBROUTINE cell_manifold_init
     282              : 
     283              : ! **************************************************************************************************
     284              : !> \brief Compute cell lengths, angles in radians, and volume s(q).
     285              : !> \param q canonical cell components (AX, BX, BY, CX, CY, CZ)
     286              : !> \param s geometric quantities (a, b, c, alpha, beta, gamma, V)
     287              : !> \param info info_ok or info_degenerate
     288              : ! **************************************************************************************************
     289        36761 :    SUBROUTINE cell_manifold_geometry(q, s, info)
     290              :       REAL(KIND=dp), INTENT(IN)                          :: q(n_slot)
     291              :       REAL(KIND=dp), INTENT(OUT)                         :: s(n_geom)
     292              :       INTEGER, INTENT(OUT)                               :: info
     293              : 
     294              :       REAL(KIND=dp)                                      :: ax, bx, by, cos_ang(3), cx, cy, cz, &
     295              :                                                             vec_a(3), vec_b(3), vec_c(3)
     296              : 
     297        36761 :       s = 0.0_dp
     298       257327 :       IF (.NOT. ALL(ieee_is_finite(q))) THEN
     299            0 :          info = info_degenerate
     300           12 :          RETURN
     301              :       END IF
     302       257315 :       IF (ANY(ABS(q) > max_cell_coord)) THEN
     303            2 :          info = info_degenerate
     304            2 :          RETURN
     305              :       END IF
     306              : 
     307        36759 :       ax = q(slot_ax)
     308        36759 :       bx = q(slot_bx)
     309        36759 :       by = q(slot_by)
     310        36759 :       cx = q(slot_cx)
     311        36759 :       cy = q(slot_cy)
     312        36759 :       cz = q(slot_cz)
     313              : 
     314              :       ! Positive diagonal entries ensure V = AX*BY*CZ > 0 and right-handed cell vectors.
     315        36759 :       IF (MIN(ax, by, cz) < min_cell_length) THEN
     316            8 :          info = info_degenerate
     317            8 :          RETURN
     318              :       END IF
     319              : 
     320       147004 :       vec_a = [ax, 0.0_dp, 0.0_dp]
     321       147004 :       vec_b = [bx, by, 0.0_dp]
     322       147004 :       vec_c = [cx, cy, cz]
     323       147004 :       s(geom_a) = NORM2(vec_a)
     324       147004 :       s(geom_b) = NORM2(vec_b)
     325       147004 :       s(geom_c) = NORM2(vec_c)
     326       147004 :       cos_ang(1) = DOT_PRODUCT(vec_b, vec_c)/(s(geom_b)*s(geom_c))
     327       147004 :       cos_ang(2) = DOT_PRODUCT(vec_a, vec_c)/(s(geom_a)*s(geom_c))
     328       147004 :       cos_ang(3) = DOT_PRODUCT(vec_a, vec_b)/(s(geom_a)*s(geom_b))
     329       147002 :       IF (ANY(ABS(cos_ang) > 1.0_dp - collinear_cos_margin)) THEN
     330            2 :          info = info_degenerate
     331            2 :          s = 0.0_dp
     332            2 :          RETURN
     333              :       END IF
     334       146996 :       s(geom_alpha:geom_gamma) = ACOS(MIN(1.0_dp, MAX(-1.0_dp, cos_ang)))
     335        36749 :       s(geom_v) = ax*by*cz
     336        36749 :       info = info_ok
     337              : 
     338              :    END SUBROUTINE cell_manifold_geometry
     339              : 
     340              : ! **************************************************************************************************
     341              : !> \brief Compute the analytic 7 x 6 Jacobian J = ds/dq of the geometric quantities.
     342              : !> \param q canonical cell components (AX, BX, BY, CX, CY, CZ)
     343              : !> \param J Jacobian matrix ds/dq
     344              : !> \param info info_ok or info_degenerate
     345              : ! **************************************************************************************************
     346        15844 :    SUBROUTINE cell_manifold_jacobian(q, J, info)
     347              :       REAL(KIND=dp), INTENT(IN)                          :: q(n_slot)
     348              :       REAL(KIND=dp), INTENT(OUT)                         :: J(n_geom, n_slot)
     349              :       INTEGER, INTENT(OUT)                               :: info
     350              : 
     351              :       REAL(KIND=dp) :: ax, bx, by, cos_ang(3), cx, cy, cz, db_dq(n_slot), dc_dq(n_slot), &
     352              :          ddot_dq(n_slot), len_a, len_b, len_c, s(n_geom), sin_ang(3)
     353              : 
     354        15844 :       J = 0.0_dp
     355        15844 :       CALL cell_manifold_geometry(q, s, info)
     356        15844 :       IF (info /= info_ok) RETURN
     357              : 
     358        15844 :       ax = q(slot_ax)
     359        15844 :       bx = q(slot_bx)
     360        15844 :       by = q(slot_by)
     361        15844 :       cx = q(slot_cx)
     362        15844 :       cy = q(slot_cy)
     363        15844 :       cz = q(slot_cz)
     364        15844 :       len_a = s(geom_a)
     365        15844 :       len_b = s(geom_b)
     366        15844 :       len_c = s(geom_c)
     367              : 
     368        15844 :       J(geom_a, slot_ax) = SIGN(1.0_dp, ax)
     369              : 
     370       110908 :       db_dq = [0.0_dp, bx/len_b, by/len_b, 0.0_dp, 0.0_dp, 0.0_dp]
     371       110908 :       J(geom_b, :) = db_dq
     372              : 
     373       110908 :       dc_dq = [0.0_dp, 0.0_dp, 0.0_dp, cx/len_c, cy/len_c, cz/len_c]
     374       110908 :       J(geom_c, :) = dc_dq
     375              : 
     376              :       cos_ang = [DOT_PRODUCT([bx, by, 0.0_dp], [cx, cy, cz])/(len_b*len_c), &
     377              :                  (ax*cx)/(len_a*len_c), &
     378       110908 :                  (ax*bx)/(len_a*len_b)]
     379        63376 :       cos_ang = MIN(1.0_dp, MAX(-1.0_dp, cos_ang))
     380        63376 :       sin_ang = SIN(s(geom_alpha:geom_gamma))
     381              : 
     382              :       ! alpha = angle(b, c), with b.c = BX*CX + BY*CY.
     383       110908 :       ddot_dq = [0.0_dp, cx, cy, bx, by, 0.0_dp]
     384       110908 :       J(geom_alpha, :) = angle_grad_row(cos_ang(1), sin_ang(1), ddot_dq, len_b, len_c, db_dq, dc_dq)
     385              : 
     386              :       ! Since a = (AX, 0, 0) with AX > 0, cos(beta) = CX/|c| does not depend on AX.
     387              :       ! Force exact zero to avoid floating-point cancellation noise in d(beta)/d(AX).
     388       110908 :       ddot_dq = [cx, 0.0_dp, 0.0_dp, ax, 0.0_dp, 0.0_dp]
     389              :       J(geom_beta, :) = angle_grad_row(cos_ang(2), sin_ang(2), ddot_dq, len_a, len_c, &
     390       205972 :                                        J(geom_a, :), dc_dq)
     391        15844 :       J(geom_beta, slot_ax) = 0.0_dp
     392              : 
     393              :       ! Likewise, cos(gamma) = BX/|b| does not depend on AX when AX > 0.
     394       110908 :       ddot_dq = [bx, ax, 0.0_dp, 0.0_dp, 0.0_dp, 0.0_dp]
     395              :       J(geom_gamma, :) = angle_grad_row(cos_ang(3), sin_ang(3), ddot_dq, len_a, len_b, &
     396       205972 :                                         J(geom_a, :), db_dq)
     397        15844 :       J(geom_gamma, slot_ax) = 0.0_dp
     398              : 
     399       110908 :       J(geom_v, :) = [by*cz, 0.0_dp, ax*cz, 0.0_dp, 0.0_dp, ax*by]
     400              : 
     401              :    END SUBROUTINE cell_manifold_jacobian
     402              : 
     403              : ! **************************************************************************************************
     404              : !> \brief Evaluate the k active constraint residuals r(q).
     405              : !> \param m constraint state
     406              : !> \param q canonical cell vector
     407              : !> \param r residual vector of length m%k (zero when q satisfies all constraints)
     408              : !> \param info info_ok or info_degenerate
     409              : ! **************************************************************************************************
     410        12016 :    SUBROUTINE cell_manifold_residual(m, q, r, info)
     411              :       TYPE(cell_manifold_type), INTENT(IN)               :: m
     412              :       REAL(KIND=dp), INTENT(IN)                          :: q(n_slot)
     413              :       REAL(KIND=dp), INTENT(OUT)                         :: r(m%k)
     414              :       INTEGER, INTENT(OUT)                               :: info
     415              : 
     416              :       INTEGER                                            :: i, row
     417              :       REAL(KIND=dp)                                      :: s(n_geom)
     418              : 
     419        47660 :       r = 0.0_dp
     420        12016 :       info = info_ok
     421        84112 :       IF (.NOT. ALL(ieee_is_finite(q))) THEN
     422            0 :          info = info_degenerate
     423            2 :          RETURN
     424              :       END IF
     425        12016 :       IF (m%k == 0) RETURN
     426              : 
     427              :       ! Check cell validity even when all active rows are linear so collapsed
     428              :       ! or inverted cells cannot pass unnoticed.
     429        12016 :       CALL cell_manifold_geometry(q, s, info)
     430        12016 :       IF (info /= info_ok) RETURN
     431              : 
     432        29836 :       DO i = 1, m%n_slot_rows
     433        29836 :          r(i) = q(m%slot_rows(i)) - m%q0(m%slot_rows(i))
     434              :       END DO
     435        20604 :       DO i = 1, m%n_geom_rows
     436         8590 :          row = m%n_slot_rows + i
     437        20604 :          r(row) = s(m%geom_rows(i)) - m%s0(m%geom_rows(i))
     438              :       END DO
     439        15434 :       DO i = 1, m%n_lin_rows
     440         3420 :          row = m%n_slot_rows + m%n_geom_rows + i
     441        56474 :          r(row) = DOT_PRODUCT(m%lin_w(:, i), q) - DOT_PRODUCT(m%lin_w(:, i), m%q0)
     442              :       END DO
     443        17818 :       DO i = 1, m%n_eq_rows
     444         5804 :          row = m%n_slot_rows + m%n_geom_rows + m%n_lin_rows + i
     445        17818 :          r(row) = s(m%eq_pair(1, i)) - s(m%eq_pair(2, i))
     446              :       END DO
     447              : 
     448              :    END SUBROUTINE cell_manifold_residual
     449              : 
     450              : ! **************************************************************************************************
     451              : !> \brief Assemble the k x 6 Jacobian A(q) = dr/dq of the active constraints.
     452              : !> \param m constraint state
     453              : !> \param q canonical cell vector
     454              : !> \param A active constraint Jacobian in the same row order as cell_manifold_residual
     455              : !> \param info info_ok or info_degenerate
     456              : ! **************************************************************************************************
     457        15844 :    SUBROUTINE cell_manifold_active_jacobian(m, q, A, info)
     458              :       TYPE(cell_manifold_type), INTENT(IN)               :: m
     459              :       REAL(KIND=dp), INTENT(IN)                          :: q(n_slot)
     460              :       REAL(KIND=dp), INTENT(OUT)                         :: A(m%k, n_slot)
     461              :       INTEGER, INTENT(OUT)                               :: info
     462              : 
     463              :       INTEGER                                            :: i, row
     464              :       REAL(KIND=dp)                                      :: J(n_geom, n_slot)
     465              : 
     466       386536 :       A = 0.0_dp
     467        15844 :       info = info_ok
     468        15844 :       IF (m%k == 0) RETURN
     469              : 
     470        15842 :       CALL cell_manifold_jacobian(q, J, info)
     471        15842 :       IF (info /= info_ok) RETURN
     472              : 
     473        38944 :       DO i = 1, m%n_slot_rows
     474        38944 :          A(i, m%slot_rows(i)) = 1.0_dp
     475              :       END DO
     476        27282 :       DO i = 1, m%n_geom_rows
     477        11440 :          row = m%n_slot_rows + i
     478        95922 :          A(row, :) = J(m%geom_rows(i), :)
     479              :       END DO
     480        20228 :       DO i = 1, m%n_lin_rows
     481         4386 :          row = m%n_slot_rows + m%n_geom_rows + i
     482        46544 :          A(row, :) = m%lin_w(:, i)
     483              :       END DO
     484        22852 :       DO i = 1, m%n_eq_rows
     485         7010 :          row = m%n_slot_rows + m%n_geom_rows + m%n_lin_rows + i
     486        64912 :          A(row, :) = J(m%eq_pair(1, i), :) - J(m%eq_pair(2, i), :)
     487              :       END DO
     488              : 
     489              :    END SUBROUTINE cell_manifold_active_jacobian
     490              : 
     491              : ! **************************************************************************************************
     492              : !> \brief Map reduced coordinates u to a canonical cell q(u) that satisfies r(q) = 0.
     493              : !> \param m constraint state. This routine updates only last_iter and last_resid.
     494              : !> \param u reduced coordinates of length m%ndof
     495              : !> \param q canonical cell vector on the constraint surface
     496              : !> \param info info_ok, info_degenerate, info_licq, or info_no_converge
     497              : !> \note For qt = q0 + Z (u - u0), Newton's method solves r(qt + Y w) = 0 for w.
     498              : !>       Linear constraints satisfy r(qt) = 0 in zero iterations. When k = 0, q
     499              : !>       copies u with zero roundoff.
     500              : ! **************************************************************************************************
     501        10950 :    SUBROUTINE cell_manifold_chart_eval(m, u, q, info)
     502              :       TYPE(cell_manifold_type), INTENT(INOUT)            :: m
     503              :       REAL(KIND=dp), INTENT(IN)                          :: u(m%ndof)
     504              :       REAL(KIND=dp), INTENT(OUT)                         :: q(n_slot)
     505              :       INTEGER, INTENT(OUT)                               :: info
     506              : 
     507              :       INTEGER                                            :: it, lapack_info
     508              :       REAL(KIND=dp)                                      :: qt(n_slot), rn_rel, s_chk(n_geom), &
     509        21900 :                                                             row_scales(m%k), row_norms(m%k), &
     510        21900 :                                                             r(m%k), dw(m%k), B(m%k, m%k), &
     511        21900 :                                                             A(m%k, n_slot)
     512        21900 :       INTEGER                                            :: ipiv(m%k)
     513              : 
     514              :       ! Separate checks avoid 0*Inf and NaN traps under -ffpe-trap.
     515        44780 :       IF (.NOT. ALL(ieee_is_finite(u))) THEN
     516           14 :          q = m%q0
     517            2 :          m%last_iter = 0
     518            2 :          m%last_resid = max_cell_coord
     519            2 :          info = info_no_converge
     520            2 :          RETURN
     521              :       END IF
     522        44768 :       IF (ANY(ABS(u) > max_cell_coord)) THEN
     523           14 :          q = m%q0
     524            2 :          m%last_iter = 0
     525            2 :          m%last_resid = max_cell_coord
     526            2 :          info = info_no_converge
     527            2 :          RETURN
     528              :       END IF
     529              : 
     530        10946 :       m%last_iter = 0
     531        10946 :       m%last_resid = 0.0_dp
     532        10946 :       info = info_ok
     533              : 
     534        10946 :       IF (m%ndof == 0) THEN
     535        13398 :          q = m%q0
     536              :          RETURN
     537              :       END IF
     538              : 
     539         9032 :       IF (m%k == 0) THEN
     540        20076 :          q = u
     541         2868 :          CALL cell_manifold_geometry(q, s_chk, info)
     542         2868 :          RETURN
     543              :       END IF
     544              : 
     545       182208 :       qt = m%q0 + MATMUL(m%Z(:, 1:m%ndof), u - m%u0(1:m%ndof))
     546         6164 :       row_scales = active_row_scales(m)
     547              : 
     548         6164 :       CALL cell_manifold_residual(m, qt, r, info)
     549         6164 :       IF (info /= info_ok) THEN
     550            0 :          q = qt
     551            0 :          m%last_resid = max_cell_coord
     552            0 :          RETURN
     553              :       END IF
     554        26536 :       m%last_resid = NORM2(r)
     555        26536 :       rn_rel = NORM2(r/row_scales)
     556              : 
     557        10964 :       DO it = 1, m%max_iter
     558        10962 :          IF (rn_rel <= m%tol) EXIT
     559         4800 :          CALL normalized_active_jacobian(m, qt, A, row_norms, info)
     560         4800 :          IF (info /= info_ok) EXIT
     561         4800 :          CALL factor_normal_block(m%k, A, m%Y(:, 1:m%k), B, ipiv, info)
     562         4800 :          IF (info /= info_ok) EXIT
     563        19014 :          dw = r/row_norms
     564         4800 :          CALL dgetrs('N', m%k, 1, B, m%k, ipiv, dw, m%k, lapack_info)
     565         4800 :          IF (lapack_info /= 0) THEN
     566            0 :             info = info_licq
     567            0 :             EXIT
     568              :          END IF
     569       133098 :          qt = qt - MATMUL(m%Y(:, 1:m%k), dw)
     570         4800 :          m%last_iter = it
     571         4800 :          CALL cell_manifold_residual(m, qt, r, info)
     572         4800 :          IF (info /= info_ok) THEN
     573            0 :             m%last_resid = max_cell_coord
     574            0 :             EXIT
     575              :          END IF
     576        19014 :          m%last_resid = NORM2(r)
     577        19014 :          rn_rel = NORM2(r/row_scales)
     578        14402 :          IF (.NOT. ieee_is_finite(m%last_resid) .OR. .NOT. ieee_is_finite(rn_rel)) THEN
     579            0 :             info = info_no_converge
     580            0 :             EXIT
     581              :          END IF
     582              :       END DO
     583         6164 :       q = qt
     584         6164 :       IF (info == info_ok .AND. rn_rel > m%tol) info = info_no_converge
     585              : 
     586         6164 :    END SUBROUTINE cell_manifold_chart_eval
     587              : 
     588              : ! **************************************************************************************************
     589              : !> \brief Map a canonical cell q back to reduced coordinates u = u0 + Z^T (q - q0).
     590              : !> \param m constraint state
     591              : !> \param q canonical cell vector
     592              : !> \param u reduced coordinates of length m%ndof
     593              : !> \note Because Z^T Z = I and Z^T Y = 0, this affine projection is the exact
     594              : !>       inverse of cell_manifold_chart_eval. When k = 0, u is a direct copy of q.
     595              : ! **************************************************************************************************
     596          914 :    SUBROUTINE cell_manifold_chart_inv(m, q, u)
     597              :       TYPE(cell_manifold_type), INTENT(IN)               :: m
     598              :       REAL(KIND=dp), INTENT(IN)                          :: q(n_slot)
     599              :       REAL(KIND=dp), INTENT(OUT)                         :: u(m%ndof)
     600              : 
     601         1058 :       IF (m%ndof == 0) RETURN
     602          910 :       IF (m%k == 0) THEN
     603         1008 :          u = q
     604              :          RETURN
     605              :       END IF
     606         7740 :       u = m%u0(1:m%ndof) + MATMUL(TRANSPOSE(m%Z(:, 1:m%ndof)), q - m%q0)
     607              : 
     608              :    END SUBROUTINE cell_manifold_chart_inv
     609              : 
     610              : ! **************************************************************************************************
     611              : !> \brief Transform a 6-component cell gradient g_q into the reduced gradient g_u = (dq/du)^T g_q.
     612              : !> \param m constraint state
     613              : !> \param q current canonical cell vector
     614              : !> \param g_q enthalpy gradient +dH/dq in canonical cell components
     615              : !> \param g_u enthalpy gradient +dH/du in reduced coordinates
     616              : !> \param info info_ok, info_degenerate, or info_licq
     617              : !> \note Implicit differentiation of r(q(u)) = 0 gives dq/du = Z - Y (A Y)^{-1} A Z.
     618              : !>       Thus g_u = Z^T (g_q - A^T lam), where (A Y)^T lam = Y^T g_q.
     619              : ! **************************************************************************************************
     620         6015 :    SUBROUTINE cell_manifold_chart_pullback_grad(m, q, g_q, g_u, info)
     621              :       TYPE(cell_manifold_type), INTENT(IN)               :: m
     622              :       REAL(KIND=dp), INTENT(IN)                          :: q(n_slot), g_q(n_slot)
     623              :       REAL(KIND=dp), INTENT(OUT)                         :: g_u(m%ndof)
     624              :       INTEGER, INTENT(OUT)                               :: info
     625              : 
     626              :       INTEGER                                            :: lapack_info
     627        12030 :       REAL(KIND=dp)                                      :: row_norms(m%k), lam(m%k), B(m%k, m%k), &
     628        12030 :                                                             A(m%k, n_slot)
     629         8887 :       INTEGER                                            :: ipiv(m%k)
     630              : 
     631        26245 :       g_u = 0.0_dp
     632         6015 :       info = info_ok
     633         8088 :       IF (m%ndof == 0) RETURN
     634              : 
     635         4945 :       IF (m%k == 0) THEN
     636        14511 :          g_u = g_q
     637              :          RETURN
     638              :       END IF
     639              : 
     640         2872 :       CALL normalized_active_jacobian(m, q, A, row_norms, info)
     641         2872 :       IF (info /= info_ok) RETURN
     642              : 
     643         2872 :       CALL factor_normal_block(m%k, A, m%Y(:, 1:m%k), B, ipiv, info)
     644         2872 :       IF (info /= info_ok) RETURN
     645              : 
     646         2872 :       lam = MATMUL(TRANSPOSE(m%Y(:, 1:m%k)), g_q)
     647         2872 :       CALL dgetrs('T', m%k, 1, B, m%k, ipiv, lam, m%k, lapack_info)
     648         2872 :       IF (lapack_info /= 0) THEN
     649            0 :          info = info_licq
     650            0 :          RETURN
     651              :       END IF
     652        20104 :       g_u = MATMUL(TRANSPOSE(m%Z(:, 1:m%ndof)), g_q - MATMUL(TRANSPOSE(A), lam))
     653              : 
     654         2872 :    END SUBROUTINE cell_manifold_chart_pullback_grad
     655              : 
     656              : ! **************************************************************************************************
     657              : !> \brief Compute the pressure residual P_constr - pres_ext along unconstrained cell directions.
     658              : !> \param m constraint state
     659              : !> \param q current canonical cell components
     660              : !> \param g_phys enthalpy gradient +d(E + pres_ext*V)/dq from get_dg_dh
     661              : !> \param pdiff residual pressure P_constr - pres_ext along the allowed volume direction
     662              : !> \param projected .TRUE. if free directions change volume, .FALSE. if volume is fixed
     663              : !> \param info info_ok, info_degenerate, or info_licq
     664              : !> \note On curved constraints, ker(A(q)) rotates as q moves from q0. This routine
     665              : !>       projects dV/dq onto ker(A(q)) at the current cell q:
     666              : !>       vt = dV/dq - A(q)^T (A(q) A(q)^T)^{-1} A(q) (dV/dq).
     667              : !>       At any constrained minimum, g_phys lies in row(A(q)) and pdiff vanishes.
     668              : ! **************************************************************************************************
     669         6021 :    SUBROUTINE cell_manifold_proj_pressure(m, q, g_phys, pdiff, projected, info)
     670              :       TYPE(cell_manifold_type), INTENT(IN)               :: m
     671              :       REAL(KIND=dp), INTENT(IN)                          :: q(n_slot), g_phys(n_slot)
     672              :       REAL(KIND=dp), INTENT(OUT)                         :: pdiff
     673              :       LOGICAL, INTENT(OUT)                               :: projected
     674              :       INTEGER, INTENT(OUT)                               :: info
     675              : 
     676              :       INTEGER                                            :: lapack_info
     677              :       REAL(KIND=dp) :: ax, by, cz, denom, dv_dq(n_slot), dv_norm2, s_chk(n_geom), vt(n_slot), &
     678         2029 :          row_norms(m%k), mu(m%k), chol(m%k, m%k), A(m%k, n_slot)
     679              : 
     680         6021 :       pdiff = 0.0_dp
     681         6021 :       projected = .FALSE.
     682         6021 :       info = info_ok
     683         6021 :       IF (m%ndof == 0) RETURN
     684              : 
     685         4951 :       CALL cell_manifold_geometry(q, s_chk, info)
     686         4951 :       IF (info /= info_ok) RETURN
     687              : 
     688         4949 :       ax = q(slot_ax)
     689         4949 :       by = q(slot_by)
     690         4949 :       cz = q(slot_cz)
     691        34643 :       dv_dq = [by*cz, 0.0_dp, ax*cz, 0.0_dp, 0.0_dp, ax*by]
     692        34643 :       dv_norm2 = DOT_PRODUCT(dv_dq, dv_dq)
     693         4949 :       vt = dv_dq
     694              : 
     695         4949 :       IF (m%k > 0) THEN
     696         2874 :          CALL normalized_active_jacobian(m, q, A, row_norms, info)
     697         2874 :          IF (info /= info_ok) RETURN
     698       317260 :          chol = MATMUL(A, TRANSPOSE(A))
     699         2874 :          CALL dpotrf('U', m%k, chol, m%k, lapack_info)
     700         2874 :          IF (lapack_info /= 0) THEN
     701            0 :             info = info_licq
     702            0 :             RETURN
     703              :          END IF
     704        86184 :          mu = MATMUL(A, dv_dq)
     705         2874 :          CALL dpotrs('U', m%k, 1, chol, m%k, mu, m%k, lapack_info)
     706         2874 :          IF (lapack_info /= 0) THEN
     707            0 :             info = info_licq
     708            0 :             RETURN
     709              :          END IF
     710        25866 :          vt = dv_dq - MATMUL(TRANSPOSE(A), mu)
     711              :       END IF
     712              : 
     713        34643 :       denom = DOT_PRODUCT(vt, vt)
     714         4949 :       IF (denom <= proj_pressure_rel_tol*dv_norm2) RETURN
     715              : 
     716         3992 :       projected = .TRUE.
     717              :       ! For isotropic stress g_phys = -(P_int - pres_ext) * dV/dq, so the minus
     718              :       ! sign yields pdiff = P_constr - pres_ext.
     719        27944 :       pdiff = -DOT_PRODUCT(g_phys, vt)/denom
     720              : 
     721              :    END SUBROUTINE cell_manifold_proj_pressure
     722              : 
     723              : ! **************************************************************************************************
     724              : !> \brief Pack the upper-triangular canonical cell matrix into q(1:6).
     725              : !> \param hmat canonical 3x3 cell matrix (a along x, b in the xy plane)
     726              : !> \return q = (AX, BX, BY, CX, CY, CZ)
     727              : ! **************************************************************************************************
     728         6937 :    PURE FUNCTION cell_manifold_pack_hmat(hmat) RESULT(q)
     729              :       REAL(KIND=dp), DIMENSION(3, 3), INTENT(IN)         :: hmat
     730              :       REAL(KIND=dp), DIMENSION(n_slot)                   :: q
     731              : 
     732        48559 :       q = [hmat(1, 1), hmat(1, 2), hmat(2, 2), hmat(1, 3), hmat(2, 3), hmat(3, 3)]
     733              : 
     734         6937 :    END FUNCTION cell_manifold_pack_hmat
     735              : 
     736              : ! **************************************************************************************************
     737              : !> \brief Write q(1:6) into the upper-triangular canonical cell matrix.
     738              : !> \param q canonical cell vector (AX, BX, BY, CX, CY, CZ)
     739              : !> \param hmat canonical 3x3 cell matrix with zeroed lower triangle
     740              : ! **************************************************************************************************
     741        10056 :    PURE SUBROUTINE cell_manifold_unpack_hmat(q, hmat)
     742              :       REAL(KIND=dp), DIMENSION(n_slot), INTENT(IN)       :: q
     743              :       REAL(KIND=dp), DIMENSION(3, 3), INTENT(OUT)        :: hmat
     744              : 
     745        10056 :       hmat = 0.0_dp
     746        10056 :       hmat(1, 1) = q(slot_ax)
     747        10056 :       hmat(1, 2) = q(slot_bx)
     748        10056 :       hmat(2, 2) = q(slot_by)
     749        10056 :       hmat(1, 3) = q(slot_cx)
     750        10056 :       hmat(2, 3) = q(slot_cy)
     751        10056 :       hmat(3, 3) = q(slot_cz)
     752              : 
     753        10056 :    END SUBROUTINE cell_manifold_unpack_hmat
     754              : 
     755              : ! **************************************************************************************************
     756              : !> \brief Return a short description of a cell_manifold_utils status code.
     757              : !> \param info status code from a cell_manifold_* routine
     758              : !> \return diagnostic text
     759              : ! **************************************************************************************************
     760           12 :    PURE FUNCTION cell_manifold_status_label(info) RESULT(label)
     761              :       INTEGER, INTENT(IN)                                :: info
     762              :       CHARACTER(LEN=default_string_length)               :: label
     763              : 
     764           14 :       SELECT CASE (info)
     765              :       CASE (info_ok)
     766            2 :          label = "ok"
     767              :       CASE (info_degenerate)
     768            2 :          label = "degenerate or non-positive cell"
     769              :       CASE (info_licq)
     770            2 :          label = "cell constraints are linearly dependent or ill-conditioned"
     771              :       CASE (info_no_converge)
     772            2 :          label = "nonlinear cell constraint solver did not converge"
     773              :       CASE (info_bad_input)
     774            2 :          label = "invalid constraint mask or row specification"
     775              :       CASE DEFAULT
     776           12 :          label = "unknown cell-constraint status"
     777              :       END SELECT
     778              : 
     779           12 :    END FUNCTION cell_manifold_status_label
     780              : 
     781              : ! **************************************************************************************************
     782              : !> \brief Test whether one candidate constraint stays zero on manifold m near q0.
     783              : !> \param m active constraint manifold anchored at q0
     784              : !> \param mask candidate comp_* or gfix_* bit, or 0 for a lin_w or eq_pair test
     785              : !> \param lin_w optional 6 x 1 column of candidate linear weights
     786              : !> \param eq_pair optional 2 x 1 index pair for a candidate equality
     787              : !> \return .TRUE. if the candidate residual stays zero along m, or .FALSE. otherwise
     788              : ! **************************************************************************************************
     789          108 :    LOGICAL FUNCTION cell_manifold_is_redundant(m, mask, lin_w, eq_pair) RESULT(is_redundant)
     790              :       TYPE(cell_manifold_type), INTENT(IN)               :: m
     791              :       INTEGER, INTENT(IN)                                :: mask
     792              :       REAL(KIND=dp), INTENT(IN), OPTIONAL                :: lin_w(:, :)
     793              :       INTEGER, INTENT(IN), OPTIONAL                      :: eq_pair(:, :)
     794              : 
     795              :       REAL(KIND=dp), PARAMETER                           :: inv_sqrt2 = SQRT(0.5_dp)
     796              : 
     797              :       INTEGER                                            :: i, i_probe, info, j, n_probes
     798              :       REAL(KIND=dp) :: anchor_limit, cand_scales(1), curv_limit, drift_limit, q_probe(n_slot), &
     799              :          r0(1), r_neg(1), r_pos(1), r_snap(1), rel_norm, step_u, z_dir(n_slot)
     800              :       TYPE(cell_manifold_type)                           :: m_cand, m_work
     801          216 :       REAL(KIND=dp)                                      :: u_probe(m%ndof), &
     802          216 :                                                             u_dirs(m%ndof, m%ndof*m%ndof)
     803              : 
     804          108 :       is_redundant = .FALSE.
     805          298 :       CALL cell_manifold_init(m_cand, m%q0, mask, info, lin_w=lin_w, eq_pair=eq_pair)
     806          108 :       IF (info /= info_ok .OR. m_cand%k /= 1) RETURN
     807              : 
     808          108 :       cand_scales = active_row_scales(m_cand)
     809          108 :       anchor_limit = redundancy_rel_tol*cand_scales(1)
     810          108 :       drift_limit = redundancy_drift_rel_tol*cand_scales(1)
     811          108 :       curv_limit = redundancy_curv_rel_tol*cand_scales(1)
     812              : 
     813          108 :       CALL cell_manifold_residual(m_cand, m%q0, r0, info)
     814          108 :       IF (info /= info_ok .OR. ABS(r0(1)) > anchor_limit) RETURN
     815              : 
     816          106 :       IF (m%ndof == 0) THEN
     817          108 :          is_redundant = .TRUE.
     818              :          RETURN
     819              :       END IF
     820              : 
     821           96 :       m_work = m
     822           96 :       m_work%tol = MIN(m%tol, redundancy_chart_tol)
     823              : 
     824              :       ! Project u0 onto m_work first so input roundoff at q0 does not appear as
     825              :       ! drift or curvature along m.
     826           96 :       CALL cell_manifold_chart_eval(m_work, m%u0(1:m%ndof), q_probe, info)
     827           96 :       IF (info /= info_ok) RETURN
     828           96 :       CALL cell_manifold_residual(m_cand, q_probe, r_snap, info)
     829           96 :       IF (info /= info_ok .OR. ABS(r_snap(1)) > anchor_limit) RETURN
     830              : 
     831              :       n_probes = m%ndof*m%ndof
     832         1756 :       u_dirs = 0.0_dp
     833              :       i_probe = 0
     834          282 :       DO i = 1, m%ndof
     835          186 :          i_probe = i_probe + 1
     836          186 :          u_dirs(i, i_probe) = 1.0_dp
     837          410 :          DO j = i + 1, m%ndof
     838          128 :             i_probe = i_probe + 1
     839          128 :             u_dirs(i, i_probe) = inv_sqrt2
     840          128 :             u_dirs(j, i_probe) = inv_sqrt2
     841          128 :             i_probe = i_probe + 1
     842          128 :             u_dirs(i, i_probe) = inv_sqrt2
     843          314 :             u_dirs(j, i_probe) = -inv_sqrt2
     844              :          END DO
     845              :       END DO
     846              : 
     847          458 :       DO i_probe = 1, n_probes
     848         7558 :          z_dir = MATMUL(m%Z(:, 1:m%ndof), u_dirs(:, i_probe))
     849              :          rel_norm = NORM2([z_dir(slot_ax)/m_cand%s0(geom_a), &
     850              :                            z_dir(slot_bx)/m_cand%s0(geom_b), &
     851              :                            z_dir(slot_by)/m_cand%s0(geom_b), &
     852              :                            z_dir(slot_cx)/m_cand%s0(geom_c), &
     853              :                            z_dir(slot_cy)/m_cand%s0(geom_c), &
     854         2632 :                            z_dir(slot_cz)/m_cand%s0(geom_c)])
     855          376 :          step_u = redundancy_probe_frac/rel_norm
     856         1402 :          u_probe = m%u0(1:m%ndof) + step_u*u_dirs(:, i_probe)
     857          376 :          CALL cell_manifold_chart_eval(m_work, u_probe, q_probe, info)
     858          376 :          IF (info /= info_ok) RETURN
     859          376 :          CALL cell_manifold_residual(m_cand, q_probe, r_pos, info)
     860          376 :          IF (info /= info_ok) RETURN
     861         1402 :          u_probe = m%u0(1:m%ndof) - step_u*u_dirs(:, i_probe)
     862          376 :          CALL cell_manifold_chart_eval(m_work, u_probe, q_probe, info)
     863          376 :          IF (info /= info_ok) RETURN
     864          376 :          CALL cell_manifold_residual(m_cand, q_probe, r_neg, info)
     865          376 :          IF (info /= info_ok) RETURN
     866          376 :          IF (MAX(ABS(r_pos(1) - r_snap(1)), ABS(r_neg(1) - r_snap(1))) > drift_limit) RETURN
     867         1950 :          IF (ABS(r_pos(1) + r_neg(1) - 2.0_dp*r_snap(1)) > curv_limit) RETURN
     868              :       END DO
     869              : 
     870          108 :       is_redundant = .TRUE.
     871              : 
     872        38772 :    END FUNCTION cell_manifold_is_redundant
     873              : 
     874              : ! **************************************************************************************************
     875              : !> \brief Pin implied directions when a candidate constraint is semidefinite at q0.
     876              : !> \param m active constraint manifold at q0. This routine updates m when is_pinned is .TRUE.
     877              : !> \param mask candidate comp_* or gfix_* bit, or 0 for a lin_w or eq_pair test
     878              : !> \param is_pinned .TRUE. if this routine pins the candidate constraint
     879              : !> \param lin_w optional 6 x 1 column of candidate linear weights
     880              : !> \param eq_pair optional 2 x 1 index pair for a candidate equality
     881              : !> \note This routine builds the 6 x 6 Lagrangian Hessian at q0 from 4-point
     882              : !>       central differences of analytic Jacobians. When the reduced Hessian on
     883              : !>       ker(A(q0)) is nonzero and semidefinite, this routine pins the curved
     884              : !>       directions. It then verifies that the candidate stays zero.
     885              : ! **************************************************************************************************
     886           94 :    SUBROUTINE cell_manifold_pin_semidefinite(m, mask, is_pinned, lin_w, eq_pair)
     887              :       TYPE(cell_manifold_type), INTENT(INOUT)            :: m
     888              :       INTEGER, INTENT(IN)                                :: mask
     889              :       LOGICAL, INTENT(OUT)                               :: is_pinned
     890              :       REAL(KIND=dp), INTENT(IN), OPTIONAL                :: lin_w(:, :)
     891              :       INTEGER, INTENT(IN), OPTIONAL                      :: eq_pair(:, :)
     892              : 
     893              :       INTEGER                                            :: col, i, info, j, lapack_info, lwork, &
     894              :                                                             mask_new, max_slot, n_lin_basis, &
     895              :                                                             n_lin_new, n_pin, ndof, slot
     896              :       LOGICAL                                            :: ok, slot_fixed(n_slot)
     897              :       REAL(KIND=dp) :: cand_grad(n_slot), cand_norm, cand_scales(1), eig_thresh, g_tilde(n_slot), &
     898              :          H_tilde(n_slot, n_slot), lag_grad(n_slot), lin_basis(n_slot, n_slot), lin_col(n_slot), &
     899              :          lin_w_new(n_slot, n_slot), max_abs_eig, min_pin_norm, mode_vec(n_slot), pin_col(n_slot), &
     900              :          pin_norm, r0(1), slot_scales(n_slot), vec_tol, w_col(n_slot), w_norm, work_query(1), &
     901              :          z_scaled(n_slot)
     902           94 :       REAL(KIND=dp), ALLOCATABLE                         :: work(:)
     903              :       TYPE(cell_manifold_type)                           :: m_cand, m_pinned
     904          282 :       REAL(KIND=dp) :: Z_tilde(n_slot, m%ndof), W_pin(n_slot, m%ndof), H_red(m%ndof, m%ndof), &
     905           94 :          eigvecs(m%ndof, m%ndof), eigvals(m%ndof)
     906              : 
     907           94 :       is_pinned = .FALSE.
     908           94 :       ndof = m%ndof
     909           60 :       IF (ndof <= 0) RETURN
     910           94 :       IF (mask /= 0 .AND. IAND(m%mask, mask) /= 0) RETURN
     911              : 
     912          260 :       CALL cell_manifold_init(m_cand, m%q0, mask, info, lin_w=lin_w, eq_pair=eq_pair)
     913           94 :       IF (info /= info_ok .OR. m_cand%k /= 1) RETURN
     914              : 
     915           94 :       cand_scales = active_row_scales(m_cand)
     916           94 :       CALL cell_manifold_residual(m_cand, m%q0, r0, info)
     917           94 :       IF (info /= info_ok .OR. ABS(r0(1)) > redundancy_rel_tol*cand_scales(1)) RETURN
     918              : 
     919              :       slot_scales = [m_cand%s0(geom_a), m_cand%s0(geom_b), m_cand%s0(geom_b), &
     920          644 :                      m_cand%s0(geom_c), m_cand%s0(geom_c), m_cand%s0(geom_c)]
     921              :       CALL lagrangian_hessian_at_q0(m, m_cand, cand_scales(1), slot_scales, &
     922           92 :                                     cand_grad, lag_grad, g_tilde, H_tilde, ok)
     923           92 :       IF (.NOT. ok) RETURN
     924          630 :       cand_norm = NORM2(cand_grad)
     925              : 
     926          288 :       DO j = 1, ndof
     927         1386 :          z_scaled = m%Z(:, j)/slot_scales
     928         2664 :          Z_tilde(:, j) = z_scaled/NORM2(z_scaled)
     929              :       END DO
     930        16152 :       H_red = MATMUL(TRANSPOSE(Z_tilde), MATMUL(H_tilde, Z_tilde))
     931         1482 :       H_red = 0.5_dp*(H_red + TRANSPOSE(H_red))
     932          786 :       IF (.NOT. ALL(ieee_is_finite(H_red))) RETURN
     933              : 
     934          786 :       eigvecs = H_red
     935           90 :       CALL dsyev('V', 'U', ndof, eigvecs, ndof, eigvals, work_query, -1, lapack_info)
     936           90 :       IF (lapack_info /= 0) RETURN
     937           90 :       lwork = INT(work_query(1))
     938          270 :       ALLOCATE (work(MAX(1, lwork)))
     939           90 :       CALL dsyev('V', 'U', ndof, eigvecs, ndof, eigvals, work, SIZE(work), lapack_info)
     940           90 :       DEALLOCATE (work)
     941           90 :       IF (lapack_info /= 0) RETURN
     942              : 
     943          288 :       max_abs_eig = MAXVAL(ABS(eigvals))
     944           90 :       IF (max_abs_eig <= semidef_min_curv) RETURN
     945           38 :       eig_thresh = semidef_eig_rel_tol*max_abs_eig
     946          210 :       IF (MAXVAL(eigvals) > eig_thresh .AND. MINVAL(eigvals) < -eig_thresh) RETURN
     947              : 
     948           36 :       slot_fixed = .FALSE.
     949          134 :       DO i = 1, m%n_slot_rows
     950          134 :          slot_fixed(m%slot_rows(i)) = .TRUE.
     951              :       END DO
     952           36 :       n_lin_basis = 0
     953           36 :       lin_basis = 0.0_dp
     954           40 :       DO i = 1, m%n_lin_rows
     955           28 :          lin_col = m%lin_w(:, i)*slot_scales
     956           28 :          WHERE (slot_fixed) lin_col = 0.0_dp
     957            6 :          DO j = 1, n_lin_basis
     958           30 :             lin_col = lin_col - DOT_PRODUCT(lin_basis(:, j), lin_col)*lin_basis(:, j)
     959              :          END DO
     960           28 :          w_norm = NORM2(lin_col)
     961           40 :          IF (w_norm > semidef_vec_zero_tol) THEN
     962            4 :             n_lin_basis = n_lin_basis + 1
     963           28 :             lin_basis(:, n_lin_basis) = lin_col/w_norm
     964              :          END IF
     965              :       END DO
     966              : 
     967              :       ! Project out only fixed slots and linear rows of m so curved active rows
     968              :       ! cannot tilt a pure coordinate pin.
     969           36 :       n_pin = 0
     970           36 :       min_pin_norm = HUGE(1.0_dp)
     971          114 :       DO j = 1, ndof
     972          114 :          IF (ABS(eigvals(j)) > eig_thresh) THEN
     973           44 :             n_pin = n_pin + 1
     974          772 :             mode_vec = MATMUL(Z_tilde, eigvecs(:, j))
     975          308 :             IF (ABS(DOT_PRODUCT(mode_vec, g_tilde)) > semidef_vertex_rel_tol*ABS(eigvals(j))) RETURN
     976         1806 :             pin_col = MATMUL(H_tilde, mode_vec)
     977          294 :             WHERE (slot_fixed) pin_col = 0.0_dp
     978           42 :             DO i = 1, n_lin_basis
     979           42 :                pin_col = pin_col - DOT_PRODUCT(lin_basis(:, i), pin_col)*lin_basis(:, i)
     980              :             END DO
     981          294 :             pin_norm = NORM2(pin_col)
     982           42 :             IF (pin_norm <= TINY(1.0_dp)) RETURN
     983           42 :             min_pin_norm = MIN(min_pin_norm, pin_norm)
     984          294 :             W_pin(:, n_pin) = pin_col/pin_norm
     985              :          END IF
     986              :       END DO
     987           34 :       IF (n_pin == 0) RETURN
     988           34 :       vec_tol = MAX(semidef_vec_zero_tol, semidef_fd_noise_floor/min_pin_norm)
     989           34 :       IF (vec_tol > semidef_max_vec_tol) RETURN
     990              : 
     991           34 :       CALL echelon_pin_columns(W_pin(:, 1:n_pin), n_pin, vec_tol, ok)
     992           34 :       IF (.NOT. ok) RETURN
     993              : 
     994           34 :       mask_new = m%mask
     995         1462 :       lin_w_new = m%lin_w
     996              :       n_lin_new = m%n_lin_rows
     997           76 :       DO col = 1, n_pin
     998          294 :          pin_col = W_pin(:, col)
     999          546 :          WHERE (ABS(pin_col) <= vec_tol*MAXVAL(ABS(pin_col))) pin_col = 0.0_dp
    1000          294 :          w_col = pin_col/slot_scales
    1001          294 :          w_norm = NORM2(w_col)
    1002           42 :          IF (w_norm <= TINY(1.0_dp)) RETURN
    1003          294 :          w_col = w_col/w_norm
    1004          328 :          IF (COUNT(w_col /= 0.0_dp) == 1) THEN
    1005          280 :             slot = MAXLOC(ABS(w_col), DIM=1)
    1006           40 :             mask_new = IOR(mask_new, comp_flags(slot))
    1007              :          ELSE
    1008           14 :             max_slot = MAXLOC(ABS(w_col), DIM=1)
    1009            2 :             IF (w_col(max_slot) < 0.0_dp) w_col = -w_col
    1010            2 :             n_lin_new = n_lin_new + 1
    1011            2 :             IF (n_lin_new > n_slot) RETURN
    1012           14 :             lin_w_new(:, n_lin_new) = w_col
    1013              :          END IF
    1014              :       END DO
    1015              : 
    1016              :       CALL cell_manifold_init(m_pinned, m%q0, mask_new, info, &
    1017              :                               lin_w=lin_w_new(:, 1:n_lin_new), &
    1018           34 :                               eq_pair=m%eq_pair(:, 1:m%n_eq_rows))
    1019           34 :       IF (info /= info_ok .OR. m_pinned%ndof /= ndof - n_pin) RETURN
    1020              : 
    1021           34 :       IF (m_pinned%ndof > 0) THEN
    1022           60 :          IF (NORM2(MATMUL(TRANSPOSE(m_pinned%Z(:, 1:m_pinned%ndof)), lag_grad)) > &
    1023           52 :              redundancy_chart_tol*cand_norm) RETURN
    1024              :       END IF
    1025              : 
    1026           92 :       IF (.NOT. cell_manifold_is_redundant(m_pinned, mask, lin_w=lin_w, eq_pair=eq_pair)) RETURN
    1027              : 
    1028           34 :       m = m_pinned
    1029           34 :       is_pinned = .TRUE.
    1030              : 
    1031        33838 :    END SUBROUTINE cell_manifold_pin_semidefinite
    1032              : 
    1033              : ! **************************************************************************************************
    1034              : !> \brief Return positive reference scales for each active residual component of m.
    1035              : !> \param m initialized constraint state (m%k > 0)
    1036              : !> \return per-row reference magnitudes in the row order of cell_manifold_residual
    1037              : ! **************************************************************************************************
    1038         6366 :    PURE FUNCTION active_row_scales(m) RESULT(scales)
    1039              :       TYPE(cell_manifold_type), INTENT(IN)               :: m
    1040              :       REAL(KIND=dp)                                      :: scales(m%k)
    1041              : 
    1042              :       INTEGER                                            :: g, i, p1, p2, row
    1043              :       REAL(KIND=dp)                                      :: coord_scale
    1044              : 
    1045        44562 :       coord_scale = MAXVAL(ABS(m%q0))
    1046        18592 :       DO i = 1, m%n_slot_rows
    1047        18592 :          scales(i) = coord_scale
    1048              :       END DO
    1049         9596 :       DO i = 1, m%n_geom_rows
    1050         3230 :          row = m%n_slot_rows + i
    1051         3230 :          g = m%geom_rows(i)
    1052         9596 :          IF (g >= geom_alpha .AND. g <= geom_gamma) THEN
    1053         1456 :             scales(row) = 1.0_dp
    1054              :          ELSE
    1055         1774 :             scales(row) = ABS(m%s0(g))
    1056              :          END IF
    1057              :       END DO
    1058         9730 :       DO i = 1, m%n_lin_rows
    1059         3364 :          row = m%n_slot_rows + m%n_geom_rows + i
    1060         9730 :          scales(row) = coord_scale
    1061              :       END DO
    1062         8120 :       DO i = 1, m%n_eq_rows
    1063         1754 :          row = m%n_slot_rows + m%n_geom_rows + m%n_lin_rows + i
    1064         1754 :          p1 = m%eq_pair(1, i)
    1065         1754 :          p2 = m%eq_pair(2, i)
    1066         8120 :          IF (p1 >= geom_alpha .AND. p2 >= geom_alpha) THEN
    1067          702 :             scales(row) = 1.0_dp
    1068              :          ELSE
    1069         1052 :             scales(row) = MAX(ABS(m%s0(p1)), ABS(m%s0(p2)))
    1070              :          END IF
    1071              :       END DO
    1072              : 
    1073         6366 :    END FUNCTION active_row_scales
    1074              : 
    1075              : ! **************************************************************************************************
    1076              : !> \brief Evaluate the dimensionless Lagrangian gradient and 6 x 6 Hessian at q0.
    1077              : !> \param m active constraint manifold at q0
    1078              : !> \param m_cand single-row manifold for the candidate constraint at q0
    1079              : !> \param cand_scale reference residual scale of m_cand
    1080              : !> \param slot_scales per-slot length scales (a, b, b, c, c, c) at q0
    1081              : !> \param cand_grad candidate row gradient dr_cand/dq at q0
    1082              : !> \param lag_grad Lagrangian stationarity gradient at q0
    1083              : !> \param g_tilde dimensionless Lagrangian gradient at q0
    1084              : !> \param H_tilde dimensionless 6 x 6 symmetric Hessian of the Lagrangian at q0
    1085              : !> \param ok .TRUE. when the candidate is first-order tangent and all evaluations succeed
    1086              : ! **************************************************************************************************
    1087           92 :    SUBROUTINE lagrangian_hessian_at_q0(m, m_cand, cand_scale, slot_scales, &
    1088              :                                        cand_grad, lag_grad, g_tilde, H_tilde, ok)
    1089              :       TYPE(cell_manifold_type), INTENT(IN)               :: m, m_cand
    1090              :       REAL(KIND=dp), INTENT(IN)                          :: cand_scale, slot_scales(n_slot)
    1091              :       REAL(KIND=dp), INTENT(OUT)                         :: cand_grad(n_slot), lag_grad(n_slot), &
    1092              :                                                             g_tilde(n_slot), &
    1093              :                                                             H_tilde(n_slot, n_slot)
    1094              :       LOGICAL, INTENT(OUT)                               :: ok
    1095              : 
    1096              :       REAL(KIND=dp), PARAMETER :: richardson_w1 = 8.0_dp/12.0_dp, richardson_w2 = 1.0_dp/12.0_dp, &
    1097              :          stencil_offsets(4) = [1.0_dp, -1.0_dp, 2.0_dp, -2.0_dp]
    1098              : 
    1099              :       INTEGER                                            :: i_pt, i_try, info, lapack_info, slot
    1100              :       LOGICAL                                            :: stencil_ok
    1101              :       REAL(KIND=dp) :: A_cand(1, n_slot), cand_norm, dg_dq(n_slot), q_pert(n_slot), &
    1102          184 :          stencil_grad(n_slot, 4), step_q, row_norms(m%k), lam_raw(m%k), lam(m%k), B(m%k, m%k), &
    1103          184 :          A_raw(m%k, n_slot), A_norm(m%k, n_slot)
    1104          184 :       INTEGER                                            :: ipiv(m%k)
    1105              : 
    1106           92 :       cand_grad = 0.0_dp
    1107           92 :       lag_grad = 0.0_dp
    1108           92 :       g_tilde = 0.0_dp
    1109           92 :       H_tilde = 0.0_dp
    1110           92 :       ok = .FALSE.
    1111              : 
    1112           92 :       CALL cell_manifold_active_jacobian(m_cand, m%q0, A_cand, info)
    1113           94 :       IF (info /= info_ok) RETURN
    1114           92 :       cand_grad = A_cand(1, :)
    1115          644 :       cand_norm = NORM2(cand_grad)
    1116           92 :       IF (cand_norm <= TINY(1.0_dp)) RETURN
    1117          294 :       IF (NORM2(MATMUL(TRANSPOSE(m%Z(:, 1:m%ndof)), cand_grad)) > licq_pivot_tol*cand_norm) RETURN
    1118              : 
    1119           90 :       lag_grad = cand_grad
    1120           90 :       IF (m%k > 0) THEN
    1121           90 :          CALL normalized_active_jacobian(m, m%q0, A_norm, row_norms, info)
    1122           90 :          IF (info /= info_ok) RETURN
    1123           90 :          CALL factor_normal_block(m%k, A_norm, m%Y(:, 1:m%k), B, ipiv, info)
    1124           90 :          IF (info /= info_ok) RETURN
    1125           90 :          lam = MATMUL(TRANSPOSE(m%Y(:, 1:m%k)), cand_grad)
    1126           90 :          CALL dgetrs('T', m%k, 1, B, m%k, ipiv, lam, m%k, lapack_info)
    1127           90 :          IF (lapack_info /= 0) RETURN
    1128          432 :          WHERE (ABS(lam) <= semidef_lam_rel_tol*cand_norm) lam = 0.0_dp
    1129          432 :          lam_raw = lam/row_norms
    1130          720 :          lag_grad = cand_grad - MATMUL(TRANSPOSE(A_norm), lam)
    1131              :       END IF
    1132              : 
    1133          630 :       g_tilde = (slot_scales/cand_scale)*lag_grad
    1134              : 
    1135          630 :       DO slot = 1, n_slot
    1136          540 :          step_q = semidef_fd_step*slot_scales(slot)
    1137          540 :          stencil_ok = .FALSE.
    1138          540 :          DO i_try = 1, semidef_fd_max_tries
    1139         2700 :             stencil_ok = .TRUE.
    1140         2700 :             DO i_pt = 1, SIZE(stencil_offsets)
    1141        15120 :                q_pert = m%q0
    1142         2160 :                q_pert(slot) = q_pert(slot) + stencil_offsets(i_pt)*step_q
    1143         2160 :                CALL cell_manifold_active_jacobian(m_cand, q_pert, A_cand, info)
    1144         2160 :                IF (info /= info_ok) THEN
    1145              :                   stencil_ok = .FALSE.
    1146              :                   EXIT
    1147              :                END IF
    1148        15120 :                stencil_grad(:, i_pt) = A_cand(1, :)
    1149         4860 :                IF (m%k > 0) THEN
    1150         2160 :                   CALL cell_manifold_active_jacobian(m, q_pert, A_raw, info)
    1151         2160 :                   IF (info /= info_ok) THEN
    1152              :                      stencil_ok = .FALSE.
    1153            0 :                      EXIT
    1154              :                   END IF
    1155        17280 :                   stencil_grad(:, i_pt) = stencil_grad(:, i_pt) - MATMUL(TRANSPOSE(A_raw), lam_raw)
    1156              :                END IF
    1157              :             END DO
    1158          540 :             IF (stencil_ok) EXIT
    1159          540 :             step_q = semidef_fd_shrink*step_q
    1160              :          END DO
    1161          540 :          IF (.NOT. stencil_ok) RETURN
    1162              :          dg_dq = (richardson_w1*(stencil_grad(:, 1) - stencil_grad(:, 2)) - &
    1163         3780 :                   richardson_w2*(stencil_grad(:, 3) - stencil_grad(:, 4)))/step_q
    1164         3870 :          H_tilde(:, slot) = (slot_scales*slot_scales(slot)/cand_scale)*dg_dq
    1165              :       END DO
    1166              : 
    1167         7650 :       H_tilde = 0.5_dp*(H_tilde + TRANSPOSE(H_tilde))
    1168         3870 :       IF (.NOT. ALL(ieee_is_finite(H_tilde))) RETURN
    1169           90 :       ok = .TRUE.
    1170              : 
    1171              :    END SUBROUTINE lagrangian_hessian_at_q0
    1172              : 
    1173              : ! **************************************************************************************************
    1174              : !> \brief Reduce pin columns to reduced column-echelon form.
    1175              : !> \param W_pin 6 x n_pin matrix of dimensionless pin normals. This routine reduces W_pin in place.
    1176              : !> \param n_pin number of active pin columns
    1177              : !> \param vec_tol minimum pivot magnitude for independent pin columns
    1178              : !> \param ok .TRUE. when all n_pin columns have pivots above vec_tol
    1179              : !> \note Full-pivoted Gauss-Jordan reduction gives a LAPACK-independent basis and turns
    1180              : !>       pure coordinate directions in span(W_pin) into single-slot columns.
    1181              : ! **************************************************************************************************
    1182           34 :    PURE SUBROUTINE echelon_pin_columns(W_pin, n_pin, vec_tol, ok)
    1183              :       INTEGER, INTENT(IN)                                :: n_pin
    1184              :       REAL(KIND=dp), INTENT(INOUT)                       :: W_pin(n_slot, n_pin)
    1185              :       REAL(KIND=dp), INTENT(IN)                          :: vec_tol
    1186              :       LOGICAL, INTENT(OUT)                               :: ok
    1187              : 
    1188              :       INTEGER                                            :: col, i, j, piv_col, piv_row, tmp_row, &
    1189            0 :                                                             piv_rows(n_pin)
    1190              :       LOGICAL                                            :: row_used(n_slot)
    1191              :       REAL(KIND=dp)                                      :: piv_val, tmp_col(n_slot)
    1192              : 
    1193           34 :       ok = .FALSE.
    1194           34 :       row_used = .FALSE.
    1195           76 :       DO col = 1, n_pin
    1196              :          piv_val = 0.0_dp
    1197              :          piv_row = 0
    1198              :          piv_col = 0
    1199          294 :          DO i = 1, n_slot
    1200          252 :             IF (row_used(i)) CYCLE
    1201          590 :             DO j = col, n_pin
    1202          560 :                IF (ABS(W_pin(i, j)) > piv_val) THEN
    1203           44 :                   piv_val = ABS(W_pin(i, j))
    1204           44 :                   piv_row = i
    1205           44 :                   piv_col = j
    1206              :                END IF
    1207              :             END DO
    1208              :          END DO
    1209           42 :          IF (piv_row == 0 .OR. piv_val <= vec_tol) RETURN
    1210           42 :          row_used(piv_row) = .TRUE.
    1211           42 :          piv_rows(col) = piv_row
    1212           42 :          IF (piv_col /= col) THEN
    1213            0 :             tmp_col = W_pin(:, col)
    1214            0 :             W_pin(:, col) = W_pin(:, piv_col)
    1215            0 :             W_pin(:, piv_col) = tmp_col
    1216              :          END IF
    1217          294 :          W_pin(:, col) = W_pin(:, col)/W_pin(piv_row, col)
    1218          142 :          DO j = 1, n_pin
    1219          252 :             IF (j /= col) W_pin(:, j) = W_pin(:, j) - W_pin(piv_row, j)*W_pin(:, col)
    1220              :          END DO
    1221              :       END DO
    1222              : 
    1223           42 :       DO col = 1, n_pin - 1
    1224           54 :          DO j = col + 1, n_pin
    1225           20 :             IF (piv_rows(j) < piv_rows(col)) THEN
    1226            0 :                tmp_row = piv_rows(col)
    1227            0 :                piv_rows(col) = piv_rows(j)
    1228            0 :                piv_rows(j) = tmp_row
    1229            0 :                tmp_col = W_pin(:, col)
    1230            0 :                W_pin(:, col) = W_pin(:, j)
    1231            0 :                W_pin(:, j) = tmp_col
    1232              :             END IF
    1233              :          END DO
    1234              :       END DO
    1235           34 :       ok = .TRUE.
    1236              : 
    1237              :    END SUBROUTINE echelon_pin_columns
    1238              : 
    1239              : ! **************************************************************************************************
    1240              : !> \brief Build the fixed orthonormal split [Y, Z] and reduced anchor u0 at m%q0.
    1241              : !> \param m constraint state
    1242              : !> \param info info_ok, info_degenerate, or info_licq
    1243              : !> \note Cholesky factorization of the row-normalized Gram matrix A A^T rejects both
    1244              : !>       exact row dependence and near-collinear rows before QR builds [Y, Z].
    1245              : !>       Pivoted Gram-Schmidt over the kernel rows makes Z depend only on ker(A(q0)),
    1246              : !>       not on LAPACK reflector signs or nullspace rotations.
    1247              : ! **************************************************************************************************
    1248         1014 :    SUBROUTINE build_chart_basis(m, info)
    1249              :       TYPE(cell_manifold_type), INTENT(INOUT)            :: m
    1250              :       INTEGER, INTENT(OUT)                               :: info
    1251              : 
    1252              :       INTEGER                                            :: cs, i, info_lapack, j, lwork, piv, pmax
    1253              :       LOGICAL                                            :: slot_used(n_slot)
    1254              :       REAL(KIND=dp) :: c_rot(n_slot, n_slot), Qfull(n_slot, n_slot), row_norms_slot(n_slot), &
    1255              :          work_query(1), z_rows(n_slot, n_slot)
    1256         1014 :       REAL(KIND=dp), ALLOCATABLE                         :: work(:)
    1257         2028 :       REAL(KIND=dp)                                      :: tau(m%k), row_norms(m%k), &
    1258         2028 :                                                             gram(m%k, m%k), gdiag(m%k), &
    1259         1014 :                                                             AT(n_slot, m%k), A(m%k, n_slot)
    1260              : 
    1261         1014 :       IF (m%k == 0) THEN
    1262          234 :          m%ndof = n_slot
    1263        10062 :          m%Y = 0.0_dp
    1264        10062 :          m%Z = 0.0_dp
    1265         1638 :          DO i = 1, n_slot
    1266         1638 :             m%Z(i, i) = 1.0_dp
    1267              :          END DO
    1268         1638 :          m%u0 = m%q0
    1269          234 :          info = info_ok
    1270          234 :          RETURN
    1271              :       END IF
    1272              : 
    1273          780 :       CALL normalized_active_jacobian(m, m%q0, A, row_norms, info)
    1274          780 :       IF (info /= info_ok) RETURN
    1275              : 
    1276        65498 :       gram = MATMUL(A, TRANSPOSE(A))
    1277          780 :       CALL dpotrf('U', m%k, gram, m%k, info_lapack)
    1278          780 :       IF (info_lapack /= 0) THEN
    1279           66 :          info = info_licq
    1280           66 :          RETURN
    1281              :       END IF
    1282         4094 :       gdiag = [(gram(i, i), i=1, m%k)]
    1283         4094 :       IF (MINVAL(gdiag) <= licq_pivot_tol*MAXVAL(gdiag)) THEN
    1284            8 :          info = info_licq
    1285            8 :          RETURN
    1286              :       END IF
    1287              : 
    1288        12242 :       AT = TRANSPOSE(A)
    1289          706 :       CALL dgeqrf(n_slot, m%k, AT, n_slot, tau, work_query, -1, info_lapack)
    1290          706 :       lwork = INT(work_query(1))
    1291              :       ! DORGQR regenerates all n_slot columns and needs its own workspace query:
    1292              :       ! DGEQRF's optimal LWORK does not cover it (rejected by MKL for k < n_slot).
    1293          706 :       CALL dorgqr(n_slot, n_slot, m%k, Qfull, n_slot, tau, work_query, -1, info_lapack)
    1294          706 :       lwork = MAX(lwork, INT(work_query(1)))
    1295         2118 :       ALLOCATE (work(MAX(1, lwork)))
    1296              : 
    1297          706 :       CALL dgeqrf(n_slot, m%k, AT, n_slot, tau, work, SIZE(work), info_lapack)
    1298          706 :       IF (info_lapack /= 0) THEN
    1299            0 :          info = info_licq
    1300            0 :          DEALLOCATE (work)
    1301            0 :          RETURN
    1302              :       END IF
    1303          706 :       Qfull = 0.0_dp
    1304        12242 :       Qfull(1:n_slot, 1:m%k) = AT(1:n_slot, 1:m%k)
    1305          706 :       CALL dorgqr(n_slot, n_slot, m%k, Qfull, n_slot, tau, work, SIZE(work), info_lapack)
    1306          706 :       DEALLOCATE (work)
    1307          706 :       IF (info_lapack /= 0) THEN
    1308            0 :          info = info_licq
    1309            0 :          RETURN
    1310              :       END IF
    1311              : 
    1312          706 :       m%ndof = n_slot - m%k
    1313         2354 :       DO j = 1, m%k
    1314        11536 :          pmax = MAXLOC(ABS(Qfull(:, j)), DIM=1)
    1315         9182 :          IF (Qfull(pmax, j) < 0.0_dp) Qfull(:, j) = -Qfull(:, j)
    1316              :       END DO
    1317        30358 :       m%Y = 0.0_dp
    1318        12242 :       m%Y(:, 1:m%k) = Qfull(:, 1:m%k)
    1319              : 
    1320              :       ! Canonical tangent basis: pivoted Gram-Schmidt over the rows of the raw
    1321              :       ! kernel columns. The kernel span is basis-choice invariant, but its QR
    1322              :       ! representation is not (LAPACK builds differ by signs and rotations),
    1323              :       ! and the reduced coordinates must stay reproducible across builds.
    1324        30358 :       m%Z = 0.0_dp
    1325          706 :       IF (m%ndof > 0) THEN
    1326          688 :          c_rot = 0.0_dp
    1327          688 :          z_rows = 0.0_dp
    1328        18804 :          z_rows(:, 1:m%ndof) = Qfull(:, m%k + 1:n_slot)
    1329          688 :          slot_used = .FALSE.
    1330         3276 :          DO cs = 1, m%ndof
    1331         2588 :             row_norms_slot = 0.0_dp
    1332        18116 :             DO i = 1, n_slot
    1333        64478 :                IF (.NOT. slot_used(i)) row_norms_slot(i) = NORM2(z_rows(i, 1:m%ndof))
    1334              :             END DO
    1335              :             ! Lowest slot index among near-equal norms, so symmetric ties resolve
    1336              :             ! the same way on every build.
    1337              :             piv = FINDLOC(row_norms_slot >= (1.0_dp - basis_tie_rel_tol)*MAXVAL(row_norms_slot), &
    1338        24624 :                           .TRUE., DIM=1)
    1339        13468 :             c_rot(1:m%ndof, cs) = z_rows(piv, 1:m%ndof)/row_norms_slot(piv)
    1340         2588 :             slot_used(piv) = .TRUE.
    1341        18804 :             DO i = 1, n_slot
    1342        18116 :                IF (.NOT. slot_used(i)) THEN
    1343              :                   z_rows(i, 1:m%ndof) = z_rows(i, 1:m%ndof) - &
    1344        79758 :                                         DOT_PRODUCT(z_rows(i, 1:m%ndof), c_rot(1:m%ndof, cs))*c_rot(1:m%ndof, cs)
    1345              :                END IF
    1346              :             END DO
    1347              :          END DO
    1348        97552 :          m%Z(:, 1:m%ndof) = MATMUL(Qfull(:, m%k + 1:n_slot), c_rot(1:m%ndof, 1:m%ndof))
    1349              :       END IF
    1350         4942 :       m%u0 = 0.0_dp
    1351         3294 :       m%u0(1:m%ndof) = MATMUL(TRANSPOSE(m%Z(:, 1:m%ndof)), m%q0)
    1352              : 
    1353              :    END SUBROUTINE build_chart_basis
    1354              : 
    1355              : ! **************************************************************************************************
    1356              : !> \brief Assemble A(q) and scale each row to unit 2-norm.
    1357              : !> \param m constraint state (with m%k > 0)
    1358              : !> \param q canonical cell vector
    1359              : !> \param A row-normalized active Jacobian
    1360              : !> \param row_norms original Euclidean norm of each row before normalization
    1361              : !> \param info info_ok, info_degenerate, or info_licq
    1362              : !> \note Unit-norm rows keep linear solves independent of unit scales when large
    1363              : !>       volume gradients mix with component rows.
    1364              : ! **************************************************************************************************
    1365        11416 :    SUBROUTINE normalized_active_jacobian(m, q, A, row_norms, info)
    1366              :       TYPE(cell_manifold_type), INTENT(IN)               :: m
    1367              :       REAL(KIND=dp), INTENT(IN)                          :: q(n_slot)
    1368              :       REAL(KIND=dp), INTENT(OUT)                         :: A(m%k, n_slot), row_norms(m%k)
    1369              :       INTEGER, INTENT(OUT)                               :: info
    1370              : 
    1371              :       INTEGER                                            :: i
    1372              : 
    1373        46848 :       row_norms = 0.0_dp
    1374        11416 :       CALL cell_manifold_active_jacobian(m, q, A, info)
    1375        11416 :       IF (info /= info_ok) RETURN
    1376              : 
    1377        46848 :       DO i = 1, m%k
    1378       248024 :          row_norms(i) = NORM2(A(i, :))
    1379        35432 :          IF (row_norms(i) <= TINY(1.0_dp)) THEN
    1380            0 :             info = info_licq
    1381            0 :             RETURN
    1382              :          END IF
    1383       259440 :          A(i, :) = A(i, :)/row_norms(i)
    1384              :       END DO
    1385              : 
    1386              :    END SUBROUTINE normalized_active_jacobian
    1387              : 
    1388              : ! **************************************************************************************************
    1389              : !> \brief Form and LU-factor the k x k block B = A(q) * Y.
    1390              : !> \param k number of active constraints (k > 0)
    1391              : !> \param A row-normalized k x 6 active Jacobian at q
    1392              : !> \param Y fixed 6 x k constrained-direction basis from q0
    1393              : !> \param B LU factors of A * Y from DGETRF
    1394              : !> \param ipiv pivot indices from DGETRF
    1395              : !> \param info info_ok or info_licq
    1396              : !> \note After DGETRF, the diagonal of B holds the pivots of U. A tiny pivot ratio
    1397              : !>       means ker(A(q)) now intersects span(Y), so q(u) is no longer single-valued
    1398              : !>       along Y.
    1399              : ! **************************************************************************************************
    1400         7762 :    SUBROUTINE factor_normal_block(k, A, Y, B, ipiv, info)
    1401              :       INTEGER, INTENT(IN)                                :: k
    1402              :       REAL(KIND=dp), INTENT(IN)                          :: A(k, n_slot), Y(n_slot, k)
    1403              :       REAL(KIND=dp), INTENT(OUT)                         :: B(k, k)
    1404              :       INTEGER, INTENT(OUT)                               :: ipiv(k), info
    1405              : 
    1406              :       INTEGER                                            :: i, lapack_info
    1407        15524 :       REAL(KIND=dp)                                      :: u_pivots(k)
    1408              : 
    1409       804206 :       B = MATMUL(A, Y)
    1410         7762 :       CALL dgetrf(k, k, B, k, ipiv, lapack_info)
    1411        63516 :       u_pivots = [(ABS(B(i, i)), i=1, k)]
    1412        63516 :       IF (lapack_info /= 0 .OR. MINVAL(u_pivots) <= licq_pivot_tol*MAXVAL(u_pivots)) THEN
    1413            0 :          info = info_licq
    1414              :       ELSE
    1415         7762 :          info = info_ok
    1416              :       END IF
    1417              : 
    1418         7762 :    END SUBROUTINE factor_normal_block
    1419              : 
    1420              : ! **************************************************************************************************
    1421              : !> \brief Differentiate angle(u, v) = acos((u.v) / (|u| |v|)) with respect to q.
    1422              : !> \param cos_ang cos(angle)
    1423              : !> \param sin_ang sin(angle)
    1424              : !> \param ddot_dq derivative d(u.v)/dq of the dot product
    1425              : !> \param len_u length |u|
    1426              : !> \param len_v length |v|
    1427              : !> \param dlen_u_dq derivative d|u|/dq
    1428              : !> \param dlen_v_dq derivative d|v|/dq
    1429              : !> \return row vector d(angle)/dq
    1430              : ! **************************************************************************************************
    1431        47532 :    PURE FUNCTION angle_grad_row(cos_ang, sin_ang, ddot_dq, len_u, len_v, dlen_u_dq, dlen_v_dq) &
    1432        47532 :       RESULT(d_ang_dq)
    1433              :       REAL(KIND=dp), INTENT(IN)                          :: cos_ang, sin_ang, ddot_dq(n_slot), &
    1434              :                                                             len_u, len_v, dlen_u_dq(n_slot), &
    1435              :                                                             dlen_v_dq(n_slot)
    1436              :       REAL(KIND=dp)                                      :: d_ang_dq(n_slot)
    1437              : 
    1438              :       REAL(KIND=dp)                                      :: dcos_dq(n_slot)
    1439              : 
    1440       332724 :       dcos_dq = ddot_dq/(len_u*len_v) - cos_ang*(dlen_u_dq/len_u + dlen_v_dq/len_v)
    1441       332724 :       d_ang_dq = -dcos_dq/sin_ang
    1442              : 
    1443        47532 :    END FUNCTION angle_grad_row
    1444              : 
    1445        16204 : END MODULE cell_manifold_utils
        

Generated by: LCOV version 2.0-1