static void DeleteEverythingVerySlowly(vector *numbers)
{
long largestOriginalNumber;
fprintf(stdout, "Erasing everything in the vector by repeatedly deleting the 100th-to-last remaining element (be patient).\n");
fflush(stdout);
largestOriginalNumber = *(long *)VectorNth(numbers, VectorLength(numbers) - 1);
while (VectorLength(numbers) >= 100) {
VectorDelete(numbers, VectorLength(numbers) - 100);
assert(largestOriginalNumber == *(long *)VectorNth(numbers, VectorLength(numbers) -1));
}
fprintf(stdout, "\t[Okay, almost done... deleting the last 100 elements... ");
fflush(stdout);
while (VectorLength(numbers) > 0) VectorDelete(numbers, 0);
fprintf(stdout, "and we're all done... whew!]\n");
fflush(stdout);
}
void HtResetContents(LPVECTOR *table, unsigned int tablelen) {
unsigned int i;
for (i = 0; i != tablelen; i++) {
if (table[i]) {
VectorDelete(table[i]);
table[i] = NULL;
}
}
}
void MassiveRadixSortTest()
{
int vectorSize, i, j;
int num1, num2;
Vector* vec;
srand(time(NULL));
for(j = 0; j < RUNS; j++)
{
vectorSize = rand() % MAX_VECTOR_SIZE;
vec = VectorCreate(vectorSize, 100);
for(i = 0; i < vectorSize; i++)
{
VectorAdd(vec, rand() % MAX_NUMBER);
}
RadixSort(vec, 3);
VectorDelete(vec, &num1);
for(i = 0; i < vectorSize - 1; i++)
{
VectorDelete(vec, &num2);
if(num2 > num1)
{
printf("MassiveSortTest..................FAIL\n");
VectorDestroy(vec);
return;
}
num1 = num2;
}
VectorDestroy(vec);
}
printf("MassiveSortTest..................OK\n");
}
void MassiveGenericSortTestWithNegatives(ADTErr(*SortFunc)(Vector* _vec))
{
int vectorSize, i, j;
int num1, num2;
Vector* vec;
srand(time(NULL));
for(j = 0; j < RUNS; j++)
{
vectorSize = rand() % MAX_VECTOR_SIZE;
vec = VectorCreate(vectorSize, 100);
for(i = 0; i < vectorSize; i++)
{
i % 2 == 0 ? VectorAdd(vec, rand() % MAX_NUMBER) : VectorAdd(vec, - rand() % MAX_NUMBER);
}
SortFunc(vec);
VectorDelete(vec, &num1);
for(i = 0; i < vectorSize - 1; i++)
{
VectorDelete(vec, &num2);
if(num2 > num1)
{
printf("MassiveSortTestWithNegatives..................FAIL\n");
VectorDestroy(vec);
return;
}
num1 = num2;
}
VectorDestroy(vec);
}
printf("MassiveSortTestWithNegatives..................OK\n");
}
static void TestInsertDelete(vector *alphabet)
{
char ch = '-';
int i;
for (i = 3; i < VectorLength(alphabet); i += 4) // Insert dash every 4th char
VectorInsert(alphabet, &ch, i);
fprintf(stdout, "\nAfter insert dashes: ");
VectorMap(alphabet, PrintChar, stdout);
for (i = 3; i < VectorLength(alphabet); i += 3) // Delete every 4th char
VectorDelete(alphabet, i);
fprintf(stdout, "\nAfter deleting dashes: ");
VectorMap(alphabet, PrintChar, stdout);
ch = '!';
VectorInsert(alphabet, &ch, VectorLength(alphabet));
VectorDelete(alphabet, VectorLength(alphabet) - 1);
fprintf(stdout, "\nAfter adding and deleting to very end: ");
VectorMap(alphabet, PrintChar, stdout);
}
void SimpleRadixSort()
{
Vector* vec;
int a;
vec = VectorCreate(100, 100);
VectorAdd(vec, 46);
VectorAdd(vec, 2);
VectorAdd(vec, 83);
VectorAdd(vec, 41);
VectorAdd(vec, 102);
VectorAdd(vec, 5);
VectorAdd(vec, 17);
VectorAdd(vec, 31);
VectorAdd(vec, 64);
VectorAdd(vec, 49);
VectorAdd(vec, 18);
RadixSort(vec, 3);
VectorDelete(vec, &a);
VectorDelete(vec, &a);
VectorDelete(vec, &a);
if(a == 64)
{
printf("SimpleSort..................OK\n");
}
else
{
printf("SimpleSort..................FAIL\n");
}
VectorDestroy(vec);
}
void DetachParent(GameObject* gameObject){
if(gameObject->parent != NULL){
int i;
for(i = 0; i< VectorTotal(&gameObject->parent->childs); i++){
if(gameObject == VectorGet(&gameObject->parent->childs, i)){
VectorDelete(&gameObject->parent->childs, i);
}
}
float PI = 3.14159265;
float rad = gameObject->parent->rotation * PI / 180.0f;
Point localPosition = gameObject->position;
gameObject->position.x = gameObject->parent->position.x + (int)((float)localPosition.x * cos(rad) - (float)localPosition.y * sin(rad));
gameObject->position.y = gameObject->parent->position.y + (int)((float)localPosition.x * sin(rad) + (float)localPosition.y * cos(rad));
gameObject->scale = gameObject->parent->scale * gameObject->scale;
gameObject->rotation = fmod(gameObject->parent->rotation + gameObject->rotation, 360);
gameObject->parent = NULL;
}
}
void SimpleGenericSort(ADTErr(*SortFunc)(Vector* _vec))
{
Vector* vec;
int a;
vec = VectorCreate(100, 100);
VectorAdd(vec, 46);
VectorAdd(vec, 2);
VectorAdd(vec, 83);
VectorAdd(vec, 41);
VectorAdd(vec, 102);
VectorAdd(vec, 5);
VectorAdd(vec, 17);
VectorAdd(vec, 31);
VectorAdd(vec, 64);
VectorAdd(vec, 49);
VectorAdd(vec, 18);
/*VectorPrint(vec);*/
SortFunc(vec);
/*VectorPrint(vec);*/
VectorDelete(vec, &a);
if(a == 102)
{
printf("SimpleSort..................OK\n");
}
else
{
printf("SimpleSort..................FAIL\n");
}
VectorDestroy(vec);
}
void SimpleRadixSortForSingleMember()
{
Vector* vec;
int a;
vec = VectorCreate(100, 100);
VectorAdd(vec, 5);
RadixSort(vec, 1);
VectorDelete(vec, &a);
if(a == 5)
{
printf("SimpleSortForSingleMember..................OK\n");
}
else
{
printf("SimpleSortForSingleMember..................FAIL\n");
}
VectorDestroy(vec);
}
void SimpleGenericSortForSingleMember(ADTErr(*SortFunc)(Vector* _vec))
{
Vector* vec;
int a;
vec = VectorCreate(100, 100);
VectorAdd(vec, 5);
SortFunc(vec);
VectorDelete(vec, &a);
if(a == 5)
{
printf("SimpleSortForSingleMember..................OK\n");
}
else
{
printf("SimpleSortForSingleMember..................FAIL\n");
}
VectorDestroy(vec);
}
void _householder(double **Q, double **R, double **B, int n, int m) {
int i, j, k;
Vector Beta = VectorNew(m);
Vector W = VectorNew(n);
Matrix V = MatrixNew(n, n);
double *beta = Beta->e;
// Q = I
for (i = 0; i < n; i++) {
for (j = 0; j < n; j++) {
if (i == j) {
Q[i][j] = 1.0;
} else {
Q[i][j] = 0.0;
}
}
}
// R = B
for (i = 0; i < n; i++) {
double *Ri = R[i];
double *Bi = B[i];
for (k = 0; k < m; k++) {
Ri[k] = Bi[k];
}
}
// find the Householder vectors for each column of R
for (k = 0; k < m; k++) {
double xi;
double norm_x = 0.0;
double *w = W->e;
double *v = V->e[k];
for (i = k; i < n; i++) {
xi = R[i][k];
norm_x += xi * xi;
v[i] = xi;
}
norm_x = sqrt(norm_x);
double x1 = v[k];
double sign_x1 = sign(x1);
double gamma = -1.0 * sign_x1 * norm_x;
double v_dot_v = 2.0 * norm_x * (norm_x + sign_x1 * x1);
if (v_dot_v < DBL_EPSILON) {
for (i = k; i < m; i++) {
R[i][i] = 0.0;
}
VectorDelete(Beta);
VectorDelete(W);
MatrixDelete(V);
return;
}
v[k] -= gamma;
beta[k] = -2.0 / v_dot_v;
// w(k:m) = R(k:n, k:m)^T * v(k:n)
for (i = k; i < m; i++) {
double s = 0.0;
for (j = k; j < n; j++) { // FIX: make this row-wise
s += R[j][i] * v[j];
}
w[i] = s;
}
// R(k:n, k:m) += beta * v(k:n) * w(k:m)^T
//a[k : n, k : m] = a[k : n, k : m] + beta * (v * wT)
for (i = k; i < n; i++) {
for (j = k; j < m; j++) {
R[i][j] += beta[k] * v[i] * w[j];
}
}
}
for (k = m-1; k >= 0; k--) {
double *v = V->e[k];
double *u = W->e;
//uT = v.transpose() * Q[k : n, k : n]
for (i = k; i < n; i++) {
double s = 0.0;
for (j = k; j < n; j++) {
s += Q[j][i] * v[j];
}
u[i] = s;
}
//Q[k : n, k : n] = Q[k : n, k : n] + Beta[k] * (v * uT)
for (i = k; i < n; i++) {
for (j = k; j < n; j++) {
Q[i][j] += beta[k] * v[i] * u[j];
}
}
}
VectorDelete(Beta);
VectorDelete(W);
MatrixDelete(V);
}
double _RKOCPfhgE(Vector nlp_x, Vector nlp_h, Vector nlp_g) {
RKOCP r = thisRKOCP;
double phi = 0;
double t;
int i_node, l;
int i, j;
int m = r->nlp_m;
int p = r->nlp_p;
int n_nodes = r->n_nodes;
int n_parameters = r->n_parameters;
int n_states = r->n_states;
int n_controls = r->n_controls;
int n_inequality = r->ocp->n_inequality;
int n_initial = r->ocp->n_initial;
int n_terminal = r->ocp->n_terminal;
int rk_stages = r->rk_stages;
double **rk_a = r->rk_a->e;
double *rk_b = r->rk_b->e;
double *rk_c = r->rk_c->e;
double *rk_e = r->rk_e->e;
Vector T = r->T;
Matrix Yw = r->Yw;
Vector yc = r->yc;
Vector uc = r->uc;
Vector pc = r->pc;
Vector local_error = r->local_error;
Vector lte = r->lte;
Vector Gamma = r->Gamma;
Vector G = r->G;
Vector Psi = r->Psi;
Matrix *Ydata = r->Ydata;
Matrix *Udata = r->Udata;
Vector *Kstage = r->Kstage;
double _y;
double fac0 = 0.2, fac1 = 5.0, beta = 0.9;
int step, max_steps = 100;
Vector Y0, Y1;
double T0, h = 1.0e-8;
Y0 = VectorNew(n_states+1);
Y1 = VectorNew(n_states+1);
if (m > 0) {
VectorSetAllTo(nlp_h, 0.0);
}
if (p > 0) {
VectorSetAllTo(nlp_g, 0.0);
}
for (i = 0; i < n_parameters; i++) {
pc->e[i] = nlp_x->e[i];
}
for (j = 0; j < n_states; j++) {
Yw->e[0][j] = nlp_x->e[n_parameters+j];
}
Yw->e[0][n_states] = 0;
if (n_controls > 0) {
_setUdata(r, nlp_x);
}
// integrate the ODEs using an explicit Runge-Kutta method
Matrix Ystage;
double delta, ti;
for (i_node = 0; i_node < n_nodes-1; i_node++) {
Ystage = Ydata[i_node]; // save the stage values for use in Dfhg
ti = T->e[i_node];
delta = T->e[i_node+1] - ti;
t = ti;
//- save the control values for use in Dfhg
//- getControl(i_node, t, Udata[i_node][0]);
for (i = 0; i < n_controls; i++)
uc->e[i] = Udata[i_node]->e[0][i];
for (i = 0; i < n_states+1; i++) {
_y = Yw->e[i_node][i];
Ystage->e[0][i] = _y;
yc->e[i] = _y;
Y0->e[i] = _y;
}
// form Gamma(y(t_initial), p)
if ((i_node == 0) && (n_initial > 0)) {
r->ocp->initial_constraints(yc, pc, Gamma);
for (i = 0; i < n_initial; i++) {
nlp_h->e[i] = Gamma->e[i];
}
}
// form g(y,u,p,t)
if (n_inequality > 0) {
r->ocp->inequality_constraints(yc, uc, pc, t, G);
for (i = 0; i < n_inequality; i++) {
nlp_g->e[i + i_node * n_inequality] = G->e[i];
}
}
// K = F(y,u,p,t)
Kstage[0]->e[n_states] = r->ocp->differential_equations(yc, uc, pc, t, Kstage[0]);
T0 = t;
if (i_node == 0)
h = delta;
for (step = 0; step <= max_steps; step++) {
for (l = 1; l < rk_stages; l++) {
t = T0 + rk_c[l]*h;
//getControl(i_node, t, Udata[i_node][l]);
//for (i = 0; i < n_controls; i++)
// uc->e[i] = Udata[i_node]->e[0][i]; // constant control
//Y_l = yc + h * sum_{j=0 to j=l-1} rk_a[l][j] * K_j
for (i = 0; i < n_states + 1; i++)
Ystage->e[l][i] = Y0->e[i];//Yw->e[i_node][i];
for (j = 0; j < l; j++) {
for (i = 0; i < n_states+1; i++) {
Ystage->e[l][i] += h*rk_a[l][j]*Kstage[j]->e[i];
}
}
for (i = 0; i < n_states+1; i++)
yc->e[i] = Ystage->e[l][i];
Kstage[l]->e[n_states] = r->ocp->differential_equations(yc, uc, pc, t, Kstage[l]);
}
// Y1 = Y0 + h * sum_{l=0 to stages-1} b[l] * K[l]
for (i = 0; i < n_states+1; i++) {
Y1->e[i] = Y0->e[i];
local_error->e[i] = 0;
}
for (l = 0; l < rk_stages; l++) {
for (i = 0; i < n_states+1; i++) {
Y1->e[i] += h*rk_b[l]*Kstage[l]->e[i];
local_error->e[i] += h*rk_e[l]*Kstage[l]->e[i];
}
}
lte->e[i_node+1] = _compute_LTE(r, local_error->e, Y0->e, Y1->e);
double sigma = pow(lte->e[i_node+1], -1.0/r->rk_order);
double hnew = h * fmin(fac1, fmax(fac0, beta*sigma));
//double _dt = fabs(T0 + h - T->e[i_node+1]);
if (lte->e[i_node+1] < 1.0) {
T0 = T0 + h;
if (fabs(T0-T->e[i_node+1]) <= 100.0*DBL_EPSILON) {
for (i = 0; i < n_states+1; i++)
Yw->e[i_node+1][i] = Y1->e[i];
break;
}
for (i = 0; i < n_states+1; i++) {
Y0->e[i] = Y1->e[i];
yc->e[i] = Y1->e[i];
}
Kstage[0]->e[n_states] = r->ocp->differential_equations(yc, uc, pc, T0, Kstage[0]);
}
if (step >= max_steps) {
for (i = 0; i < n_states+1; i++)
Yw->e[i_node+1][i] = Y1->e[i];
/*printf("local error:\n");
VectorPrint(local_error);
printf("Y1:\n");
VectorPrint(Y1);
printf("step: %d, lte: %e, T0: %e, T1: %e, h: %e, hnew: %e, sigma: %e, _dt: %e\n", step, lte->e[i_node+1], T0, T->e[i_node+1], h, hnew, sigma, _dt);
pause();*/
break;
}
/*printf("control:\n");
VectorPrint(uc);
printf("local error:\n");
VectorPrint(local_error);
printf("step: %d, lte: %e, T0: %e, T1: %e, h: %e, hnew: %e, sigma: %e, _dt: %e\n", step, lte->e[i_node+1], T0, T->e[i_node+1], h, hnew, sigma, _dt);
pause();*/
h = hnew;
if (T0+h > T->e[i_node+1])
h = T->e[i_node+1] - T0;
//
}
//pause();
}
// form g at time t_final
t = T->e[n_nodes-1];
if (n_controls > 0) {
//getControl(n_nodes - 2, t, Udata[n_nodes - 1][0]);
for (i = 0; i < n_controls; i++)
uc->e[i] = Udata[n_nodes-2]->e[rk_stages-1][i];
}
for (i = 0; i < n_states; i++)
yc->e[i] = Yw->e[n_nodes-1][i];
if (n_inequality > 0) {
r->ocp->inequality_constraints(yc, uc, pc, t, G);
for (i = 0; i < n_inequality; i++) {
nlp_g->e[i + (n_nodes-1)*n_inequality] = G->e[i];
}
}
// form phi, Psi
phi = r->ocp->terminal_constraints(yc, pc, Psi);
for (i = 0; i < n_terminal; i++) {
nlp_h->e[i + n_initial] = Psi->e[i];
}
VectorDelete(Y0);
VectorDelete(Y1);
return phi + Yw->e[n_nodes-1][n_states];
}
double _RKOCPfhg_implicitZ(Vector nlp_x, Vector nlp_h, Vector nlp_g) {
RKOCP r = thisRKOCP;
double phi = 0;
int i_node;
int i, j, k;
int m = r->nlp_m;
int p = r->nlp_p;
int n_nodes = r->n_nodes;
int n_parameters = r->n_parameters;
int n_states = r->n_states;
int n_controls = r->n_controls;
int n_inequality = r->ocp->n_inequality;
int n_initial = r->ocp->n_initial;
int n_terminal = r->ocp->n_terminal;
int only_compute_lte = r->only_compute_lte;
int rk_stages = r->rk_stages;
double **W = r->rk_w->e;
//double **rk_a = r->rk_a->e;
//double *rk_b = r->rk_b->e;
double *rk_c = r->rk_c->e;
//double *rk_e = r->rk_e->e;
Vector T = r->T;
Matrix Yw = r->Yw;
Vector yc = r->yc;
Vector uc = r->uc;
Vector pc = r->pc;
//Vector local_error = r->local_error;
//Vector lte = r->lte;
Vector Gamma = r->Gamma;
Vector G = r->G;
Vector Psi = r->Psi;
Matrix *Ydata = r->Ydata; // Ydata[i] => Ystage
Matrix *Udata = r->Udata;
Vector *Zstage = r->Kstage;
Matrix Ystage;
double ti;
// FIX: preallocate the variables
Vector y_old = VectorNew(n_states+1);
Vector *Z_old = NULL;
Z_old = VectorArrayNew(r->rk_stages, n_states+1);
Vector f = VectorNew(n_states+1);
Vector res = VectorNew(rk_stages*(n_states+1));
Vector dZ = VectorNew(rk_stages*(n_states+1));
Vector y = VectorNew(n_states+1);
Matrix L;// = MatrixNew(rk_stages*(n_states+1), rk_stages*(n_states+1));
Matrix U;// = MatrixNew(rk_stages*(n_states+1), rk_stages*(n_states+1));
int *P;// = (int *)malloc(rk_stages*(n_states+1)*sizeof(int));
if (only_compute_lte == NO) {
if (m > 0) {
VectorSetAllTo(nlp_h, 0.0);
}
if (p > 0) {
VectorSetAllTo(nlp_g, 0.0);
}
}
for (i = 0; i < n_parameters; i++) {
pc->e[i] = nlp_x->e[i];
}
for (j = 0; j < n_states; j++) {
Yw->e[0][j] = nlp_x->e[n_parameters+j];
y->e[j] = Yw->e[0][j];
}
Yw->e[0][n_states] = 0;
y->e[n_states] = 0;
if (n_controls > 0) {
_setUdata(r, nlp_x);
for (i = 0; i < n_controls; i++) {
uc->e[i] = nlp_x->e[n_parameters+n_states+i];
}
}
if (only_compute_lte == NO) {
// form Gamma(y(t_initial), p)
if (n_initial > 0) {
r->ocp->initial_constraints(y, pc, Gamma);
for (i = 0; i < n_initial; i++) {
nlp_h->e[i] = Gamma->e[i];
}
}
// form g(y,u,p,t_initial)
if (n_inequality > 0) {
r->ocp->inequality_constraints(y, uc, pc, T->e[0], G);
for (i = 0; i < n_inequality; i++) {
nlp_g->e[i] = G->e[i];
}
}
}
// integrate the ODEs using an implicit Runge-Kutta method
int MAX_NEWTON_ITER = 15;
int iter;
int err;
for (i_node = 0; i_node < (n_nodes-1); i_node++) {
L = r->Lnode[i_node];
U = r->Unode[i_node];
P = r->pLUnode[i_node];
// form H = (1/h) W (x) I - I (x) J
// get the L, U, p factors of H
double h = T->e[i_node+1] - T->e[i_node];
err = _formIRKmatrixZ(r, y, uc, pc, T->e[i_node], h, L, U, P);
if (err != 0) {
printf("IRK factor error: t = %e\n", T->e[i_node]);
//pause();
r->integration_fatal_error = YES;
if (m > 0) VectorSetAllTo(nlp_h, 0);
if (p > 0) VectorSetAllTo(nlp_g, 0);
return 0;
}
// initialize Kstage
// FIX: extrapolate previous result
if (i_node == 0 ) {
for (i = 0; i < rk_stages; i++) {
for (j = 0; j < n_states+1; j++) {
Zstage[i]->e[j] = 0;
}
}
} else {
// given y0, Z0, y1, tau = h1/h0, estimate Z1
// using a Lagrange interpolation formula
double tau;
tau = h / (T->e[i_node] - T->e[i_node-1]);
_estimateZ(r, y_old, Z_old, y, tau, Zstage);
}
Ystage = Ydata[i_node];
// Y_i = y + Z_i
// Ydot_i = (1/h) sum_{j = 1, s} w_{i,j} * Z_j
// set res_i = -Ydot_i
for (i = 0; i < rk_stages; i++) {
for (j = 0; j < n_states+1; j++) {
Ystage->e[i][j] = y->e[j] + Zstage[i]->e[j];
}
for (k = 0; k < n_states+1; k++) {
//Ydot[i]->e[k] = 0.0;
res->e[i*(n_states+1) + k] = 0;
}
for (j = 0; j < rk_stages; j++) {
for (k = 0; k < n_states+1; k++) {
//Ydot[i]->e[k] += W[i][j] * Zstage[j]->e[k] / h;
res->e[i*(n_states+1) + k] -= W[i][j] * Zstage[j]->e[k] / h;
}
}
}
for (iter = 0; iter < MAX_NEWTON_ITER; iter++) {
for (i = 0; i < rk_stages; i++) {
ti = T->e[i_node] + h*rk_c[i];
// get uc = u(t_i)
for (j = 0; j < n_controls; j++) {
uc->e[j] = Udata[i_node]->e[i][j];
}
// get yc = Y_i
for (j = 0; j < n_states+1; j++) {
yc->e[j] = Ystage->e[i][j];
}
// form f(Y_i, u_i, p, t_i)
f->e[n_states] = r->ocp->differential_equations(yc, uc, pc, ti, f);
// form res_i = Ydot_i - f(Y_i, u_i, p, t_i)
for (j = 0; j < n_states+1; j++) {
res->e[i*(n_states+1) + j] += f->e[j];
}
}
// solve H*dZ = -res = -(ydot - f(y,u,p,t))
err = LUSolve(L, U, P, res, dZ);
// Z = Z + dZ
for (i = 0; i < rk_stages; i++) {
for (j = 0; j < n_states+1; j++) {
Zstage[i]->e[j] += dZ->e[i*(n_states+1) + j];
}
}
// Y_i = y + Z_i
// Ydot_i = (1/h) sum_{j = 1, s} w_{i,j} * Z_j
// set res_i = -Ydot_i
for (i = 0; i < rk_stages; i++) {
for (j = 0; j < n_states+1; j++) {
Ystage->e[i][j] = y->e[j] + Zstage[i]->e[j];
}
for (k = 0; k < n_states+1; k++) {
//Ydot[i]->e[k] = 0.0;
res->e[i*(n_states+1) + k] = 0;
}
for (j = 0; j < rk_stages; j++) {
for (k = 0; k < n_states+1; k++) {
//Ydot[i]->e[k] += W[i][j] * Zstage[j]->e[k] / h;
res->e[i*(n_states+1) + k] -= W[i][j] * Zstage[j]->e[k] / h;
}
}
}
// test for convergence
double norm_dZ = VectorNorm(dZ);
if (norm_dZ <= 0.01*r->tolerance) {
break;
}
}
//printf("i_node: %d, iter: %d\n", i_node, iter);
// y_old = y
// Z_old = Z
for (j = 0; j < n_states+1; j++) {
for (i = 0; i < rk_stages; i++) {
Z_old[i]->e[j] = Zstage[i]->e[j];
}
y_old->e[j] = y->e[j];
}
// Assumes R-K methods with c[s] = 1
// form Yw[i_node+1] = Ystage_s
// update y = Yw[i_node+1]
for (j = 0; j < n_states+1; j++) {
Yw->e[i_node+1][j] = Ystage->e[rk_stages-1][j];
y->e[j] = Yw->e[i_node+1][j];
}
// update u = u[i_node+1];
int i_node1 = i_node+1;
if ((r->c_type == CONSTANT) && (i_node1 == (r->n_nodes-1))) {
i_node1 -= 1;
}
for (i = 0; i < n_controls; i++) {
uc->e[i] = nlp_x->e[n_parameters + n_states + i_node1*n_controls + i];
}
// compute g(y(t+h),u(t+h),p,t+h)
if (only_compute_lte == NO) {
if (n_inequality > 0) {
r->ocp->inequality_constraints(y, uc, pc, T->e[i_node+1], G);
for (i = 0; i < n_inequality; i++) {
nlp_g->e[i + (i_node+1) * n_inequality] = G->e[i];
}
}
}
// FIX: compute LTE
}
if (only_compute_lte == YES) {
return 0;
}
// form phi, Psi
phi = r->ocp->terminal_constraints(yc, pc, Psi);
for (i = 0; i < n_terminal; i++) {
nlp_h->e[i + n_initial] = Psi->e[i];
}
VectorDelete(y_old);
VectorArrayDelete(Z_old, r->rk_stages);
VectorDelete(f);
VectorDelete(res);
VectorDelete(y);
VectorDelete(dZ);
//MatrixDelete(L);
//MatrixDelete(U);
//free(P);
//pause();
return phi + Yw->e[n_nodes-1][n_states];
}
