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
|