diff --git a/ADOL-C/boost-test/traceFixedPointScalarTests.cpp b/ADOL-C/boost-test/traceFixedPointScalarTests.cpp index 3f0076e2..688a0ee3 100644 --- a/ADOL-C/boost-test/traceFixedPointScalarTests.cpp +++ b/ADOL-C/boost-test/traceFixedPointScalarTests.cpp @@ -48,17 +48,18 @@ static double traceNewtonForSquareRoot(int tapeId, int sub_tape_id, adouble u; u <<= argument; - fp_iteration(tapeId, sub_tape_id, iteration, iteration, norm, - norm, // Norm for the termination criterion for the adjoint - 1e-8, // Termination threshold for fixed-point iteration - 1e-8, // Termination threshold - 6, // Maximum number of iterations - 6, // Maximum number of adjoint iterations - &x, // [in] Initial iterate of fixed-point iteration - &u, // [in] The parameters: We compute the derivative wrt this - &y, // [out] Final state of the iteration - 1, // Size of the vector x_0 - 1); // Number of parameters + ADOLC::FpIteration::fp_iteration( + tapeId, sub_tape_id, iteration, iteration, norm, + norm, // Norm for the termination criterion for the adjoint + 1e-8, // Termination threshold for fixed-point iteration + 1e-8, // Termination threshold + 6, // Maximum number of iterations + 6, // Maximum number of adjoint iterations + &x, // [in] Initial iterate of fixed-point iteration + &u, // [in] The parameters: We compute the derivative wrt this + &y, // [out] Final state of the iteration + 1, // Size of the vector x_0 + 1); // Number of parameters y >>= out; trace_off(); diff --git a/ADOL-C/include/adolc/fixpoint.h b/ADOL-C/include/adolc/fixpoint.h index fc24aa9c..7b2418e8 100644 --- a/ADOL-C/include/adolc/fixpoint.h +++ b/ADOL-C/include/adolc/fixpoint.h @@ -18,14 +18,248 @@ #include +namespace ADOLC::FpIteration { + +// define the function types, which are used internally typedef int (*double_F)(double *, double *, double *, int, int); typedef int (*adouble_F)(adouble *, adouble *, adouble *, int, int); typedef double (*norm_F)(double *, int); typedef double (*norm_deriv_F)(double *, int); -int fp_iteration(short tapeId, size_t sub_tape_num, double_F, adouble_F, norm_F, - norm_deriv_F, double epsilon, double epsilon_deriv, - size_t N_max, size_t N_max_deriv, adouble *x_0, adouble *u, - adouble *x_fix, size_t dim_x, size_t dim_u); +struct FixedPoint { + size_t lastIter{0}; /**< number of laster iteration the point belongs to */ + std::vector x; /**< x = F(x, u) */ + std::vector u; /**< parameter of the fixed-point */ +}; + +struct FpProblem { + short tapeId; /**< Id of the outer tape */ + short subTapeId; /**< Subtape ID for active evaluation */ + short internalTapeId; /**< Id of tape required for higher-order derivatives */ + double_F double_func; /**< F(x,u) in passive (double) mode */ + adouble_F adouble_func; /**< F(x,u) in active (adouble) mode */ + norm_F norm_func; /**< norm() for convergence check (passive) */ + norm_deriv_F norm_deriv_func; /**< norm() for derivative convergence */ + double epsilon; /**< tolerance for |x_{k+1}-x_k| (eq. (2)) */ + double + epsilon_deriv; /**< tolerance for |\dot{x}_{k+1}-\dot{x}_k| (eq. (5)) */ + size_t N_max; /**< maximum iterations for value iteration */ + size_t N_max_deriv; /**< maximum iterations for derivative iter. */ + adouble *x_0; /**< initial iterate of the fixed-point iteration */ + adouble *u; /**< parameter of the fixed-point */ + adouble *x_fix; /**< storage for the fixed-point: x_fix = F(x_fix, u) */ + size_t dim_x; /**< dimension of x */ + size_t dim_u; /**< dimension of u */ + FixedPoint fp; /**< stores the fixed-point and last iteration */ + bool isInternal{ + false}; /**< flag to indicate if we use internalTape or subTape */ +}; + +/** + * @enum FpMode + * @brief Indicates the mode of the fixed-point driver. + */ +enum class FpMode { + firstOrder, + secondOrder, +}; + +/** + * @struct fpi_data + * @brief Storage of fixed-point iteration parameters and function pointers. + * + * Holds the passive and active evaluation functions F (double/adouble), + * norms, tolerances, and maximum iteration counts. + * Corresponds to the description in Section 2.3 of the paper. + */ +struct fpi_data { + size_t edfIdx; /**< External differentiated function index */ + FpProblem problem; + ext_diff_fct *edfIteration; +}; + +/// Stack of active fixed-point configurations +inline static std::vector &fpiStack() { + static std::vector fpi_stack; + return fpi_stack; +} +inline FpProblem &getFpProblem(size_t edfIdx) { + // Locate iteration parameters + auto fpiDataPtr = std::find_if(fpiStack().begin(), fpiStack().end(), + [&](auto &&v) { return v.edfIdx == edfIdx; }); + + if (fpiDataPtr == fpiStack().end()) + ADOLCError::fail(ADOLCError::ErrorType::FP_NO_EDF, CURRENT_LOCATION); + + return fpiDataPtr->problem; +} + +/** + * @brief Passive fixed-point iteration x_{k+1} = F(x_k, u). + * + * Implements equation (2) of the paper: + * x_{k+1} = F(x_k, u) + * and checks convergence: + * \|x_{k+1} - x_k\| < epsilon. + * + * @param tapeId ADOL-C tape identifier (unused here). + * @param dim_xu Total dimension of [x; u]. + * @param xu Input/output array: [x; u] passive values. + * @param dim_x Dimension of x. + * @param x_fix Output array to store converged x^*. + * @return Number of iterations k when converged, or -1 if not. + */ +int iteration(short tapeId, size_t dim_xu, double *xu, size_t dim_x, + double *x_fix); + +/** + * @brief Zero-Order Scalar (ZOS) forward. Does the same as `iteration`. + * + * @param tapeId ADOL-C tape identifier (unused here). + * @param dim_xu Total dimension of [x; u]. + * @param xu Input/output array: [x; u] passive values. + * @param dim_x Dimension of x. + * @param x_fix Output array to store converged x^*. + * @return Number of iterations k when converged, or -1 if not. + */ +int fp_zos_forward(short tapeId, size_t dim_xu, double *xu, size_t dim_x, + double *x_fix); + +/** + * @brief First-order Scalar (FOS) forward for differentiating fixed-point + * iterations. + * + * This function implements the forward mode differentiated fixed-point + * iteration described in Equation (5) of the referenced paper: + * + * \dot{x}_{k+1} = F'_x(x_*, u) \dot{x}_k + F'_u(x_*, u) \dot{u} + * + * It uses ADOL-C's `fos_forward` to evaluate the directional derivatives and + * iterates until both primal and directional residuals fall below specified + * tolerances or the maximal number of iterations is reached. + * + * @param tapeId ID of the outer tape. + * @param dim_xu Dimension of total input vector [x; u]. + * @param xu Input/output primal vector. + * @param xu_dot Input/output directional derivative vector. + * @param dim_x Dimension of x. + * @param x_fix Reference solution x_*. + * @param x_fix_dot Initial directional derivative of x (\dot{x}_0). + * + * @return Iteration count on success, -1 if convergence not reached. + */ +int fp_fos_forward(short tapeId, size_t dim_xu, double *xu, double *xu_dot, + size_t dim_x, double *x_fix, double *x_fix_dot); + +/** + * @brief First-Order Scalar (FOS) reverse mode for for differentiating + * fixed-point iterations. + * + * Implements the fixed-point reverse mode derivative iteration from + * Equation (6) in the paper: + * + * \xeta^T_{k+1} = \xeta^T_k F_x(x_*, u) + \bar{x}^T, + * \bar{u} = \xeta^T_k F_u(x_*, u) + * + * This sweep accumulates adjoint contributions to the control inputs `u` + * using reverse-mode directional derivatives via `fos_reverse`. + * Convergence is determined using the norm of the residual in the adjoint + * direction. + * + * @param tapeId ID of the outer (value) tape. + * @param dim_x Dimension of primal state x. + * @param x_fix_bar Input adjoint of x_*. + * @param dim_xu Dimension of total input vector [x; u]. + * @param xu_bar Output adjoint for [x; u], incremented in-place. + * @param unused1 Unused. + * @param unused2 Unused. + * + * @return Number of iterations to convergence, or -1 if not converged. + */ + +int fp_fos_reverse(short tapeId, size_t dim_x, double *x_fix_bar, size_t dim_xu, + double *xu_bar, double * /*unused*/, double * /*unused*/); + +int fp_hos_ti_reverse(short tapeId, size_t dim_x, double **x_fix_bar, + size_t dim_xu, size_t d, double **xu_bar, double **dpp_x, + double **dpp_y); + +inline ext_diff_fct *registerFpIteration(const FpProblem &problem) { + // declare extern differentiated function using the fixed-point functions + ext_diff_fct *edfIteration = + reg_ext_fct(problem.tapeId, problem.subTapeId, &iteration); + edfIteration->zos_forward = &fp_zos_forward; + edfIteration->fos_forward = &fp_fos_forward; + edfIteration->fos_reverse = &fp_fos_reverse; + edfIteration->hos_ti_reverse = &fp_hos_ti_reverse; + + // add parameters of the fp iteration to the stack + fpiStack().emplace_back(fpi_data{edfIteration->index, problem, edfIteration}); + + return edfIteration; +} + +int firstOrderFp(const FpProblem &problem); +int secondOrderFp(const FpProblem &problem); +/** + * @brief Register and perform fixed-point iteration + * with active (adouble) types, using advanced taping of subtapes. + * + * Wraps up: takes initialization of τ, uses the fixed-point φ, and gives the + * result back to χ as in eq. (7) and Section 2.3. + * + * @param tapeId Tape identifier for outer tape. + * @param subTapeId Tape identifier for active subtape. + * @param double_func Pointer to passive F(x,u). + * @param adouble_func Pointer to active F(x,u). + * @param norm_func Passive norm function. + * @param norm_deriv_func Derivative norm function. + * @param epsilon Convergence tolerance for values. + * @param epsilon_deriv Tolerance for derivative iteration. + * @param N_max Max iterations for values. + * @param N_max_deriv Max iterations for derivatives. + * @param x_0 Initial x vector (active) (output of τ). + * @param u Control vector (active) (output of τ). + * @param x_fix Output fixed-point (active). + * @param dim_x Dimension of x. + * @param dim_u Dimension of u. + * @return Number of fixed-point iterations performed. + */ +template +int fp_iteration(short tapeId, short subTapeId, double_F double_func, + adouble_F adouble_func, norm_F norm_func, + norm_deriv_F norm_deriv_func, double epsilon, + double epsilon_deriv, size_t N_max, size_t N_max_deriv, + adouble *x_0, adouble *u, adouble *x_fix, size_t dim_x, + size_t dim_u) { + return fp_iteration( + FpProblem{.tapeId = tapeId, + .subTapeId = subTapeId, + .double_func = double_func, + .adouble_func = adouble_func, + .norm_func = norm_func, + .norm_deriv_func = norm_deriv_func, + .epsilon = epsilon, + .epsilon_deriv = epsilon_deriv, + .N_max = N_max, + .N_max_deriv = N_max_deriv, + .x_0 = x_0, + .u = u, + .x_fix = x_fix, + .dim_x = dim_x, + .dim_u = dim_u, + .fp = FixedPoint{.x = std::vector(dim_x), + .u = std::vector(dim_u)}}); +} + +template int fp_iteration(const FpProblem &problem) { + if constexpr (mode == FpMode::firstOrder) + return firstOrderFp(problem); + if constexpr (mode == FpMode::secondOrder) + return secondOrderFp(problem); + else + static_assert("Not implemented for Template parameter mode!"); +} +}; // namespace ADOLC::FpIteration #endif // ADOLC_FIXPOINT_H diff --git a/ADOL-C/src/fixpoint.cpp b/ADOL-C/src/fixpoint.cpp index 71bb1331..74f4d7fa 100644 --- a/ADOL-C/src/fixpoint.cpp +++ b/ADOL-C/src/fixpoint.cpp @@ -1,12 +1,15 @@ /*---------------------------------------------------------------------------- ADOL-C -- Automatic Differentiation by Overloading in C++ - File: fixpoint.c + File: fixpoint.cpp Revision: $Id$ - Contents: all C functions directly accessing at least one of the four tapes - (operations, locations, constants, value stack) - Copyright (c) Andreas Kowarz, Sebastian Schlenkrich + This file implements fixed-point iterations and their derivative + computations as described in "Differentiating Fixed Point Iterations + with ADOL-C: Gradient Calculation for Fluid Dynamics" (Schlenkrich et al., + 2008). The methods correspond to equations (1)–(6) in the paper. + + Copyright (c) Andreas Kowarz, Sebastian Schlenkrich This file is part of ADOL-C. This software is provided as open source. Any use, reproduction, or distribution of the software constitutes @@ -16,278 +19,306 @@ #include #include -#include #include #include #include #include #include -/*--------------------------------------------------------------------------*/ - -/* F(x,u,y,dim_x,dim_u) */ -/* norm(x,dim_x) */ -struct fpi_data { - size_t edf_index; - size_t sub_tape_num; - double_F double_func; - adouble_F adouble_func; - norm_F norm_func; - norm_deriv_F norm_deriv_func; - double epsilon; - double epsilon_deriv; - size_t N_max; - size_t N_max_deriv; -}; - -static std::vector fpi_stack; - -static int iteration(short tapeId, size_t dim_xu, double *xu, size_t dim_x, - double *x_fix) { - double err; - const fpi_data ¤t = fpi_stack.back(); +namespace ADOLC::FpIteration { - for (size_t i = 0; i < dim_x; ++i) - x_fix[i] = xu[i]; +void prepareFixedPoint(ext_diff_fct *edfIteration, const FpProblem &problem, + FixedPoint &fp) { + // put x and u together for the iteration + std::vector xu(problem.dim_x + problem.dim_u); - for (size_t k = 1; k <= current.N_max; ++k) { - for (size_t i = 0; i < dim_x; ++i) - xu[i] = x_fix[i]; + // initialize x_0 + for (size_t i = 0; i < problem.dim_x; ++i) + xu[i] = problem.x_0[i]; - current.double_func(xu, xu + dim_x, x_fix, dim_x, dim_xu - dim_x); + // initialize u + for (size_t i = 0; i < problem.dim_u; ++i) + xu[problem.dim_x + i] = problem.u[i]; - for (size_t i = 0; i < dim_x; ++i) - xu[i] = x_fix[i] - xu[i]; + fp.lastIter = call_ext_fct(edfIteration, problem.dim_x + problem.dim_u, + xu.data(), problem.dim_x, problem.x_fix); - err = current.norm_func(xu, dim_x); - if (err < current.epsilon) - return k; - } - return -1; + // copy fixed-point of x for output + for (size_t i = 0; i < problem.dim_x; ++i) + fp.x[i] = problem.x_fix[i].value(); + + // copy fixed-point of u for output + for (size_t i = 0; i < problem.dim_u; ++i) + fp.u[i] = xu[problem.dim_x + i].value(); } -static int fp_zos_forward(short tapeId, size_t dim_xu, double *xu, size_t dim_x, - double *x_fix) { +void tapeLastFpIteration(short tapeId, const FpProblem &problem, + FixedPoint &fp) { + currentTape().ensureContiguousLocations(2 * (problem.dim_u + problem.dim_x)); + std::vector x_fix_new(problem.dim_u + problem.dim_x); + std::vector xu_sub_tape(problem.dim_u + problem.dim_x); + // copy fixed-point + for (size_t i = 0; i < problem.dim_x; ++i) { + x_fix_new[i] = fp.x[i]; + } + + // tape the last fixed-point iteration and keep the result + trace_on(tapeId, 1); + for (size_t i = 0; i < problem.dim_x; ++i) + xu_sub_tape[i] <<= fp.x[i]; - ValueTape &tape = findTape(tapeId); - double err; - const size_t edf_index = tape.ext_diff_fct_index(); + for (size_t i = 0; i < problem.dim_u; ++i) + xu_sub_tape[problem.dim_x + i] <<= fp.u[i]; - // Find fpi_stack element with index 'edf_index'. - auto current = - std::find_if(fpi_stack.begin(), fpi_stack.end(), - [&](auto &&v) { return v.edf_index == edf_index; }); + // IMPORTANT: Dont reuse x_fix here. The location of the x_fix's + // adoubles could change and the old locations are already stored on the + // tape due to externa differentation. This would cause errors + problem.adouble_func(xu_sub_tape.data(), xu_sub_tape.data() + problem.dim_x, + x_fix_new.data(), problem.dim_x, problem.dim_u); + + double dummy_out; + for (size_t i = 0; i < problem.dim_x; ++i) + x_fix_new[i] >>= dummy_out; - if (current == fpi_stack.end()) - ADOLCError::fail(ADOLCError::ErrorType::FP_NO_EDF, CURRENT_LOCATION); + trace_off(); +} +int iteration(short tapeId, size_t dim_xu, double *xu, size_t dim_x, + double *x_fix) { + assert(dim_xu > dim_x && "Dimension mismatch, dim_xu <= dim_x"); + double err = 0.0; + FpProblem problem = getFpProblem(findTape(tapeId).ext_diff_fct_index()); + // Initialize x_0 from xu[0..dim_x-1] for (size_t i = 0; i < dim_x; ++i) x_fix[i] = xu[i]; - for (size_t k = 1; k <= current->N_max; ++k) { + // Main fixed-point loop (eq. (2)) + for (size_t k = 1; k <= problem.N_max; ++k) { + // copy x_fix to xu for (size_t i = 0; i < dim_x; ++i) xu[i] = x_fix[i]; - current->double_func(xu, xu + dim_x, x_fix, dim_x, dim_xu - dim_x); + // passive call: x_{k+1} = F(x_k, u) + problem.double_func(xu, xu + dim_x, x_fix, dim_x, dim_xu - dim_x); + // residual: x_fix - F(x_k, u) for (size_t i = 0; i < dim_x; ++i) xu[i] = x_fix[i] - xu[i]; - err = current->norm_func(xu, dim_x); - if (err < current->epsilon) + // convergence check: ||xu|| < epsilon + double err = problem.norm_func(xu, dim_x); + assert(err >= 0 && "Error should not be negative"); + if (err < problem.epsilon) return k; } return -1; } -static int fp_fos_forward(short tapeId, size_t dim_xu, double *xu, - double *xu_dot, size_t dim_x, double *x_fix, - double *x_fix_dot) { - - // Piggy back - double err, err_deriv; - ValueTape &tape = findTape(tapeId); +int fp_zos_forward(short tapeId, size_t dim_xu, double *xu, size_t dim_x, + double *x_fix) { + assert(dim_xu > dim_x && "Dimension mismatch, dim_xu <= dim_x"); + double err = 0.0; + FpProblem problem = getFpProblem(findTape(tapeId).ext_diff_fct_index()); - const size_t edf_index = tape.ext_diff_fct_index(); + // identical to 'iteration' passive loop + for (size_t i = 0; i < dim_x; ++i) + x_fix[i] = xu[i]; + for (size_t k = 1; k <= problem.N_max; ++k) { + for (size_t i = 0; i < dim_x; ++i) + xu[i] = x_fix[i]; + problem.double_func(xu, xu + dim_x, x_fix, dim_x, dim_xu - dim_x); + for (size_t i = 0; i < dim_x; ++i) + xu[i] = x_fix[i] - xu[i]; + err = problem.norm_func(xu, dim_x); + assert(err >= 0 && "Error should not be negative"); + if (err < problem.epsilon) + return k; + } + return -1; +} - // Find fpi_stack element with index 'edf_index'. - auto current = - std::find_if(fpi_stack.begin(), fpi_stack.end(), - [&](auto &&v) { return v.edf_index == edf_index; }); +int fp_fos_forward(short tapeId, size_t dim_xu, double *xu, double *xu_dot, + size_t dim_x, double *x_fix, double *x_fix_dot) { + assert(dim_xu > dim_x && "Dimension mismatch, dim_xu <= dim_x"); + double err = 0; + double err_deriv = 0; + FpProblem problem = getFpProblem(findTape(tapeId).ext_diff_fct_index()); + const size_t maxIter = std::max(problem.N_max_deriv, problem.N_max); - if (current == fpi_stack.end()) - ADOLCError::fail(ADOLCError::ErrorType::FP_NO_EDF, CURRENT_LOCATION); - for (size_t k = 1; (k < current->N_max_deriv) || (k < current->N_max); ++k) { + for (size_t k = 1; k < maxIter; ++k) { if (k > 1) { - for (size_t i = 0; i < dim_x; ++i) + // copies x_*= f(x_*, u) and \dot{x}_{k+1} = F'(x_*, u)\dot{x_k} + for (size_t i = 0; i < dim_x; ++i) { xu[i] = x_fix[i]; - - for (size_t i = 0; i < dim_x; ++i) xu_dot[i] = x_fix_dot[i]; + } } - fos_forward(current->sub_tape_num, dim_x, dim_xu, 0, xu, xu_dot, x_fix, + // Compute F(x_*,u) and F'(x_*, u)[\dot{x_k}; \dot{u}] + fos_forward(problem.subTapeId, dim_x, dim_xu, 2, xu, xu_dot, x_fix, x_fix_dot); - for (size_t i = 0; i < dim_x; ++i) + // Compute residuals in primal and tangent values + for (size_t i = 0; i < dim_x; ++i) { xu[i] = x_fix[i] - xu[i]; - - err = current->norm_func(xu, dim_x); - for (size_t i = 0; i < dim_x; ++i) xu_dot[i] = x_fix_dot[i] - xu_dot[i]; - - err_deriv = current->norm_deriv_func(xu_dot, dim_x); - if ((err < current->epsilon) && (err_deriv < current->epsilon_deriv)) { + } + err = problem.norm_func(xu, dim_x); + err_deriv = problem.norm_deriv_func(xu_dot, dim_x); + assert(err >= 0 && "Error should not be negative"); + assert(err_deriv >= 0 && "Error should not be negative"); + // check if converged + if ((err < problem.epsilon) && (err_deriv < problem.epsilon_deriv)) { return k; } } return -1; } -static int fp_fos_reverse(short tapeId, size_t dim_x, double *x_fix_bar, - size_t dim_xu, double *xu_bar, double * /*unused*/, - double * /*unused*/) { - // (d x_fix) / (d x_0) = 0 (!) - +int fp_fos_reverse(short tapeId, size_t dim_x, double *x_fix_bar, size_t dim_xu, + double *xu_bar, double * /*unused*/, double * /*unused*/) { + assert(dim_xu > dim_x && "Dimension mismatch, dim_xu <= dim_x"); double err = 0.0; - ValueTape &tape = findTape(tapeId); - const size_t edf_index = tape.ext_diff_fct_index(); - // Find fpi_stack element with index 'edf_index'. - auto current = - std::find_if(fpi_stack.begin(), fpi_stack.end(), - [&](auto &&v) { return v.edf_index == edf_index; }); + FpProblem problem = getFpProblem(findTape(tapeId).ext_diff_fct_index()); + std::vector xeta_u(dim_xu); + std::fill(xeta_u.begin(), xeta_u.end(), 0.0); + short tapeId_ = + problem.isInternal ? problem.internalTapeId : problem.subTapeId; + std::vector xeta(dim_x); - if (current == fpi_stack.end()) - ADOLCError::fail(ADOLCError::ErrorType::FP_NO_EDF, CURRENT_LOCATION); - - double *U = new double[dim_xu]; - double *xi = new double[dim_x]; - - for (size_t k = 1; k < current->N_max_deriv; ++k) { + for (size_t k = 1; k < problem.N_max_deriv; ++k) { for (size_t i = 0; i < dim_x; ++i) - xi[i] = U[i]; + xeta[i] = xeta_u[i]; - fos_reverse(current->sub_tape_num, dim_x, dim_xu, xi, U); + // compute xeta_u = (\xeta^T_{k+1} - x_*^T, u_{k+1}) = (\xeta_k^T F_x, + // \xeta_k^T F_u) + fos_reverse(tapeId_, dim_x, dim_xu, xeta.data(), xeta_u.data()); - for (size_t i = 0; i < dim_x; ++i) - U[i] += x_fix_bar[i]; - - for (size_t i = 0; i < dim_x; ++i) - xi[i] = U[i] - xi[i]; + // compoute xeta_u[0, ... dim_x - 1] = \xeta_{k+1}^T and residual + for (size_t i = 0; i < dim_x; ++i) { + xeta_u[i] += x_fix_bar[i]; // = \xeta_{k+1}^T + xeta[i] = xeta_u[i] - xeta[i]; // residual + } - err = current->norm_deriv_func(xi, dim_x); - if (err < current->epsilon_deriv) { - for (size_t i = 0; i < dim_xu - dim_x; ++i) { - xu_bar[dim_x + i] += U[dim_x + i]; - } + err = problem.norm_deriv_func(xeta.data(), dim_x); + assert(err >= 0 && "Error should not be negative"); + // check convergence + if (err < problem.epsilon_deriv) { + // copy u_* to xu[dim_x, ..., dim_u -1] + for (size_t i = 0; i < dim_xu; ++i) + xu_bar[i] += xeta_u[i]; - delete[] xi; - delete[] U; return k; } } - for (size_t i = 0; i < dim_xu - dim_x; ++i) - xu_bar[dim_x + i] += U[dim_x + i]; - - delete[] xi; - delete[] U; + // we are not converged in maxIter + // copy our best value for u_* + for (size_t i = 0; i < dim_xu; ++i) + xu_bar[i] += xeta_u[i]; return -1; } -int fp_iteration(short tapeId, size_t sub_tape_num, double_F double_func, - adouble_F adouble_func, norm_F norm_func, - norm_deriv_F norm_deriv_func, double epsilon, - double epsilon_deriv, size_t N_max, size_t N_max_deriv, - adouble *x_0, adouble *u, adouble *x_fix, size_t dim_x, - size_t dim_u) { - - // declare extern differentiated function and data - ext_diff_fct *edf_iteration = reg_ext_fct(tapeId, sub_tape_num, &iteration); - edf_iteration->zos_forward = &fp_zos_forward; - edf_iteration->fos_forward = &fp_fos_forward; - edf_iteration->fos_reverse = &fp_fos_reverse; - - // add new fp information - fpi_stack.emplace_back(fpi_data{ - edf_iteration->index, - sub_tape_num, - double_func, - adouble_func, - norm_func, - norm_deriv_func, - epsilon, - epsilon_deriv, - N_max, - N_max_deriv, - }); - std::vector u_vals(dim_u); - std::vector x_vals(dim_x); - - ValueTape &tape = findTape(tapeId); - // ensure that the adoubles are contiguous - tape.ensureContiguousLocations(dim_x + dim_u); - // to reset old default later - const short last_default_tape_id = currentTape().tapeId(); - // ensure that the "new" allocates the adoubles for the "tape" - setCurrentTape(tape.tapeId()); - // put x and u together - adouble *xu = new adouble[dim_x + dim_u]; - - for (size_t i = 0; i < dim_x; ++i) - xu[i] = x_0[i]; - - for (size_t i = 0; i < dim_u; ++i) - xu[dim_x + i] = u[i]; +int fp_hos_ti_reverse(short tapeId, size_t dim_x, double **x_fix_bar, + size_t dim_xu, size_t degree, double **xu_bar, + double **dpp_x, double **dpp_y) { + + // assumption: x_N = Fx(x_N,u) \dot{x}_N + Fu(x_N,u)\dot{u} is taped! + // assumption: x_N = F(x_N, u) is taped! + + std::cout << "TODO: add ADOLCError, document hos_ti_reverse" << std::endl; + if (degree > 1) + std::cerr << "fp_hos_reverse is not defined for degree != 1" << std::endl; + + // reference because we set the "isInternal" + FpProblem &problem = getFpProblem(findTape(tapeId).ext_diff_fct_index()); + + // 1. compute [\bar{xi_N}, \bar{u}] via fp_fos_reverse (line 8-10 in algo) + std::vector xi_u_bar(dim_xu); + std::fill(xi_u_bar.begin(), xi_u_bar.end(), 0.0); + + // use internal Tape! + problem.isInternal = true; + fp_fos_reverse(problem.tapeId, dim_x, x_fix_bar[0], dim_xu, xi_u_bar.data(), + nullptr, nullptr); + problem.isInternal = false; + + // 2. compute r and u via hos_reverse of subTapeNum, thus we have + // to keep the (line 13 and part of line 21) Here we need the + // taped fos_forward of the fp iteration -> keep = 2 + std::vector xi_bar(xi_u_bar.begin(), xi_u_bar.begin() + dim_x); + hos_reverse(problem.subTapeId, dim_x, dim_xu, 1, xi_bar.data(), xu_bar); + + // We now have xu_bar[1] = [xi_k^T(Fxx \dot{x} + Fxu \dot{u}), + // xi_k^T(Fux \dot{x} + Fuu \dot(u))] + + // 3. compute fp_fos_reverse with \bar{xi} and \bar{x} = \dot{\bar{r}} = + // \bar{xu}[1][0] + std::vector xi_bar_dot(dim_x); + std::vector r_bar_dot(dim_x); + for (auto i = 0; i < dim_x; ++i) { + r_bar_dot[i] = xu_bar[i][1]; + } - const int k = call_ext_fct(edf_iteration, dim_x + dim_u, xu, dim_x, x_fix); + problem.isInternal = true; + fp_fos_reverse(problem.tapeId, dim_x, r_bar_dot.data(), dim_xu, + xi_bar_dot.data(), nullptr, nullptr); + problem.isInternal = false; + // We now have xi_bar_dot = [xi_K^T Fx + \bar{r}, xi_K^T Fu] - // read out x_fix - for (size_t i = 0; i < dim_x; ++i) - x_vals[i] = x_fix[i].value(); + // 4. return xi_k, u = u part of last fos_reverse + u part of hos_reverse + for (auto i = 0; i < problem.dim_u; ++i) + xu_bar[dim_x + i][1] += xi_bar_dot[dim_x + i]; - // read out xu - for (size_t i = 0; i < dim_u; ++i) - u_vals[i] = xu[i].value(); + return 0; +} - setCurrentTape(sub_tape_num); - currentTape().ensureContiguousLocations(2 * (dim_u + dim_x)); - adouble *x_fix_new = new adouble[dim_u + dim_x]; - adouble *xu_sub_tape = new adouble[dim_u + dim_x]; +int firstOrderFp(const FpProblem &problem) { + std::cout << "TODO: document firstOrderFP" << std::endl; + auto edfIteration = registerFpIteration(problem); - // read out x_fix - for (size_t i = 0; i < dim_x; ++i) - x_fix_new[i] = x_vals[i]; + FixedPoint fp{.x = std::vector(problem.dim_x), + .u = std::vector(problem.dim_u)}; - // tape near solution - trace_on(sub_tape_num, 1); + ValueTape &tape = findTape(problem.tapeId); + // ensure that the adoubles are contiguous + tape.ensureContiguousLocations(problem.dim_x + problem.dim_u); + // to reset old default later + const short last_default_tape_id = currentTape().tapeId(); + // ensure that the "new" allocates the adoubles for the "tape" + setCurrentTape(tape.tapeId()); + prepareFixedPoint(edfIteration, problem, fp); + setCurrentTape(problem.subTapeId); + tapeLastFpIteration(problem.subTapeId, problem, fp); - for (size_t i = 0; i < dim_x; ++i) - // xu[i] <<= x_fix[i].value(); - xu_sub_tape[i] <<= x_vals[i]; + // reset previous default tape + setCurrentTape(last_default_tape_id); - for (size_t i = 0; i < dim_u; ++i) - // xu[dim_x + i] <<= u[i].value(); - xu_sub_tape[dim_x + i] <<= u_vals[i]; + return fp.lastIter; +} - // IMPORTANT: Dont reuse x_fix here. The location of the x_fix's adoubles - // could change and the old locations are already stored on the tape. This - // would cause errors - adouble_func(xu_sub_tape, xu_sub_tape + dim_x, x_fix_new, dim_x, dim_u); +int secondOrderFp(const FpProblem &problem) { + std::cout << "TODO: document secondOrderFP" << std::endl; + auto edfIteration = registerFpIteration(problem); + ValueTape &tape = findTape(problem.tapeId); - double dummy_out; - for (size_t i = 0; i < dim_x; ++i) - x_fix_new[i] >>= dummy_out; + FixedPoint fp{.x = std::vector(problem.dim_x), + .u = std::vector(problem.dim_u)}; - trace_off(); + // ensure that the adoubles are contiguous + tape.ensureContiguousLocations(problem.dim_x + problem.dim_u); + // to reset old default later + const short last_default_tape_id = currentTape().tapeId(); + // ensure that the "new" allocates the adoubles for the "tape" + setCurrentTape(tape.tapeId()); + prepareFixedPoint(edfIteration, problem, fp); + setCurrentTape(problem.subTapeId); + tapeLastFpIteration(problem.subTapeId, problem, fp); - delete[] xu_sub_tape; - delete[] x_fix_new; + setCurrentTape(problem.internalTapeId); + tapeLastFpIteration(problem.internalTapeId, problem, fp); - setCurrentTape(tapeId); - delete[] xu; - // reset default tape setCurrentTape(last_default_tape_id); - - return k; + return fp.lastIter; } +}; // namespace ADOLC::FpIteration \ No newline at end of file