void soclcp_unitary_compute_and_add_error(double *z , double *w, unsigned int dim, double mu, double * error,
double * worktmp)
{
double rho = 1.0;
for (unsigned int i =0; i < dim; ++i)
{
worktmp[i] = z[i] - rho * w[i];
}
/* printf("mu = %f\n", mu); */
/* for (int i=0; i < dim; i++ ) */
/* { */
/* printf("-- worktmp[%i]=%e\t\t\t",i,(worktmp)[i]); */
/* printf("z[%i]=%e\n",i,(z)[i]); */
/* } */
projectionOnSecondOrderCone(worktmp, mu, dim);
/* for (int i=0; i < dim; i++ ) */
/* { */
/* printf("-- worktmp[%i]=%e\t\t\n",i,(worktmp)[i]); */
/* } */
for (unsigned int i =0; i < dim; ++i)
{
worktmp[i] = z[i] - worktmp[i];
*error += worktmp[i] * worktmp[i];
}
}
int soclcp_compute_error_v(SecondOrderConeLinearComplementarityProblem* problem, double *z , double *w, double tolerance, SolverOptions *options, double * error)
{
/* Checks inputs */
if(problem == NULL || z == NULL || w == NULL)
numericsError("soclcp_compute_error", "null input for problem and/or z and/or w");
/* Computes w = Mz + q */
int incx = 1, incy = 1;
int nc = problem->nc;
int n = problem->n;
double *mu = problem->mu;
double invmu = 0.0;
cblas_dcopy(n , problem->q , incx , z , incy); // z <-q
// Compute the current reaction
prodNumericsMatrix(n, n, 1.0, problem->M, w, 1.0, z);
*error = 0.;
double rho = 1.0;
for(int ic = 0 ; ic < nc ; ic++)
{
int dim = problem->coneIndex[ic+1]-problem->coneIndex[ic];
double * worktmp = (double *)malloc(dim*sizeof(double)) ;
int nic = problem->coneIndex[ic];
for (int i=0; i < dim; i++)
{
worktmp[i] = w[nic+i] - rho * z[nic+i];
}
invmu = 1.0 / mu[ic];
projectionOnSecondOrderCone(worktmp, invmu, dim);
for (int i=0; i < dim; i++)
{
worktmp[i] = w[nic+i] - worktmp[i];
*error += worktmp[i] * worktmp[i];
}
free(worktmp);
}
*error = sqrt(*error);
/* Computes error */
double normq = cblas_dnrm2(n , problem->q , incx);
*error = *error / (normq + 1.0);
if(*error > tolerance)
{
/* if (verbose > 0) printf(" Numerics - soclcp_compute_error_velocity failed: error = %g > tolerance = %g.\n",*error, tolerance); */
return 1;
}
else
return 0;
}
void Projection_VI_SOCLCP(void *viIn, double *x, double *PX)
{
DEBUG_PRINT("Projection_VI_SOCLCP(void *viIn, double *x, double *PX)\n")
VariationalInequality * vi = (VariationalInequality *) viIn;
SecondOrderConeLinearComplementarityProblem_as_VI* pb = (SecondOrderConeLinearComplementarityProblem_as_VI*)vi->env;
SecondOrderConeLinearComplementarityProblem * soclcp = pb->soclcp;
//SecondOrderConeLinearComplementarityProblem_display(soclcp);
int cone =0;
int n = soclcp->n;
cblas_dcopy(n , x , 1 , PX, 1);
int dim;
for (cone = 0 ; cone < soclcp->nc ; ++cone)
{
dim=soclcp->coneIndex[cone+1]-soclcp->coneIndex[cone];
projectionOnSecondOrderCone(&PX[soclcp->coneIndex[cone]], soclcp->tau[cone], dim);
}
}
