| 12 | #endif |
| 13 | |
| 14 | void initialize_format_matrices(RhoAdapter* adapter, int nx, int nu, int N) { |
| 15 | // Calculate dimensions |
| 16 | int x_decision_size = nx * N + nu * (N-1); |
| 17 | int constraint_rows = (nx + nu) * (N-1); |
| 18 | |
| 19 | // Pre-allocate matrices |
| 20 | adapter->A_matrix = tinyMatrix::Zero(constraint_rows, x_decision_size); |
| 21 | adapter->z_vector = tinyMatrix::Zero(constraint_rows, 1); |
| 22 | adapter->y_vector = tinyMatrix::Zero(constraint_rows, 1); |
| 23 | adapter->x_decision = tinyMatrix::Zero(x_decision_size, 1); |
| 24 | |
| 25 | // Pre-compute P matrix structure |
| 26 | adapter->P_matrix = tinyMatrix::Zero(x_decision_size, x_decision_size); |
| 27 | adapter->q_vector = tinyMatrix::Zero(x_decision_size, 1); |
| 28 | |
| 29 | // Pre-allocate residual computation matrices |
| 30 | adapter->Ax_vector = tinyMatrix::Zero(constraint_rows, 1); |
| 31 | adapter->r_prim_vector = tinyMatrix::Zero(constraint_rows, 1); |
| 32 | adapter->r_dual_vector = tinyMatrix::Zero(x_decision_size, 1); |
| 33 | adapter->Px_vector = tinyMatrix::Zero(x_decision_size, 1); |
| 34 | adapter->ATy_vector = tinyMatrix::Zero(x_decision_size, 1); |
| 35 | |
| 36 | // Store dimensions |
| 37 | adapter->format_nx = nx; |
| 38 | adapter->format_nu = nu; |
| 39 | adapter->format_N = N; |
| 40 | |
| 41 | adapter->matrices_initialized = true; |
| 42 | } |
| 43 | |
| 44 | void format_matrices( |
| 45 | RhoAdapter* adapter, |