Function format_matrices

src/tinympc/rho_benchmark.cpp:44–144

}

void format_matrices(
    RhoAdapter* adapter,
    const tinyMatrix& x_prev,
    const tinyMatrix& u_prev,
    const tinyMatrix& v_prev,
    const tinyMatrix& z_prev,
    const tinyMatrix& g_prev,
    const tinyMatrix& y_prev,
    TinyCache* cache,
    TinyWorkspace* work,
    int N
) {
    if (!adapter->matrices_initialized) {
        initialize_format_matrices(adapter, x_prev.rows(), u_prev.rows(), N);
    }
    
    int nx = adapter->format_nx;
    int nu = adapter->format_nu;
    
    // Fill x_decision
    int x_idx = 0;
    for (int i = 0; i < N; i++) {
        adapter->x_decision.block(x_idx, 0, nx, 1) = x_prev.col(i);
        x_idx += nx;
        if (i < N-1) {
            adapter->x_decision.block(x_idx, 0, nu, 1) = u_prev.col(i);
            x_idx += nu;
        }
    }
    
    // Clear A matrix for reuse
    adapter->A_matrix.setZero();
    
    // Fill A matrix with dynamics and input constraints
    for (int i = 0; i < N-1; i++) {
        // Input constraints
        int row_start = i * nu;
        int col_start = i * (nx+nu) + nx;
        adapter->A_matrix.block(row_start, col_start, nu, nu) = tinyMatrix::Identity(nu, nu);
        
        // Dynamics constraints
        row_start = (N-1) * nu + i * nx;
        col_start = i * (nx+nu);
        adapter->A_matrix.block(row_start, col_start, nx, nx) = work->Adyn;
        adapter->A_matrix.block(row_start, col_start+nx, nx, nu) = work->Bdyn;
        
        int next_state_idx = col_start + nx + nu;
        if (next_state_idx < adapter->A_matrix.cols()) {
            adapter->A_matrix.block(row_start, next_state_idx, nx, nx) = -tinyMatrix::Identity(nx, nx);
        }
    }
    
    // Fill z and y vectors
    for (int i = 0; i < N-1; i++) {
        adapter->z_vector.block(i*nu, 0, nu, 1) = z_prev.col(i);
        adapter->z_vector.block((N-1)*nu+i*nx, 0, nx, 1) = v_prev.col(i+1);
        
        adapter->y_vector.block(i*nu, 0, nu, 1) = y_prev.col(i);