CAMLprim value c_densematrix_ormqr(value va, value vormqr)
{
CAMLparam2(va, vormqr);
realtype *beta = REAL_ARRAY(Field(vormqr, 0));
realtype *vv = REAL_ARRAY(Field(vormqr, 1));
realtype *vw = REAL_ARRAY(Field(vormqr, 2));
realtype *work = REAL_ARRAY(Field(vormqr, 3));
DenseORMQR(DLSMAT(va), beta, vv, vw, work);
CAMLreturn (Val_unit);
}
static int cpDenseProjSolve(CPodeMem cp_mem, N_Vector b, N_Vector x,
N_Vector y, N_Vector cy,
N_Vector c_tmp1, N_Vector s_tmp1)
{
CPDlsProjMem cpdlsP_mem;
realtype **g_mat, *bd, *xd, *col_i, *s_tmpd;
realtype *ewt_data, *d_data, *da_data, tmp;
long int nd, i, k, pk;
cpdlsP_mem = (CPDlsProjMem) lmemP;
g_mat = G->cols;
ewt_data = N_VGetArrayPointer(ewt);
bd = N_VGetArrayPointer(b);
xd = N_VGetArrayPointer(x);
d_data = N_VGetArrayPointer(s_tmp1);
nd = ny - nc;
/* Solve the linear system, depending on ftype */
switch (ftype) {
case CPDIRECT_LU:
/*
* Solve L*U1*alpha = bd
* (a) solve L*beta = bd using fwd. subst. (row version)
* (b) solve U1*alpha = beta using bckwd. subst (row version)
* where L^T and U1^T are stored in G[0...nc-1][0...nc-1].
* beta and then alpha overwrite bd.
*/
bd[0] /= g_mat[0][0];
for (i=1; i<nc; i++) {
col_i = g_mat[i];
for (k=0; k<i; k++) bd[i] -= col_i[k]*bd[k];
bd[i] /= col_i[i];
}
for (i=nc-2; i>=0; i--) {
col_i = g_mat[i];
for (k=i+1; k<nc; k++) bd[i] -= col_i[k]*bd[k];
}
/*
* Compute S^T * (D1 * alpha)
* alpha is stored in bd.
* S^T is stored in g_mat[nc...ny-1][0...nc-1].
* Store result in x2 = x[nc...ny-1].
*/
if (pnorm == CP_PROJ_ERRNORM) {
/* Load squared error weights into d */
for (k=0; k<ny; k++) d_data[k] = ewt_data[k] * ewt_data[k];
/* Permute elements of d, based on pivotsP. Swap d[k] and d[pivotsP[k]]. */
for (k=0; k<nc; k++) {
pk = pivotsP[k];
if (pk != k) {
tmp = d_data[k];
d_data[k] = d_data[pk];
d_data[pk] = tmp;
}
}
/* Compute D1*alpha and store it into da_data */
da_data = N_VGetArrayPointer(c_tmp1);
for(k=0; k<nc; k++) da_data[k] = d_data[k] * bd[k];
/* Compute S^T * D1 * alpha = S^T * da */
for(i=0; i<nd; i++) {
xd[nc+i] = ZERO;
for(k=0; k<nc; k++) xd[nc+i] += g_mat[k][nc+i]*da_data[k];
}
} else {
/* Compute S^T * alpha */
for(i=0; i<nd; i++) {
xd[nc+i] = ZERO;
for(k=0; k<nc; k++) xd[nc+i] += g_mat[k][nc+i]*bd[k];
}
}
/*
* Solve K*x2 = S^T*D1*alpha, using the Cholesky decomposition available in K.
* S^T*D1*alpha is stored in x2 = x[nc...ny-1].
*/
DensePOTRS(K, &xd[nc]);
/*
* Compute x1 = alpha - S*x2
* alpha is stored in bd.
* x2 is stored in x[nc...ny-1].
* S^T is stored in g_mat[nc...ny-1][0...nc-1].
* Store result in x1 = x[0...nc-1].
*/
for (i=0; i<nc; i++) {
xd[i] = bd[i];
col_i = g_mat[i];
for (k=nc; k<ny; k++) xd[i] -= col_i[k]*xd[k];
}
/*
* Compute P^T * x, where P is encoded into pivotsP.
* Store result in x.
*/
for (k=nc-1; k>=0; k--) {
pk = pivotsP[k];
if(pk != k) {
tmp = xd[k];
xd[k] = xd[pk];
xd[pk] = tmp;
}
}
break;
case CPDIRECT_QR:
/*
* Solve R^T*alpha = bd using fwd. subst. (row version)
* alpha overwrites bd.
*/
bd[0] /= g_mat[0][0];
for (i=1; i<nc; i++) {
col_i = g_mat[i];
for (k=0; k<i; k++) bd[i] -= bd[k]*col_i[k];
bd[i] /= col_i[i];
}
/* If projecting in WRMS norm, solve K*beta = alpha */
if (pnorm == CP_PROJ_ERRNORM) DensePOTRS(K, bd);
/* Compute x = Q*alpha */
s_tmpd = N_VGetArrayPointer(s_tmp1);
DenseORMQR(G, beta, bd, xd, s_tmpd);
/* If projecting in WRMS norm, scale x by D^(-1) */
if (pnorm == CP_PROJ_ERRNORM) {
for (i=0; i<ny; i++)
xd[i] /= ewt_data[i]*ewt_data[i];
}
break;
case CPDIRECT_SC:
/*
* Solve K*xi = bd, using the Cholesky decomposition available in K.
* xi overwrites bd.
*/
DensePOTRS(K, bd);
/* Compute x = G^T * xi
* G^T is stored in g_mat[0...ny-1][0...nc-1]
*/
for(i=0; i<ny; i++) {
xd[i] = ZERO;
for(k=0; k<nc; k++) xd[i] += g_mat[k][i]*bd[k];
}
/* If projecting in WRMS norm, scale x by D^(-1) */
if (pnorm == CP_PROJ_ERRNORM) {
for (i=0; i<ny; i++)
xd[i] /= ewt_data[i]*ewt_data[i];
}
break;
}
return(0);
}