static void precalculate_cholesky_projection_matrices_bubble()
{
// *** triangles ***
ref_map_pss.set_mode(MODE_TRIANGLE);
int order = ref_map_shapeset.get_max_order();
// calculate projection matrix of maximum order
int nb = ref_map_shapeset.get_num_bubbles(order);
int* indices = ref_map_shapeset.get_bubble_indices(order);
bubble_proj_matrix_tri = calculate_bubble_projection_matrix(nb, indices);
// cholesky factorization of the matrix
bubble_tri_p = new double[nb];
choldc(bubble_proj_matrix_tri, nb, bubble_tri_p);
// *** quads ***
ref_map_pss.set_mode(MODE_QUAD);
order = ref_map_shapeset.get_max_order();
order = make_quad_order(order, order);
// calculate projection matrix of maximum order
nb = ref_map_shapeset.get_num_bubbles(order);
indices = ref_map_shapeset.get_bubble_indices(order);
bubble_proj_matrix_quad = calculate_bubble_projection_matrix(nb, indices);
// cholesky factorization of the matrix
bubble_quad_p = new double[nb];
choldc(bubble_proj_matrix_quad, nb, bubble_quad_p);
}
void solve_reduced(int n, int m, double h_x[], double h_y[],
double a[], double x_x[], double x_y[],
double c_x[], double c_y[],
double workspace[], int step)
{
int i,j,k;
double *p_x;
double *p_y;
double *t_a;
double *t_c;
double *t_y;
p_x = workspace; /* together n + m + n*m + n + m = n*(m+2)+2*m */
p_y = p_x + n;
t_a = p_y + m;
t_c = t_a + n*m;
t_y = t_c + n;
if (step == PREDICTOR) {
choldc(h_x, n, p_x); /* do cholesky decomposition */
for (i=0; i<m; i++) /* forward pass for A' */
chol_forward(h_x, n, p_x, a+i*n, t_a+i*n);
for (i=0; i<m; i++) /* compute (h_y + a h_x^-1A') */
for (j=i; j<m; j++)
for (k=0; k<n; k++)
h_y[m*i + j] += t_a[n*j + k] * t_a[n*i + k];
choldc(h_y, m, p_y); /* and cholesky decomposition */
}
chol_forward(h_x, n, p_x, c_x, t_c);
/* forward pass for c */
for (i=0; i<m; i++) { /* and solve for x_y */
t_y[i] = c_y[i];
for (j=0; j<n; j++)
t_y[i] += t_a[i*n + j] * t_c[j];
}
cholsb(h_y, m, p_y, t_y, x_y);
for (i=0; i<n; i++) { /* finally solve for x_x */
t_c[i] = -t_c[i];
for (j=0; j<m; j++)
t_c[i] += t_a[j*n + i] * x_y[j];
}
chol_backward(h_x, n, p_x, t_c, x_x);
}
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) ;
}
void cholesky(double *cf, double *a, int n)
{
int i, j, k ;
double *tt ;
double *p ;
ZALLOC(tt, n*n, double) ;
ZALLOC(p, n, double) ;
copyarr(a,tt,n*n);
choldc(tt, n, p ) ;
vzero(cf, n*n) ;
for (i = 0; i < n; i++) {
tt[i*n+i] = p[i] ;
for (j=0; j <= i ; j++) {
k = (i)*n+(j) ;
cf[k] = tt[i*n+j] ;
}
}
free(tt) ;
free(p) ;
}
// preparation of projection matrices, Cholesky factorization
static void precalculate_cholesky_projection_matrix_edge()
{
int order = ref_map_shapeset.get_max_order();
int n = order - 1; // number of edge basis functions
edge_proj_matrix = new_matrix<double>(n, n);
// calculate projection matrix of maximum order
for (int i = 0; i < n; i++)
{
for (int j = i; j < n; j++)
{
int o = i + j + 4;
double2* pt = quad1d.get_points(o);
double val = 0.0;
for (int k = 0; k < quad1d.get_num_points(o); k++)
{
double x = pt[k][0];
double fi = lob[i+2](x);
double fj = lob[j+2](x);
val += pt[k][1] * (fi * fj);
}
edge_proj_matrix[i][j] = edge_proj_matrix[j][i] = val;
}
}
// Cholesky factorization of the matrix
edge_p = new double[n];
choldc(edge_proj_matrix, n, edge_p);
}
// ****************************************************************************
// Calculates the Cholesky decomposition of N x N matrix A, such that A = L x transpose(L).
// This is only possible if matrix A is symmetric and positive definite (all eigenvalues
// positive).
// It is assumed that the memory required for this has already been allocated.
// The data in matrix A is not destroyed, unless the same address is supplied for L.
// (This case should be successfully handled also.)
// The function returns -1 if the matrix is not symmetric or not positive definite.
// ****************************************************************************
int matrix_cholesky(double *A, double *L, long int N)
{
long int row, col;
double *p, *backup;
int result;
if (A==NULL || L==NULL || N<1) quit_error((char*)"Bad input data to matrix_cholesky");
// Check that the A matrix is at least symmetric. (It also needs to be positive definite
// but that cannot be determined yet.)
for (row=0; row<N; row++)
{
for (col=row; col<N; col++)
{
if (fabs(A[row*N+col]-A[col*N+row])>DBL_EPSILON) return -1;
}
}
// Backup the A matrix so that manipulations can be performed here
// without damaging the original matrix.
backup=(double *)malloc(sizeof(double)*N*N);
if (backup==NULL) quit_error((char*)"Out of memory");
for (row=0; row<N; row++)
{
for (col=0; col<N; col++)
{
backup[row*N+col]=A[row*N+col];
}
}
// Create the vector for the results along the main diagonal
p=(double *)malloc(sizeof(double)*N);
if (p==NULL) quit_error((char*)"Out of memory");
// Calculate the Cholesky decomposition
result=choldc(backup, N, p);
// Assemble the answer in a nicer format
for (row=0; row<N; row++)
{
for (col=0; col<N; col++)
{
if (row<col) // Upper
{
L[row*N+col]=0.0;
}
else if (row>col) // Lower
{
L[row*N+col]=backup[row*N+col];
}
else L[row*N+col]=p[row]; // Diagonal
}
}
free(p);
free(backup);
return result;
}
void xchol(double **aorig, double **chol, int n, double *p, double **a)
{
int i,j;
//double **a, *p;
// p = dvector(n);
// a = dmatrix(n,n);
/* printf("xchol: n = %d\n",n); */
/* printf("xchol: starting reassignments\n"); */
for(i=0;i<n;i++) {
for(j=0;j<n;j++) {
/* printf("%12.7lf",aorig[i][j]); */
a[i][j] = aorig[i][j];
chol[i][j] = 0.0;
}
/* fprintf(stdout,"\n"); */
}
/* printf("xchol: calling cholesky routine\n"); */
choldc(a,n,p);
for(i=0;i<n;i++){
for(j=0;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);
}
}
// free(p);
// free_dmatrix(a,n);
}
double pdinv(double *cinv, double *coeff, int n)
// cinv and coeff can be same
// cinv can be NULL
// return log det (coeff)
{
double *tt;
double *p ;
double t, sum, y ;
int i,j, k ;
/**
pmat(coeff, n) ;
*/
ZALLOC (tt, n*n, double);
ZALLOC (p, n, double );
copyarr(coeff,tt,n*n);
choldc (tt, n, p) ;
for (i=0; i<n; i++) {
tt[i*n+i] = 1.0/p[i] ;
for (j=i+1; j<n; j++) {
sum=0.0 ;
for (k=i; k<j; k++) {
sum -= tt[j*n+k]*tt[k*n+i] ;
}
tt[j*n+i] = sum/p[j] ;
}
}
for (i=0; i<n; i++)
for (j=i; j<n; j++) {
sum=0.0 ;
if (cinv == NULL) break ;
for (k=j; k<n; k++) {
sum += tt[k*n+j]*tt[k*n+i] ;
}
cinv[i*n+j] = cinv[j*n+i] = sum ;
}
vlog(p, p, n) ;
y = 2.0*asum(p, n) ;
free(tt) ;
free(p) ;
return y ;
}
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;
}
void pdinv(double *cinv, double *coeff, int n)
{
double *tt;
double *p ;
double t, sum ;
int i,j, k ;
/**
pmat(coeff, n) ;
*/
ZALLOC (tt, n*n, double);
ZALLOC (p, n, double );
copyarr(coeff,tt,n*n);
choldc (tt, n, p) ;
for (i=0; i<n; i++) {
tt[i*n+i] = 1.0/p[i] ;
for (j=i+1; j<n; j++) {
sum=0.0 ;
for (k=i; k<j; k++) {
sum -= tt[j*n+k]*tt[k*n+i] ;
}
tt[j*n+i] = sum/p[j] ;
}
}
for (i=0; i<n; i++)
for (j=i; j<n; j++) {
sum=0.0 ;
for (k=j; k<n; k++) {
sum += tt[k*n+j]*tt[k*n+i] ;
}
cinv[i*n+j] = cinv[j*n+i] = sum ;
}
free(tt) ;
free(p) ;
}
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 ;
}
void linsys_solve(double a[], int n, int k, double b[], double x[], int flag, double f[])
{
unsigned int i,j ;
double *p=calloc(n, sizeof(double)) ;
choldc(a, n, p, f) ;
if (*f>0) /* check result */
{
free(p) ; /* matrix is not positiv definite */
return ;
} ;
for (i=0; i<k; i++)
cholsb(a, n, p, &b[i*n], &x[i*n]) ;
if (flag)
for (i=0; i<n; i++)
for (j=i+1; j<n; j++)
a[j*n+i]=a[j+i*n] ;
free(p) ;
}
void sym_chol_inv(int n,double **a,double **li)
//
// put inverse of the choleski root of a into li
//
{
int i,j,k;
// copy a into li
for(i=1;i<=n;i++) {
li[i][i] = a[i][i];
for(j=(i+1);j<=n;j++) li[i][j] = a[i][j];
}
double *p = new double [n+1];
// get lower triangular chol root of ai=a
choldc(li,n,p);
double sum;
//get inverse of li = chol(a)
for(i=1;i<=n;i++) {
li[i][i] = 1.0/p[i];
for(j=(i+1);j<=n;j++) {
sum = 0.0;
for(k=i;k<j;k++) sum -= li[j][k]*li[k][i];
li[j][i] = sum/p[j];
li[i][j] = 0.0;
}
}
delete [] p;
}
void Apply(float *x_in, float *y_in, int num_points,
double& xc, double& yc,
double& xa, double& ya,
double& la, double& lb)
{
int np = num_points;
static double invL[7][7];
static double D[MAXP+1][7];
static double S[7][7];
static double Const[7][7];
static double temp[7][7];
static double L[7][7];
static double C[7][7];
static double d [7];
static double V[7][7];
static double sol[7][7];
static double tx,ty;
int nrot=0;
int npts=50;
static double XY[3][MAXP+1];
assert(num_points<MAXP);
int mode = FPF;
for (int i=0; i<7; i++)
{
d[i] = 0;
for (int j=0; j<MAXP+1; j++)
{
D[j][i] = 0;
}
for (int j=0; j<7; j++)
{
S[i][j] = 0;
Const[i][j] = 0;
temp[i][j] = 0;
L[i][j] = 0;
C[i][j] = 0;
invL[i][j] = 0;
V[i][j] = 0;
sol[i][j] = 0;
}
}
switch (mode) {
case(FPF):
//System.out.println("FPF mode");
Const[1][3]=-2;
Const[2][2]=1;
Const[3][1]=-2;
break;
case(TAUBIN):
// g.drawString(warn_taub_str,size().width/18 , size().height/18 );
break;
case(BOOKSTEIN):
//System.out.println("BOOK mode");
Const[1][1]=2;
Const[2][2]=1;
Const[3][3]=2;
}
if (np<6)
return;
// Now first fill design matrix
for (int i=1; i <= np; i++)
{
tx = x_in[i-1];
ty = y_in[i-1];
// printf("%d %d, %g, %g\n", i, np, tx, ty);
D[i][1] = tx*tx;
D[i][2] = tx*ty;
D[i][3] = ty*ty;
D[i][4] = tx;
D[i][5] = ty;
D[i][6] = 1.0;
}
// printf("Done\n");
//pm(Const,"Constraint");
// Now compute scatter matrix S
A_TperB(D,D,S,np,6,np,6);
//pm(S,"Scatter");
choldc(S,6,L);
//pm(L,"Cholesky");
inverse7(L,invL,6);
//pm(invL,"inverse");
AperB_T(Const,invL,temp,6,6,6,6);
AperB(invL,temp,C,6,6,6,6);
//pm(C,"The C matrix");
jacobi(C,6,d,V,nrot);
//pm(V,"The Eigenvectors");
//pv(d,"The eigevalues");
A_TperB(invL,V,sol,6,6,6,6);
//pm(sol,"The GEV solution unnormalized");
// Now normalize them
for (int j=1;j<=6;j++)
{
double mod = 0.0;
for (int i=1;i<=6;i++)
mod += sol[i][j]*sol[i][j];
for (int i=1;i<=6;i++)
sol[i][j] /= sqrt(mod);
}
//pm(sol,"The GEV solution");
double zero=10e-20;
double minev=10e+20;
int solind=0;
switch (mode) {
case(BOOKSTEIN): // smallest eigenvalue
for (int i=1; i<=6; i++)
if (d[i]<minev && fabs(d[i])>zero)
solind = i;
break;
case(FPF):
for (int i=1; i<=6; i++)
if (d[i]<0 && fabs(d[i])>zero)
solind = i;
}
// Now fetch the right solution
for (int j=1;j<=6;j++)
{
pvec[j] = sol[j][solind];
//params[j-1] = sol[j][solind];
}
//pv(pvec,"the solution");
// double xc, yc, xa, ya, a, b;
get_param(pvec,xc,yc,xa,ya,la,lb);
// ...and plot it
// draw_conic(pvec,4);
/*
for (int i=1; i<npts; i++) {
if (XY[1][i]==-1 || XY[1][i+1]==-1 )
continue;
else
if (i<npts)
g.drawLine((int)XY[1][i],(int)XY[2][i],(int)XY[1][i+1],(int)XY[2][i+1] );
else
g.drawLine((int)XY[1][i],(int)XY[2][i],(int)XY[1][1],(int)XY[2][1] );
}
*/
}
int main(void)
{
// Number of rows and columns of the input and output matrix
// Change it as you desire
const int n = 10;
int sizeOfMatrixInt = n * n * sizeof(uint32_t);
int sizeOfMatrixFloat = n * n * sizeof(float);
// Input matrix a, C output matrix l_test and Max output matrix l
uint32_t *a = malloc(sizeOfMatrixInt);
float *l = malloc(sizeOfMatrixFloat);
float *l_test = malloc(sizeOfMatrixFloat);
int i, j;
// Variables needed to initialize matrix a
int duplicate = n;
int currentNumber = 1;
// The following loop initializes the matrixes
// It fills l and l_test with zeros
// It fills a so it can be a symmetric positive-definite matrix
// For example, if n = 5,
// matrix a would look like:
// 1 1 1 1 1
// 1 2 2 2 2
// 1 2 3 3 3
// 1 2 3 4 4
// 1 2 3 4 5
for (i = 0; i < n; ++i) {
for (j = 0; j < (n - duplicate); ++j) {
a[i * n + j] = currentNumber;
l[i * n + j] = 0;
l_test[i * n + j] = 0;
// Instead of only incrementing the currentNumber by 1,
// you can increment it how much you want, as long
// as you keep increasing and not decrementing the values
++currentNumber;
// Only incrementing the values by 1 will produce an output matrix
// whose elements are only 1 and 0 which is easier
// for the purpose of this test
}
for (; j < n; ++j) {
a[i * n + j] = currentNumber;
l[i * n + j] = 0;
l_test[i * n + j] = 0;
}
--duplicate;
currentNumber = 1;
}
printf("Running Cholesky.\n");
// Runs the Cholesky decomposition
clock_t choleskyTime = clock();
choldc(n, a, l_test);
choleskyTime = clock() - choleskyTime;
printf("Running Cholesky on DFE.\n");
// Runs the Cholesky decomposition on DFE
clock_t choleskyMaxTime = clock();
CholeskyStream(n, a, l_test, l);
choleskyMaxTime = clock() - choleskyMaxTime;
// Compares the results
cmpResults(n, l, l_test);
// Prints the execution times
printf("Cholesky execution time: %f\n", ((float) choleskyTime) / CLOCKS_PER_SEC);
printf("Cholesky execution time on DFE: %f\n", ((float) choleskyMaxTime) / CLOCKS_PER_SEC);
free(a);
free(l);
free(l_test);
printf("Done.\n");
return 0;
}
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;
}
bool EllipseFitting(POINT* rgPoints, int nPoints, double &a, double &b, double &c, double &d, double &e, double &f)
{
int i, j;
int np = nPoints;
int nrot=0;
Matrix<double> D(np+1,7); //Design matrix
Matrix<double> S(7,7); //Scatter matrix
Matrix<double> Const(7,7); //Constraint matrix
Matrix<double> temp(7,7);
Matrix<double> L(7,7); //The lower triangular matrix L * L' = S
Matrix<double> C(7,7);
Matrix<double> invL(7,7); //Inverse matrix of L
Matrix<double> V(7,7); //The eigen vectors
Matrix<double> sol(7,7); //The GVE solution
double ev[7]; //The eigen value
double pvec[7];
if (np < 6)
{
return false;
}
//Build design matrix D
for (i = 0; i < np; i++)
{
D[i][1] = rgPoints[i].x * rgPoints[i].x;
D[i][2] = rgPoints[i].x * rgPoints[i].y;
D[i][3] = rgPoints[i].y * rgPoints[i].y;
D[i][4] = rgPoints[i].x;
D[i][5] = rgPoints[i].y;
D[i][6] = 1.0;
}
//Build Scatter matrix S
A_TperB(D, D, S, np, 6, np, 6);
//Build 6*6 constraint matrix
Const[1][3] = -2;
Const[2][2] = 1;
Const[3][1] = -2;
//Sovle generalized eigen system
choldc(S, 6, L);
if (!inverse(L, invL, 6))
{
return false;
}
AperB_T(Const, invL, temp, 6, 6, 6, 6);
AperB(invL, temp, C, 6, 6, 6, 6);
jacobi(C, 6, ev, V, nrot);
A_TperB(invL, V, sol, 6, 6, 6, 6);
//Normalize the solution
for (j = 1; j <= 6; j++)
{
double mod = 0.0;
for (i = 1; i <= 6; i++)
{
mod += sol[i][j] * sol[i][j];
}
mod = sqrt(mod);
for (i = 1 ; i <= 6; i++)
{
sol[i][j] /= mod;
}
}
double zero = 10e-20;
int solind = 0;
//Find the only negative eigen value
for (i = 1; i <= 6; i++)
{
if ((ev[i] < 0) && (fabs(ev[i]) > zero))
{
solind = i;
}
}
//Get the fitted parameters
for (j = 1; j <= 6; j++)
{
pvec[j] = sol[j][solind];
}
a = pvec[1];
b = pvec[2];
c = pvec[3];
d = pvec[4];
e = pvec[5];
f = pvec[6];
return true;
}
int CUKF::UnscentedUpdate(int idf, int idx)
{
// Set up some variables
int dim = XX->rows;
int N = 2*dim+1;
double scale = 3;
int kappa = scale-dim;
// Create samples
// SVD
CvSize P = cvGetSize(PX);
CvMat *D = cvCreateMat(P.height, P.width, CV_64FC1);
CvMat *U = cvCreateMat(P.height, P.height, CV_64FC1);
CvMat *V = cvCreateMat(P.width, P.width, CV_64FC1);
CvMat *Ds = cvCreateMat(P.height, P.width, CV_64FC1);
CvMat *UD = cvCreateMat(P.height, P.width, CV_64FC1);
CvMat *Ps = cvCreateMat(P.height, P.width, CV_64FC1);
cvSVD(PX, D, U, V, CV_SVD_V_T);
MatSqrt(D, Ds);
cvMatMul(U, Ds, UD);
cvScale(UD, Ps, sqrt(scale), 0);
// CvMat *ss = cvCreateMat(dim, N, CV_64FC1);
// int i,j,jj;
// int row = dim;
// int col = N;
// cvRepeat(XX, ss);
// // for (i=0; i<row; i++) for (j=0; j<col; j++)
// // ss->data.db[i*col+j] = XXA->data.db[i];
//
// col -= dim;
// for(i=0; i<row; i++) for(j=1, jj=0; j<col+1; j++, jj++)
// ss->data.db[i*col+j] += Ps->data.db[i*col+jj];
// for(i=0; i<row; i++) for(j=1+dim, jj=0; j<col+1+dim; j++, jj++)
// ss->data.db[i*col+j] -= Ps->data.db[i*col+jj];
CvMat *ss = cvCreateMat(dim, N, CV_64FC1);
int i,j,jj;
int row = dim;
int col = N;
cvRepeat(XX, ss);
double *ssdb = ss->data.db;
double *Psdb = Ps->data.db;
for(i=0; i<row; i++)
{
for(j=1, jj=0; j<dim+1; j++, jj++)
{
ssdb[i*ss->cols+j] += Psdb[i*Ps->cols+jj];
}
}
for(i=0; i<row; i++)
{
for(j=1+dim, jj=0; j<ss->cols; j++, jj++)
{
ssdb[i*ss->cols+j] -= Psdb[i*Ps->cols+jj];
}
}
// Transform samples according to observation model to obtain the predicted observation samples
CvMat *zs = cvCreateMat(2, N, CV_64FC1);
// CvMat *base = cvCreateMat(3, EP, CV_64FC1);
int Nxv = 3;
int f = Nxv + idf*2 - 2;
// PrintCvMat(ss, "ss");
double *zsdb = zs->data.db;
for(j=0; j<N; j++)
{
double dx = ssdb[f*ss->cols+j] - ssdb[0*ss->cols+j];
double dy = ssdb[(f+1)*ss->cols+j] - ssdb[1*ss->cols+j];
double d2 = dx*dx + dy*dy;
double d = sqrt(d2);
zsdb[0*zs->cols+j] = d;
zsdb[1*zs->cols+j] = atan2(dy,dx) - ssdb[2*ss->cols+j];
}
// zz = repvec(z,N);
// dz = feval(dzfunc, zz, zs); % compute correct residual
// zs = zz - dz; % offset zs from z according to correct residual
// PrintCvMat(zs, "zs");
CvMat *zz = cvCreateMat(2, N, CV_64FC1);
CvMat *dz = cvCreateMat(2, N, CV_64FC1);
CvMat *zprev = cvCreateMat(2, 1, CV_64FC1);
zprev->data.db[0] = zf[idx].x;
zprev->data.db[1] = zf[idx].y;
cvRepeat(zprev, zz);
cvSub(zz, zs, dz);
double *dzdb = dz->data.db;
for(j=0; j<dz->cols; j++) dzdb[1*dz->cols+j] = PI2PI(dzdb[1*dz->cols+j]);
cvSub(zz, dz, zs);
// PrintCvMat(zs, "zs");
CvMat *zm = cvCreateMat(2, 1, CV_64FC1);
CvMat *dx = cvCreateMat(dim ,N, CV_64FC1);
// CvMat *dz = cvCreateMat(2, N, CV_64FC1);
CvMat *repx = cvCreateMat(dim ,N, CV_64FC1);
CvMat *repzm = cvCreateMat(2, N, CV_64FC1);
CvMat *Pxz = cvCreateMat(dim, 2, CV_64FC1);
CvMat *Pzz = cvCreateMat(2, 2, CV_64FC1);
CvMat *dxu = cvCreateMat(dim, 1, CV_64FC1);
CvMat *dxl = cvCreateMat(dim, N-1, CV_64FC1);
CvMat *dzu = cvCreateMat(2, 1, CV_64FC1);
CvMat *dzl = cvCreateMat(2, N-1, CV_64FC1);
CvMat *dztu = cvCreateMat(1, 2, CV_64FC1);
CvMat *dztl = cvCreateMat(N-1, 2, CV_64FC1);
CvMat *dxdzu = cvCreateMat(dim, 2, CV_64FC1);
CvMat *dxdzl = cvCreateMat(dim, 2, CV_64FC1);
CvMat *dzdzu = cvCreateMat(2, 2, CV_64FC1);
CvMat *dzdzl = cvCreateMat(2, 2, CV_64FC1);
double *zmdb = zm->data.db;
zmdb[0] = kappa*zs->data.db[0*zs->cols];
zmdb[1] = kappa*zs->data.db[1*zs->cols];
for(j=1; j<N; j++)
{
zm->data.db[0] += 0.5*zsdb[0*zs->cols+j];
zm->data.db[1] += 0.5*zsdb[1*zs->cols+j];
}
cvScale(zm, zm, 1/(double)(scale));
// Calculate predicted observation mean
cvRepeat(XX, repx);
cvRepeat(zm, repzm);
// PrintCvMat(ss, "ss");
// PrintCvMat(zs, "zs");
// PrintCvMat(repx, "repx");
// PrintCvMat(repzm, "repzm");
// Calculate observation covariance and the state-observation correlation matrix
cvSub(ss, repx, dx);
cvSub(zs, repzm, dz);
// PrintCvMat(dx, "dx");
// PrintCvMat(dz, "dz");
double *dxdb = dx->data.db;
double *dzudb = dzu->data.db;
double *dzldb = dzl->data.db;
double *dxudb = dxu->data.db;
double *dxldb = dxl->data.db;
// dx, dz를 1열과 나머지 열로 분할
for(i=0; i<2; i++) dzudb[i] = dzdb[i*dz->cols];
for(i=0; i<2; i++) for(j=1, jj=0; j<N; j++, jj++)
dzldb[i*dzl->cols+jj] = dzdb[i*dz->cols+j];
for(i=0; i<dim; i++) dxudb[i] = dxdb[i*dx->cols];
for(i=0; i<dim; i++) for(j=1, jj=0; j<N; j++, jj++)
dxldb[i*dxl->cols+jj] = dxdb[i*dx->cols+j];
cvT(dzu, dztu);
cvT(dzl, dztl);
// PrintCvMat(dxu, "dxu");
// PrintCvMat(dztu, "dztu");
// PrintCvMat(dxl, "dxl");
// PrintCvMat(dztl, "dztl");
cvMatMul(dxu, dztu, dxdzu);
cvScale(dxdzu, dxdzu, 2*kappa);
cvMatMul(dxl, dztl, dxdzl);
cvAdd(dxdzu, dxdzl, Pxz);
cvScale(Pxz, Pxz, 1/(double)(2*scale));
cvMatMul(dzu, dztu, dzdzu);
cvScale(dzdzu, dzdzu, 2*kappa);
cvMatMul(dzl, dztl, dzdzl);
cvAdd(dzdzu, dzdzl, Pzz);
cvScale(Pzz, Pzz, 1/(double)(2*scale));
// PrintCvMat(dx, "dx");
// PrintCvMat(dz, "dz");
// PrintCvMat(dzu, "dzu");
// PrintCvMat(dztu, "dztu");
// PrintCvMat(dzl, "dzl");
// PrintCvMat(dztl, "dztl");
// PrintCvMat(dxu, "dxu");
// PrintCvMat(dxl, "dxl");
// PrintCvMat(dxdzu, "dxdzu");
// PrintCvMat(dxdzl, "dxdzl");
// PrintCvMat(dzdzu, "dzdzu");
// PrintCvMat(dzdzl, "dzdzl");
// PrintCvMat(Pxz, "Pxz");
// PrintCvMat(Pzz, "Pzz");
// Compute Kalman gain
CvMat *S = cvCreateMat(2, 2, CV_64FC1);
CvMat *p = cvCreateMat(2, 2, CV_64FC1);
CvMat *Sct = cvCreateMat(2, 2, CV_64FC1);
CvMat *Sc = cvCreateMat(2, 2, CV_64FC1);
CvMat *Sci = cvCreateMat(2, 2, CV_64FC1);
CvMat *Scit = cvCreateMat(2, 2, CV_64FC1);
CvMat *Wc = cvCreateMat(dim, 2, CV_64FC1);
CvMat *W = cvCreateMat(dim, 2, CV_64FC1);
CvMat *Wz = cvCreateMat(dim, 1, CV_64FC1);
CvMat *Wct = cvCreateMat(2, dim, CV_64FC1);
CvMat *WcWc = cvCreateMat(dim, dim, CV_64FC1);
// % Compute Kalman gain
cvAdd(Pzz, R, S);
//cholesky decomposition이 실패하면 업데이트는 하지 않는다.
if(choldc(S, p, Sct) < 0)
{
TRACE("idf : %d\n", idf);
PrintCvMat(UD, "UD");
PrintCvMat(U, "U");
PrintCvMat(D, "D");
PrintCvMat(V, "V");
PrintCvMat(zprev, "zprev");
PrintCvMat(XX, "XX");
PrintCvMat(PX, "PX");
PrintCvMat(Ps, "Ps");
PrintCvMat(ss, "ss");
for(j=0; j<N; j++)
{
double dx = ss->data.db[f*ss->cols+j] - ss->data.db[0*ss->cols+j];
double dy = ss->data.db[(f+1)*ss->cols+j] - ss->data.db[1*ss->cols+j];
double d2 = dx*dx + dy*dy;
double d = sqrt(d2);
double angle = atan2(dy,dx) - ss->data.db[2*ss->cols+j];
TRACE("%.4f %.4f %.4f %.4f %.4f %.4f %.4f\n", dx, dy, d2, d, angle, atan2(dy, dx), ss->data.db[2*ss->cols+j]);
}
PrintCvMat(zs, "zs");
PrintCvMat(zz, "zz");
PrintCvMat(zm, "zm");
PrintCvMat(dx, "dx");
PrintCvMat(dz, "dz");
PrintCvMat(Pxz, "Pxz");
PrintCvMat(Pzz, "Pzz");
PrintCvMat(R, "R");
return -1;
}
cvT(Sct, Sc);
cvInv(Sc, Sci);
cvT(Sci, Scit);
// PrintCvMat(S, "S");
// PrintCvMat(Sct, "Sct");
// PrintCvMat(Sc, "Sc");
// PrintCvMat(Sci, "Sci");
cvMatMul(Pxz, Sci, Wc);
// PrintCvMat(Wc, "Wc");
cvMatMul(Wc, Scit, W);
// PrintCvMat(W, "W");
cvSub(zprev, zm, zprev);
cvMatMul(W, zprev, Wz);
cvAdd(XX, Wz, XX);
cvT(Wc, Wct);
cvMatMul(Wc, Wct, WcWc);
cvSub(PX, WcWc, PX);
// PrintCvMat(Pzz, "Pzz");
// PrintCvMat(R, "R");
// PrintCvMat(S, "S");
// PrintCvMat(Pxz, "Pxz");
// PrintCvMat(Sc, "Sc");
// PrintCvMat(Sct, "Sct");
// PrintCvMat(Sci, "Sci");
// PrintCvMat(Scit, "Scit");
// PrintCvMat(W, "W");
// PrintCvMat(zprev, "zprev");
// PrintCvMat(zm, "zm");
// PrintCvMat(Wc, "Wc");
// PrintCvMat(Wct, "Wct");
// PrintCvMat(WcWc, "WcWc");
// PrintCvMat(PX, "Px");
//
// PrintCvMat(XX, "XX");
// PrintCvMat(PX, "PX");
cvReleaseMat(&D);
cvReleaseMat(&U);
cvReleaseMat(&V);
cvReleaseMat(&Ds);
cvReleaseMat(&UD);
cvReleaseMat(&Ps);
cvReleaseMat(&ss);
cvReleaseMat(&zs);
cvReleaseMat(&zz);
cvReleaseMat(&dz);
cvReleaseMat(&zprev);
cvReleaseMat(&zm);
cvReleaseMat(&dx);
cvReleaseMat(&repx);
cvReleaseMat(&repzm);
cvReleaseMat(&Pxz);
cvReleaseMat(&Pzz);
cvReleaseMat(&dxu);
cvReleaseMat(&dxl);
cvReleaseMat(&dzu);
cvReleaseMat(&dzl);
cvReleaseMat(&dztu);
cvReleaseMat(&dztl);
cvReleaseMat(&dxdzu);
cvReleaseMat(&dxdzl);
cvReleaseMat(&dzdzu);
cvReleaseMat(&dzdzl);
cvReleaseMat(&S);
cvReleaseMat(&p);
cvReleaseMat(&Sct);
cvReleaseMat(&Sc);
cvReleaseMat(&Sci);
cvReleaseMat(&Scit);
cvReleaseMat(&Wc);
cvReleaseMat(&W);
cvReleaseMat(&Wz);
cvReleaseMat(&Wct);
cvReleaseMat(&WcWc);
return 0;
}