void
solvit (double *prod, double *rhs, int n, double *ans)
{
//The coefficient matrix should be positive definite
/*AT : changed this code to take in matrix in a linear array form*/
double *ttt;
double *b;
double *p;
int i ;
ZALLOC (ttt, n*n, double);
ZALLOC (p, n, double);
ZALLOC(b,n,double);
copyarr(prod,ttt,n*n);
copyarr(rhs,b,n);
choldc (ttt, n, p);
cholsl (ttt, n, p, b, ans);
free (ttt) ;
free(b);
free (p) ;
}
static void calc_edge_projection(Element* e, int edge, Nurbs** nurbs, int order, double2* proj)
{
ref_map_pss.set_active_element(e);
int i, j, k;
int mo1 = quad1d.get_max_order();
int np = quad1d.get_num_points(mo1);
int ne = order - 1;
int mode = e->get_mode();
assert(np <= 15 && ne <= 10);
double2 fn[15];
double rhside[2][10];
memset(fn, 0, sizeof(double2) * np);
memset(rhside[0], 0, sizeof(double) * ne);
memset(rhside[1], 0, sizeof(double) * ne);
double a_1, a_2, b_1, b_2;
a_1 = ctm.m[0] * ref_vert[mode][edge][0] + ctm.t[0];
a_2 = ctm.m[1] * ref_vert[mode][edge][1] + ctm.t[1];
b_1 = ctm.m[0] * ref_vert[mode][e->next_vert(edge)][0] + ctm.t[0];
b_2 = ctm.m[1] * ref_vert[mode][e->next_vert(edge)][1] + ctm.t[1];
// values of nonpolynomial function in two vertices
double2 fa, fb;
calc_ref_map(e, nurbs, a_1, a_2, fa);
calc_ref_map(e, nurbs, b_1, b_2, fb);
double2* pt = quad1d.get_points(mo1);
for (j = 0; j < np; j++) // over all integration points
{
double2 x, v;
double t = pt[j][0];
edge_coord(e, edge, t, x, v);
calc_ref_map(e, nurbs, x[0], x[1], fn[j]);
for (k = 0; k < 2; k++)
fn[j][k] = fn[j][k] - (fa[k] + (t+1)/2.0 * (fb[k] - fa[k]));
}
double2* result = proj + e->nvert + edge * (order - 1);
for (k = 0; k < 2; k++)
{
for (i = 0; i < ne; i++)
{
for (j = 0; j < np; j++)
{
double t = pt[j][0];
double fi = lob[i+2](t);
rhside[k][i] += pt[j][1] * (fi * fn[j][k]);
}
}
// solve
cholsl(edge_proj_matrix, ne, edge_p, rhside[k], rhside[k]);
for (i = 0; i < ne; i++)
result[i][k] = rhside[k][i];
}
}
int main(void)
{
int i,j,k;
float sum,**a,**atest,**chol,*p,*x;
static float aorig[N+1][N+1]=
{0.0,0.0,0.0,0.0,
0.0,100.0,15.0,0.01,
0.0,15.0,2.3,0.01,
0.0,0.01,0.01,1.0};
static float b[N+1]={0.0,0.4,0.02,99.0};
a=matrix(1,N,1,N);
atest=matrix(1,N,1,N);
chol=matrix(1,N,1,N);
p=vector(1,N);
x=vector(1,N);
for (i=1;i<=N;i++)
for (j=1;j<=N;j++) a[i][j]=aorig[i][j];
choldc(a,N,p);
printf("Original matrix:\n");
for (i=1;i<=N;i++) {
for (j=1;j<=N;j++) {
chol[i][j]=((i > j) ? a[i][j] : (i == j ? p[i] : 0.0));
if (i > j) chol[i][j]=a[i][j];
else chol[i][j]=(i == j ? p[i] : 0.0);
printf("%16.6e",aorig[i][j]);
}
printf("\n");
}
printf("\n");
printf("Product of Cholesky factors:\n");
for (i=1;i<=N;i++) {
for (j=1;j<=N;j++) {
for (sum=0.0,k=1;k<=N;k++) sum += chol[i][k]*chol[j][k];
atest[i][j]=sum;
printf("%16.6e",atest[i][j]);
}
printf("\n");
}
printf("\n");
printf("Check solution vector:\n");
cholsl(a,N,p,b,x);
for (i=1;i<=N;i++) {
for (sum=0.0,j=1;j<=N;j++) sum += aorig[i][j]*x[j];
p[i]=sum;
printf("%16.6e%16.6e\n",p[i],b[i]);
}
free_vector(x,1,N);
free_vector(p,1,N);
free_matrix(chol,1,N,1,N);
free_matrix(atest,1,N,1,N);
free_matrix(a,1,N,1,N);
return 0;
}
int ekf_step(void * v, double * z)
{
/* unpack incoming structure */
int * ptr = (int *)v;
int n = *ptr;
ptr++;
int m = *ptr;
ptr++;
int s = *ptr;
ekf_t ekf;
unpack(v, &ekf, n, m, s);
// NON-ADDITIVE
/* P_k = F_{k-1} P_{k-1} F^T_{k-1} + L_{k-1} Y_{k-1} L^T_{k-1} + Q_{k-1} */
mulmat(ekf.F, ekf.P, ekf.tmp0, n, n, n);
transpose(ekf.F, ekf.Ft, n, n);
mulmat(ekf.tmp0, ekf.Ft, ekf.Pp, n, n, n);
mulmat(ekf.L, ekf.Y, ekf.tmp6, n, s, s);
transpose(ekf.L, ekf.Lt, n, s);
mulmat(ekf.tmp6, ekf.Lt, ekf.tmp0, n, s, n);
accum(ekf.Pp, ekf.tmp0, n, n);
accum(ekf.Pp, ekf.Q, n, n);
/* G_k = P_k H^T_k (H_k P_k H^T_k + R)^{-1} */
transpose(ekf.H, ekf.Ht, m, n);
mulmat(ekf.Pp, ekf.Ht, ekf.tmp1, n, n, m);
mulmat(ekf.H, ekf.Pp, ekf.tmp2, m, n, n);
mulmat(ekf.tmp2, ekf.Ht, ekf.tmp3, m, n, m);
accum(ekf.tmp3, ekf.R, m, m);
if (cholsl(ekf.tmp3, ekf.tmp4, ekf.tmp5, m)) return 1;
mulmat(ekf.tmp1, ekf.tmp4, ekf.G, n, m, m);
/* \hat{x}_k = \hat{x_k} + G_k(z_k - h(\hat{x}_k)) */
sub(z, ekf.hx, ekf.tmp5, m);
mulvec(ekf.G, ekf.tmp5, ekf.tmp2, n, m);
add(ekf.fx, ekf.tmp2, ekf.x, n);
/* P_k = (I - G_k H_k) P_k */
mulmat(ekf.G, ekf.H, ekf.tmp0, n, m, n);
negate(ekf.tmp0, n, n);
mat_addeye(ekf.tmp0, n);
mulmat(ekf.tmp0, ekf.Pp, ekf.P, n, n, n);
/* success */
return 0;
}
int
solvitfix (double *prod, double *rhs, int n, double *ans, int *vfix, double *vvals, int nfix)
// force variables in vfix list to vvals)
{
//The coefficient matrix should be positive definite
/*AT : changed this code to take in matrix in a linear array form */
double *ttt;
double *b;
double *p;
int i, k, t ;
int ret ;
ZALLOC (ttt, n*n, double);
ZALLOC (p, n, double);
ZALLOC(b,n,double);
copyarr(prod,ttt,n*n);
copyarr(rhs,b,n);
for (k=0; k<nfix; ++k) {
vzclear(ttt, b, n, vfix[k], vvals[k]) ;
}
ret = choldc (ttt, n, p);
if (ret<0) return -1 ; // not pos def
cholsl (ttt, n, p, b, ans);
for (k=0; k<nfix; ++k) {
t = vfix[k] ;
printf("zz solvitfix:%d %d %12.6f %12.6f\n", n, t, vvals[k], ans[t]) ;
}
free (ttt) ;
free(b);
free (p) ;
return 1 ;
}
int dirdynared(LocalDataStruct *lds,MBSdataStruct *s)
{
int i,j,k;
int iter=0;
int nL,nC,nk;
double term;
int has_a_line_of_zeros;
// Expression des variables commandées
if (s->nqc>0) user_DrivenJoints(s,s->tsim);
// Résolution des Contraintes
if (s->Ncons > 0)
{
// résolution géométrique
iter = mbs_close_geo(s, lds);
if (iter>=lds->MAX_NR_ITER)
{
return -1;
}
// résolution cinématique
mbs_close_kin(s, lds);
}//if(s->Ncons > 0)
// calcul des forces appliquées sur les corps
for(i=1;i<=s->nbody;i++)
{
for(j=1;j<=3;j++)
{
s->frc[j][i]=0.0;
s->trq[j][i]=0.0;
}
}
#ifdef CXX
if(s->Nlink > 0) link1D(s->frc,s->trq,s->Fl,s->Z,s->Zd,s,s->tsim);
#else
if(s->Nlink > 0) link(s->frc,s->trq,s->Fl,s->Z,s->Zd,s,s->tsim);
#endif
if(s->Nxfrc > 0) extforces(s->frc,s->trq,s,s->tsim);
s->Qq = user_JointForces(s,s->tsim);
// calcul de la matrice de masse et du vecteur des forces non linéaires
dirdyna(lds->M,lds->c,s,s->tsim);
for(i=1;i<=s->njoint;i++)
{
lds->F[i] = lds->c[i] - s->Qq[i];
}
// calcul de la matrice de masse et du vecteur des forces reduites (DAE => ODE)
if (s->nqv>0)
{
// Mr_uc = Muc_uc + Muc_v*Bvuc + Bvuc'*Mv_uc + Bvuc'*Mvv*Bvuc;
// Mr_uc = Muc_uc + Muc_v*Bvuc + (Muc_v*Bvuc)' + (Bvuc'*Mvv)*Bvuc;
// Fr_uc = Fuc + Muc_v*bprim + Bvuc'*Fv + Bvuc'*Mvv*bprim;
// Fr_uc = Fuc + (Muc_v+ Bvuc'*Mvv)*bprim + Bvuc'*Fv ;
// BtMvu = Bvuc'*Mv_uc
nL = nC = lds->iquc[0];
nk = s->nqv;
for(i=1;i<=nL;i++)
{
for(j=1;j<=nC;j++)
{
term = 0.0;
for(k=1;k<=nk;k++)
{
term += lds->Bvuc[k][i] * lds->M[s->qv[k]][lds->iquc[j]];
}
lds->BtMvu[i][j] = term;
}
}
// BtMvv = Bvuc'*Mvv
nL = lds->iquc[0];
nC = nk = s->nqv;
for(i=1;i<=nL;i++)
{
for(j=1;j<=nC;j++)
{
term = 0.0;
for(k=1;k<=nk;k++)
{
term += lds->Bvuc[k][i] * lds->M[s->qv[k]][s->qv[j]];
}
lds->BtMvv[i][j] = term;
}
}
// BtFv = Bvuc'*Fv
nL = lds->iquc[0];
nk = s->nqv;
for(i=1;i<=nL;i++)
{
term = 0.0;
for(k=1;k<=nk;k++)
{
term += lds->Bvuc[k][i] * lds->F[s->qv[k]];
}
lds->BtFv[i] = term;
}
// BtMB = BtMvv*Bvuc
nL = nC = lds->iquc[0];
nk = s->nqv;
for(i=1;i<=nL;i++)
{
for(j=1;j<=nC;j++)
{
term = 0.0;
for(k=1;k<=nk;k++)
{
term += lds->BtMvv[i][k] * lds->Bvuc[k][j];
}
lds->BtMB[i][j] = term;
}
}
// MBMb = (Muv+Bvuc'*Mvv)*bprim
nL = lds->iquc[0];
nk = s->nqv;
for(i=1;i<=nL;i++)
{
term = 0.0;
for(k=1;k<=nk;k++)
{
term += (lds->M[lds->iquc[i]][s->qv[k]] + lds->BtMvv[i][k]) * lds->bp[k];
}
lds->MBMb[i] = term;
}
/*
nL = lds->iquc[0];
nk = s->nqv;
for(i=1;i<=nL;i++)
{
// BtMvu = Bvuc'*Mvu
nC = lds->iquc[0];
for(j=1;j<=nC;j++)
{
term = 0.0;
for(k=1;k<=nk;k++)
{
term += lds->Bvuc[k][i] * lds->M[s->qv[k]][lds->iquc[j]];
}
lds->BtMvu[i][j] = term;
}
// BtMvv = Bvuc'*Mvv
nC = s->nqv;
for(j=1;j<=nC;j++)
{
term = 0.0;
for(k=1;k<=nk;k++)
{
term += lds->Bvuc[k][i] * lds->M[s->qv[k]][s->qv[j]];
}
lds->BtMvv[i][j] = term;
}
// BtFv = Bvuc'*Fv
term = 0.0;
for(k=1;k<=nk;k++)
{
term += lds->Bvuc[k][i] * lds->F[s->qv[k]];
}
lds->BtFv[i] = term;
// BtMB = BtMvv*Bvuc
nC = lds->iquc[0];
for(j=1;j<=nC;j++)
{
term = 0.0;
for(k=1;k<=nk;k++)
{
term += lds->BtMvv[i][k] * lds->Bvuc[k][j];
}
lds->BtMB[i][j] = term;
}
// MBMb = (Muv+Bvuc'*Mvv)*bprim
nC = s->nqv;
term = 0.0;
for(k=1;k<=nk;k++)
{
term += (lds->M[lds->iquc[i]][s->qv[k]] + lds->BtMvv[i][k]) * lds->bp[k];
}
lds->MBMb[i] = term;
}
*/
// Mruc Fruc
nL = nC = lds->iquc[0];
for(i=1;i<=nL;i++)
{
for(j=1;j<=nC;j++)
{
lds->Mruc[i][j] = lds->M[lds->iquc[i]][lds->iquc[j]] +
lds->BtMvu[i][j] + lds->BtMvu[j][i] +
lds->BtMB[i][j];
}
lds->Fruc[i] = lds->F[lds->iquc[i]] + lds->MBMb[i] + lds->BtFv[i];
}
}
else
{
// Mruc Fruc
nL = nC = lds->iquc[0];
for(i=1;i<=nL;i++)
{
for(j=1;j<=nC;j++)
{
lds->Mruc[i][j] = lds->M[lds->iquc[i]][lds->iquc[j]];
}
lds->Fruc[i] = lds->F[lds->iquc[i]];
}
}
// Mr Fr
nL = nC = s->nqu;
for(i=1;i<=nL;i++)
{
has_a_line_of_zeros = 1;
for(j=1;j<=nC;j++)
{
lds->Mr[i][j] = lds->Mruc[i][j]; // Muu
if (lds->Mr[i][j]>1e-16)
has_a_line_of_zeros = 0;
}
if (has_a_line_of_zeros)
{
printf("The line %d of the reduced mass matrix, associated to q(%d), is full of zeros\n",i,lds->iquc[i]);
for(k=1;k<=nC;k++)
printf("lds->Mr[%d][%d] = %e;\n",i,k,lds->Mr[i][k]);
fprintf(stderr,"The reduced mass matrix has a line of zeros\n");
}
term = 0.0;
for(k=1;k<=s->nqc;k++)
{
term += lds->Mruc[i][s->nqu+k] * s->qdd[lds->iquc[s->nqu+k]];
}
lds->Fr[i] = -(lds->Fruc[i] + term);
}
// calcul des accelerations reduites : 'resolution' du systeme ODE = Mr*qddu = Fr;
choldc(lds->Mr,s->nqu,lds->p_Mr);
cholsl(lds->Mr,s->nqu,lds->p_Mr,lds->Fr,s->qddu);
nL = s->nqu;
for(i=1;i<=nL;i++)
{
s->qdd[s->qu[i]] = s->qddu[i];
}
if (s->nqv>0)
{
// qdd_v = Bvuc*qdd_u + bp
nL = s->nqv;
nk = s->nqu;
for(i=1;i<=nL;i++)
{
term = 0.0;
for(k=1;k<=nk;k++)
{
term += lds->Bvuc[i][k] * s->qddu[k];
}
s->qdd[s->qv[i]] = term + lds->bp[i];
}
}
// Qc = Mruc_cu * qddu + Mruc_cc * qddc + Fruc_c
if (s->nqc>0)
{
nL = s->nqc;
for(i=1;i<=nL;i++)
{
term = 0.0;
nk = s->nqu;
for(k=1;k<=nk;k++)
{
term += lds->Mruc[s->nqu+i][k] * s->qddu[k];
}
nk = s->nqc;
for(k=1;k<=nk;k++)
{
term += lds->Mruc[s->nqu+i][s->nqu+k] * s->qdd[lds->iquc[s->nqu+k]];
}
lds->Qc[i] = term + lds->Fruc[s->nqu+i];
}
}
return iter;
}
static void calc_bubble_projection(Element* e, Nurbs** nurbs, int order, double2* proj)
{
ref_map_pss.set_active_element(e);
int i, j, k;
int mo2 = quad2d.get_max_order();
int np = quad2d.get_num_points(mo2);
int qo = e->is_quad() ? make_quad_order(order, order) : order;
int nb = ref_map_shapeset.get_num_bubbles(qo);
AUTOLA_OR(double2, fn, np);
memset(fn, 0, sizeof(double2) * np);
double* rhside[2];
double* old[2];
for (i = 0; i < 2; i++) {
rhside[i] = new double[nb];
old[i] = new double[np];
memset(rhside[i], 0, sizeof(double) * nb);
memset(old[i], 0, sizeof(double) * np);
}
// compute known part of projection (vertex and edge part)
old_projection(e, order, proj, old);
// fn values of both components of nonpolynomial function
double3* pt = quad2d.get_points(mo2);
for (j = 0; j < np; j++) // over all integration points
{
double2 a;
a[0] = ctm.m[0] * pt[j][0] + ctm.t[0];
a[1] = ctm.m[1] * pt[j][1] + ctm.t[1];
calc_ref_map(e, nurbs, a[0], a[1], fn[j]);
}
double2* result = proj + e->nvert + e->nvert * (order - 1);
for (k = 0; k < 2; k++)
{
for (i = 0; i < nb; i++) // loop over bubble basis functions
{
// bubble basis functions in all integration points
double *bfn;
int index_i = ref_map_shapeset.get_bubble_indices(qo)[i];
ref_map_pss.set_active_shape(index_i);
ref_map_pss.set_quad_order(mo2);
bfn = ref_map_pss.get_fn_values();
for (j = 0; j < np; j++) // over all integration points
rhside[k][i] += pt[j][2] * (bfn[j] * (fn[j][k] - old[k][j]));
}
// solve
if (e->nvert == 3)
cholsl(bubble_proj_matrix_tri, nb, bubble_tri_p, rhside[k], rhside[k]);
else
cholsl(bubble_proj_matrix_quad, nb, bubble_quad_p, rhside[k], rhside[k]);
for (i = 0; i < nb; i++)
result[i][k] = rhside[k][i];
}
for (i = 0; i < 2; i++) {
delete [] rhside[i];
delete [] old[i];
}
}
