void CSubdivisionDoc::OnSubdivisionCatmull()
{
// TODO: Add your command handler code here
MeshSubdivision m_sub(m_pmesh);
m_pmesh = m_sub.Catmull_Clark();
UpdateAllViews(NULL);
}
static void kalman_correct(kalman_t *kf, float p, float v)
{
/* K = P * HT * inv(H * P * HT + R) */
m_mlt(kf->H, kf->P, kf->T0);
mmtr_mlt(kf->T0, kf->H, kf->T1);
m_add(kf->T1, kf->R, kf->T0);
m_inverse(kf->T0, kf->T1);
mmtr_mlt(kf->P, kf->H, kf->T0);
m_mlt(kf->T0, kf->T1, kf->K);
/* x = x + K * (z - H * x) */
mv_mlt(kf->H, kf->x, kf->t0);
v_set_val(kf->z, 0, p);
v_set_val(kf->z, 1, v);
v_sub(kf->z, kf->t0, kf->t1);
mv_mlt(kf->K, kf->t1, kf->t0);
v_add(kf->x, kf->t0, kf->t1);
v_copy(kf->t1, kf->x);
/* P = (I - K * H) * P */
m_mlt(kf->K, kf->H, kf->T0);
m_sub(kf->I, kf->T0, kf->T1);
m_mlt(kf->T1, kf->P, kf->T0);
m_copy(kf->T0, kf->P);
}
void CSubdivisionDoc::OnOperationNew()
{
// TODO: Add your command handler code here
MeshSubdivision m_sub(m_pmesh);
m_pmesh = m_sub.Doo_Sabin();
UpdateAllViews(NULL);
}
static void test_gmres(ITER *ip, int i, MAT *Q, MAT *R,
VEC *givc, VEC *givs, double h_val)
#endif
{
VEC vt, vt1;
STATIC MAT *Q1=MNULL, *R1=MNULL;
int j;
/* test Q*A*Q^T = R */
Q = m_resize(Q,i+1,ip->b->dim);
Q1 = m_resize(Q1,i+1,ip->b->dim);
R1 = m_resize(R1,i+1,i+1);
MEM_STAT_REG(Q1,TYPE_MAT);
MEM_STAT_REG(R1,TYPE_MAT);
vt.dim = vt.max_dim = ip->b->dim;
vt1.dim = vt1.max_dim = ip->b->dim;
for (j=0; j <= i; j++) {
vt.ve = Q->me[j];
vt1.ve = Q1->me[j];
ip->Ax(ip->A_par,&vt,&vt1);
}
mmtr_mlt(Q,Q1,R1);
R1 = m_resize(R1,i+2,i+1);
for (j=0; j < i; j++)
R1->me[i+1][j] = 0.0;
R1->me[i+1][i] = h_val;
for (j = 0; j <= i; j++) {
rot_rows(R1,j,j+1,givc->ve[j],givs->ve[j],R1);
}
R1 = m_resize(R1,i+1,i+1);
m_sub(R,R1,R1);
/* if (m_norm_inf(R1) > MACHEPS*ip->b->dim) */
#ifndef MEX
printf(" %d. ||Q*A*Q^T - H|| = %g [cf. MACHEPS = %g]\n",
ip->steps,m_norm_inf(R1),MACHEPS);
#endif
/* check Q*Q^T = I */
Q = m_resize(Q,i+1,ip->b->dim);
mmtr_mlt(Q,Q,R1);
for (j=0; j <= i; j++)
R1->me[j][j] -= 1.0;
#ifndef MEX
if (m_norm_inf(R1) > MACHEPS*ip->b->dim)
printf(" ! m_norm_inf(Q*Q^T) = %g\n",m_norm_inf(R1));
#endif
#ifdef THREADSAFE
M_FREE(Q1);
M_FREE(R1);
#endif
}
int main()
{
char op;
int a ,b, c, d;
scanf("%d, %d, %c, %d, %d", &a, &b, &op, &c, &d);
switch (op) {
case '+': m_add(a, b, c, d); break;
case '*': m_mul(a, b, c, d); break;
case '-': m_sub(a, b, c, d); break;
case '/': m_div(a, b, c, d); break;
}
return 0;
}
void orthoit(double* a, int lda, int n, double* x, int k, double eps){
double *q_hat, *y, *lambda, *test;
double norm;
q_hat = (double*) malloc(n*k*sizeof(double));
y = (double*) malloc(n*k*sizeof(double));
lambda = (double*) malloc(k*k*sizeof(double));
test = (double*) malloc(n*k*sizeof(double)); /* Matrix der Abbruchbedingung */
/* QR=X und Q extrahieren */
qr_decomp(x, n, k, n);
get_q(x, n, n, k, q_hat, k);
/* Y=AQ rechnen */
m_mult(a, n, n, 0, q_hat, n, k, 0, y);
/* Lambda=Q*Y rechnen */
m_mult(q_hat, n, k, 1, y, n, k, 0, lambda);
while (1)
{
// Abbruchbedingung berechnen und checken
m_mult(q_hat, n, k, 0, lambda, k, k, 0, test);
m_sub(test, test, y, n, n, k);
norm = norm_frob(test, n, n, k);
if (norm < eps) break;
// QR=Y und Q extrahieren
qr_decomp(y, n, k, n);
get_q(y, n, n, k, q_hat, k);
m_mult(a, n, n, 0, q_hat, n, k, 0, y);
m_mult(q_hat, n, k, 1, y, n, k, 0, lambda);
}
memcpy(x, q_hat, n*k*sizeof(double));
free(q_hat);
free(y);
free(lambda);
free(test);
}
/*
* Kalman filter update
*
* P is [4,4] and holds the covariance matrix
* R is [3,3] and holds the measurement weights
* C is [4,3] and holds the euler to quat tranform
* X is [4,1] and holds the estimated state as a quaterion
* Xe is [3,1] and holds the euler angles of the estimated state
* Xm is [3,1] and holds the measured euler angles
*/
static inline void
kalman_update(
m_t P,
m_t R,
m_t C,
v_t X,
const v_t Xe,
const v_t Xm
)
{
m_t T1;
m_t T2;
m_t E;
m_t K;
v_t err;
v_t temp;
/* E[3,3] = C[3,4] P[4,4] C'[4,3] + R[3,3] */
m_mult( T1, C, P, 3, 4, 4 );
m_transpose( T2, C, 3, 4 );
m_mult( E, T1, T2, 3, 4, 3 );
m_add( E, R, E, 3, 3 );
/* K[4,3] = P[4,4] C'[4,3] inv(E)[3,3] */
m_mult( T1, P, T2, 4, 4, 3 );
m33_inv( E, T2 );
m_mult( K, T1, T2, 4, 3, 3 );
/* X[4] = X[4] + K[4,3] ( Xm[3] - Xe[3] ) */
v_sub( err, Xm, Xe, 3 );
mv_mult( temp, K, err, 4, 3 );
v_add( X, temp, X, 4 );
/* P[4,4] = P[4,4] - K[4,3] C[3,4] P[4,4] */
m_mult( T1, K, C, 4, 3, 4 );
m_mult( T2, T1, P, 4, 4, 4 );
m_sub( P, T2, P, 4, 4 );
}
void
mp_msub(MINT *a, MINT *b, MINT *c)
{
MINT x, y;
int sign;
x.len = y.len = 0;
_mp_move(a, &x);
_mp_move(b, &y);
_mp_xfree(c);
sign = 1;
if (x.len >= 0) {
if (y.len >= 0) {
if (x.len >= y.len) {
m_sub(&x, &y, c);
} else {
sign = -1;
mp_msub(&y, &x, c);
}
} else {
y.len = -y.len;
mp_madd(&x, &y, c);
}
} else {
if (y.len <= 0) {
x.len = -x.len;
y.len = -y.len;
mp_msub(&y, &x, c);
} else {
x.len = -x.len;
mp_madd(&x, &y, c);
sign = -1;
}
}
c->len = sign * c->len;
_mp_xfree(&x);
_mp_xfree(&y);
}
void check_cm_grp(FILE *fp, t_vcm *vcm, t_inputrec *ir, real Temp_Max)
{
int m, g;
real ekcm, ekrot, tm, tm_1, Temp_cm;
rvec jcm;
tensor Icm;
/* First analyse the total results */
if (vcm->mode != ecmNO)
{
for (g = 0; (g < vcm->nr); g++)
{
tm = vcm->group_mass[g];
if (tm != 0)
{
tm_1 = 1.0/tm;
svmul(tm_1, vcm->group_p[g], vcm->group_v[g]);
}
/* Else it's zero anyway! */
}
if (vcm->mode == ecmANGULAR)
{
for (g = 0; (g < vcm->nr); g++)
{
tm = vcm->group_mass[g];
if (tm != 0)
{
tm_1 = 1.0/tm;
/* Compute center of mass for this group */
for (m = 0; (m < DIM); m++)
{
vcm->group_x[g][m] *= tm_1;
}
/* Subtract the center of mass contribution to the
* angular momentum
*/
cprod(vcm->group_x[g], vcm->group_v[g], jcm);
for (m = 0; (m < DIM); m++)
{
vcm->group_j[g][m] -= tm*jcm[m];
}
/* Subtract the center of mass contribution from the inertia
* tensor (this is not as trivial as it seems, but due to
* some cancellation we can still do it, even in parallel).
*/
clear_mat(Icm);
update_tensor(vcm->group_x[g], tm, Icm);
m_sub(vcm->group_i[g], Icm, vcm->group_i[g]);
/* Compute angular velocity, using matrix operation
* Since J = I w
* we have
* w = I^-1 J
*/
get_minv(vcm->group_i[g], Icm);
mvmul(Icm, vcm->group_j[g], vcm->group_w[g]);
}
/* Else it's zero anyway! */
}
}
}
for (g = 0; (g < vcm->nr); g++)
{
ekcm = 0;
if (vcm->group_mass[g] != 0 && vcm->group_ndf[g] > 0)
{
for (m = 0; m < vcm->ndim; m++)
{
ekcm += sqr(vcm->group_v[g][m]);
}
ekcm *= 0.5*vcm->group_mass[g];
Temp_cm = 2*ekcm/vcm->group_ndf[g];
if ((Temp_cm > Temp_Max) && fp)
{
fprintf(fp, "Large VCM(group %s): %12.5f, %12.5f, %12.5f, Temp-cm: %12.5e\n",
vcm->group_name[g], vcm->group_v[g][XX],
vcm->group_v[g][YY], vcm->group_v[g][ZZ], Temp_cm);
}
if (vcm->mode == ecmANGULAR)
{
ekrot = 0.5*iprod(vcm->group_j[g], vcm->group_w[g]);
if ((ekrot > 1) && fp && !EI_RANDOM(ir->eI))
{
/* if we have an integrator that may not conserve momenta, skip */
tm = vcm->group_mass[g];
fprintf(fp, "Group %s with mass %12.5e, Ekrot %12.5e Det(I) = %12.5e\n",
vcm->group_name[g], tm, ekrot, det(vcm->group_i[g]));
fprintf(fp, " COM: %12.5f %12.5f %12.5f\n",
vcm->group_x[g][XX], vcm->group_x[g][YY], vcm->group_x[g][ZZ]);
fprintf(fp, " P: %12.5f %12.5f %12.5f\n",
vcm->group_p[g][XX], vcm->group_p[g][YY], vcm->group_p[g][ZZ]);
fprintf(fp, " V: %12.5f %12.5f %12.5f\n",
vcm->group_v[g][XX], vcm->group_v[g][YY], vcm->group_v[g][ZZ]);
fprintf(fp, " J: %12.5f %12.5f %12.5f\n",
vcm->group_j[g][XX], vcm->group_j[g][YY], vcm->group_j[g][ZZ]);
fprintf(fp, " w: %12.5f %12.5f %12.5f\n",
vcm->group_w[g][XX], vcm->group_w[g][YY], vcm->group_w[g][ZZ]);
pr_rvecs(fp, 0, "Inertia tensor", vcm->group_i[g], DIM);
}
}
}
}
}
void Ukf(VEC *omega, VEC *mag_vec, VEC *mag_vec_I, VEC *sun_vec, VEC *sun_vec_I, VEC *Torq_ext, double t, double h, int eclipse, VEC *state, VEC *st_error, VEC *residual, int *P_flag, double sim_time)
{
static VEC *omega_prev = VNULL, *mag_vec_prev = VNULL, *sun_vec_prev = VNULL, *q_s_c = VNULL, *x_prev = VNULL, *Torq_prev, *x_m_o;
static MAT *Q = {MNULL}, *R = {MNULL}, *Pprev = {MNULL};
static double alpha, kappa, lambda, sqrt_lambda, w_m_0, w_c_0, w_i, beta;
static int n_states, n_sig_pts, n_err_states, iter_num, initialize=0;
VEC *x = VNULL, *x_priori = VNULL, *x_err_priori = VNULL, *single_sig_pt = VNULL, *v_temp = VNULL, *q_err_quat = VNULL,
*err_vec = VNULL, *v_temp2 = VNULL, *x_ang_vel = VNULL, *meas = VNULL, *meas_priori = VNULL,
*v_temp3 = VNULL, *x_posteriori_err = VNULL, *x_b_m = VNULL, *x_b_g = VNULL;
MAT *sqrt_P = {MNULL}, *P = {MNULL}, *P_priori = {MNULL}, *sig_pt = {MNULL}, *sig_vec_mat = {MNULL},
*err_sig_pt_mat = {MNULL}, *result = {MNULL}, *result_larger = {MNULL}, *result1 = {MNULL}, *Meas_err_mat = {MNULL},
*P_zz = {MNULL}, *iP_vv = {MNULL}, *P_xz = {MNULL}, *K = {MNULL}, *result2 = {MNULL}, *result3 = {MNULL}, *C = {MNULL};
int update_mag_vec, update_sun_vec, update_omega, i, j;
double d_res;
if (inertia == MNULL)
{
inertia = m_get(3,3);
m_ident(inertia);
inertia->me[0][0] = 0.007;
inertia->me[1][1] = 0.014;
inertia->me[2][2] = 0.015;
}
if (initialize == 0){
iter_num = 1;
n_states = (7+6);
n_err_states = (6+6);
n_sig_pts = 2*n_err_states+1;
alpha = sqrt(3);
kappa = 3 - n_states;
lambda = alpha*alpha * (n_err_states+kappa) - n_err_states;
beta = -(1-(alpha*alpha));
w_m_0 = (lambda)/(n_err_states + lambda);
w_c_0 = (lambda/(n_err_states + lambda)) + (1 - (alpha*alpha) + beta);
w_i = 0.5/(n_err_states +lambda);
initialize = 1;
sqrt_lambda = (lambda+n_err_states);
if(q_s_c == VNULL)
{
q_s_c = v_get(4);
q_s_c->ve[0] = -0.020656;
q_s_c->ve[1] = 0.71468;
q_s_c->ve[2] = -0.007319;
q_s_c->ve[3] = 0.6991;
}
if(Torq_prev == VNULL)
{
Torq_prev = v_get(3);
v_zero(Torq_prev);
}
quat_normalize(q_s_c);
}
result = m_get(9,9);
m_zero(result);
result1 = m_get(n_err_states, n_err_states);
m_zero(result1);
if(x_m_o == VNULL)
{
x_m_o = v_get(n_states);
v_zero(x_m_o);
}
x = v_get(n_states);
v_zero(x);
x_err_priori = v_get(n_err_states);
v_zero(x_err_priori);
x_ang_vel = v_get(3);
v_zero(x_ang_vel);
sig_pt = m_get(n_states, n_err_states);
m_zero(sig_pt);
if (C == MNULL)
{
C = m_get(9, 12);
m_zero(C);
}
if (P_priori == MNULL)
{
P_priori = m_get(n_err_states, n_err_states);
m_zero(P_priori);
}
if (Q == MNULL)
{
Q = m_get(n_err_states, n_err_states);
m_ident(Q);
//
Q->me[0][0] = 0.0001;
Q->me[1][1] = 0.0001;
Q->me[2][2] = 0.0001;
Q->me[3][3] = 0.0001;
Q->me[4][4] = 0.0001;
Q->me[5][5] = 0.0001;
Q->me[6][6] = 0.000001;
Q->me[7][7] = 0.000001;
Q->me[8][8] = 0.000001;
Q->me[9][9] = 0.000001;
Q->me[10][10] = 0.000001;
Q->me[11][11] = 0.000001;
}
if( Pprev == MNULL)
{
Pprev = m_get(n_err_states, n_err_states);
m_ident(Pprev);
Pprev->me[0][0] = 1e-3;
Pprev->me[1][1] = 1e-3;
Pprev->me[2][2] = 1e-3;
Pprev->me[3][3] = 1e-3;
Pprev->me[4][4] = 1e-3;
Pprev->me[5][5] = 1e-3;
Pprev->me[6][6] = 1e-4;
Pprev->me[7][7] = 1e-4;
Pprev->me[8][8] = 1e-4;
Pprev->me[9][9] = 1e-3;
Pprev->me[10][10] = 1e-3;
Pprev->me[11][11] = 1e-3;
}
if (R == MNULL)
{
R = m_get(9,9);
m_ident(R);
R->me[0][0] = 0.034;
R->me[1][1] = 0.034;
R->me[2][2] = 0.034;
R->me[3][3] = 0.00027;
R->me[4][4] = 0.00027;
R->me[5][5] = 0.00027;
R->me[6][6] = 0.000012;
R->me[7][7] = 0.000012;
R->me[8][8] = 0.000012;
}
if(eclipse==0)
{
R->me[0][0] = 0.00034;
R->me[1][1] = 0.00034;
R->me[2][2] = 0.00034;
R->me[3][3] = 0.00027;
R->me[4][4] = 0.00027;
R->me[5][5] = 0.00027;
R->me[6][6] = 0.0000012;
R->me[7][7] = 0.0000012;
R->me[8][8] = 0.0000012;
Q->me[0][0] = 0.00001;
Q->me[1][1] = 0.00001;
Q->me[2][2] = 0.00001;
Q->me[3][3] = 0.0001;//0.000012;//0.0175;//1e-3;
Q->me[4][4] = 0.0001;//0.0175;//1e-3;
Q->me[5][5] = 0.0001;//0.0175;//1e-3;
Q->me[6][6] = 0.0000000001;//1e-6;
Q->me[7][7] = 0.0000000001;
Q->me[8][8] = 0.0000000001;
Q->me[9][9] = 0.0000000001;
Q->me[10][10] = 0.0000000001;
Q->me[11][11] = 0.0000000001;
}
else
{
R->me[0][0] = 0.34;
R->me[1][1] = 0.34;
R->me[2][2] = 0.34;
R->me[3][3] = 0.0027;
R->me[4][4] = 0.0027;
R->me[5][5] = 0.0027;
R->me[6][6] = 0.0000012;
R->me[7][7] = 0.0000012;
R->me[8][8] = 0.0000012;
Q->me[0][0] = 0.00001;
Q->me[1][1] = 0.00001;
Q->me[2][2] = 0.00001;
Q->me[3][3] = 0.0001;
Q->me[4][4] = 0.0001;
Q->me[5][5] = 0.0001;
Q->me[6][6] = 0.0000000001;
Q->me[7][7] = 0.0000000001;
Q->me[8][8] = 0.0000000001;
Q->me[9][9] = 0.0000000001;
Q->me[10][10] = 0.0000000001;
Q->me[11][11] = 0.0000000001;
}
if(omega_prev == VNULL)
{
omega_prev = v_get(3);
v_zero(omega_prev);
}
if(mag_vec_prev == VNULL)
{
mag_vec_prev = v_get(3);
v_zero(mag_vec_prev);
}
if(sun_vec_prev == VNULL)
{
sun_vec_prev = v_get(3);
v_zero(sun_vec_prev);
}
if (err_sig_pt_mat == MNULL)
{
err_sig_pt_mat = m_get(n_err_states, n_sig_pts);
m_zero(err_sig_pt_mat);
}
if(q_err_quat == VNULL)
{
q_err_quat = v_get(4);
// q_err_quat = v_resize(q_err_quat,4);
v_zero(q_err_quat);
}
if(err_vec == VNULL)
{
err_vec = v_get(3);
v_zero(err_vec);
}
v_temp = v_get(9);
v_resize(v_temp,3);
if(x_prev == VNULL)
{
x_prev = v_get(n_states);
v_zero(x_prev);
x_prev->ve[3] = 1;
quat_mul(x_prev,q_s_c,x_prev);
x_prev->ve[4] = 0.0;
x_prev->ve[5] = 0.0;
x_prev->ve[6] = 0.0;
x_prev->ve[7] = 0.0;
x_prev->ve[8] = 0.0;
x_prev->ve[9] = 0.0;
x_prev->ve[10] = 0.0;
x_prev->ve[11] = 0.0;
x_prev->ve[12] = 0.0;
}
sqrt_P = m_get(n_err_states, n_err_states);
m_zero(sqrt_P);
//result = m_resize(result, n_err_states, n_err_states);
result_larger = m_get(n_err_states, n_err_states);
int n, m;
for(n = 0; n < result->n; n++)
{
for(m = 0; m < result->m; m++)
{
result_larger->me[m][n] = result->me[m][n];
}
}
//v_resize(v_temp, n_err_states);
V_FREE(v_temp);
v_temp = v_get(n_err_states);
symmeig(Pprev, result_larger, v_temp);
i = 0;
for (j=0;j<n_err_states;j++){
if(v_temp->ve[j]>=0);
else{
i = 1;
}
}
m_copy(Pprev, result1);
sm_mlt(sqrt_lambda, result1, result_larger);
catchall(CHfactor(result_larger), printerr(sim_time));
for(i=0; i<n_err_states; i++){
for(j=i+1; j<n_err_states; j++){
result_larger->me[i][j] = 0;
}
}
expandstate(result_larger, x_prev, sig_pt);
sig_vec_mat = m_get(n_states, n_sig_pts);
m_zero(sig_vec_mat);
for(j = 0; j<(n_err_states+1); j++)
{
for(i = 0; i<n_states; i++)
{
if(j==0)
{
sig_vec_mat->me[i][j] = x_prev->ve[i];
}
else if(j>0)
{
sig_vec_mat->me[i][j] = sig_pt->me[i][j-1];
}
}
}
sm_mlt(-1,result_larger,result_larger);
expandstate(result_larger, x_prev, sig_pt);
for(j = (n_err_states+1); j<n_sig_pts; j++)
{
for(i = 0; i<n_states; i++)
{
sig_vec_mat->me[i][j] = sig_pt->me[i][j-(n_err_states+1)];
}
}
single_sig_pt = v_get(n_states);
quat_rot_vec(q_s_c, Torq_ext);
for(j=0; j<(n_sig_pts); j++)
{
//v_temp = v_resize(v_temp,n_states);
V_FREE(v_temp);
v_temp = v_get(n_states);
get_col(sig_vec_mat, j, single_sig_pt);
v_copy(single_sig_pt, v_temp);
rk4(t, v_temp, h, Torq_prev);
set_col(sig_vec_mat, j, v_temp);
}
v_copy(Torq_ext, Torq_prev);
x_priori = v_get(n_states);
v_zero(x_priori);
v_resize(v_temp,n_states);
v_zero(v_temp);
for(j=0; j<n_sig_pts; j++)
{
get_col( sig_vec_mat, j, v_temp);
if(j == 0)
{
v_mltadd(x_priori, v_temp, w_m_0, x_priori);
}
else
{
v_mltadd(x_priori, v_temp, w_i, x_priori);
}
}
v_copy(x_priori, v_temp);
v_resize(v_temp,4);
quat_normalize(v_temp);//zaroori hai ye
for(i=0; i<4; i++)
{
x_priori->ve[i] = v_temp->ve[i];
}
v_resize(v_temp, n_states);
v_copy(x_priori, v_temp);
v_resize(v_temp, 4);
quat_inv(v_temp, v_temp);
for(i=0; i<3; i++)
{
x_ang_vel->ve[i] = x_priori->ve[i+4];
}
x_b_m = v_get(3);
v_zero(x_b_m);
x_b_g = v_get(3);
v_zero(x_b_g);
/////////////////////////check it!!!!!!!! checked... doesnt change much the estimate
for(i=0; i<3; i++)
{
x_b_m->ve[i] = x_priori->ve[i+7];
x_b_g->ve[i] = x_priori->ve[i+10];
}
v_temp2 = v_get(n_states);
v_zero(v_temp2);
for(j=0; j<n_sig_pts; j++)
{
v_resize(v_temp2, n_states);
get_col( sig_vec_mat, j, v_temp2);
for(i=0; i<3; i++)
{
err_vec->ve[i] = v_temp2->ve[i+4];
}
v_resize(v_temp2, 4);
quat_mul(v_temp2, v_temp, q_err_quat);
v_resize(q_err_quat, n_err_states);
v_sub(err_vec, x_ang_vel, err_vec);
for(i=3; i<6; i++)
{
q_err_quat->ve[i] = err_vec->ve[i-3];
}
for(i=0; i<3; i++)
{
err_vec->ve[i] = v_temp2->ve[i+7];
}
v_sub(err_vec, x_b_m, err_vec);
for(i=6; i<9; i++)
{
q_err_quat->ve[i] = err_vec->ve[i-6];
}
for(i=0; i<3; i++)
{
err_vec->ve[i] = v_temp2->ve[i+10];
}
v_sub(err_vec, x_b_g, err_vec);
for(i=9; i<12; i++)
{
q_err_quat->ve[i] = err_vec->ve[i-9];
}
set_col(err_sig_pt_mat, j, q_err_quat);
if(j==0){
v_mltadd(x_err_priori, q_err_quat, w_m_0, x_err_priori);
}
else{
v_mltadd(x_err_priori, q_err_quat, w_i, x_err_priori);
}
}
v_resize(v_temp,n_err_states);
for (j=0;j<13;j++)
{
get_col(err_sig_pt_mat, j, v_temp);
v_sub(v_temp, x_err_priori, v_temp);
get_dyad(v_temp, v_temp, result_larger);
if(j==0){
sm_mlt(w_c_0, result_larger, result_larger);
}
else{
sm_mlt(w_i, result_larger, result_larger);
}
m_add(P_priori, result_larger, P_priori);
}
symmeig(P_priori, result_larger, v_temp);
i = 0;
for (j=0;j<n_err_states;j++){
if(v_temp->ve[j]>=0);
else{
i = 1;
}
}
m_add(P_priori, Q, P_priori);
v_resize(v_temp,3);
meas = v_get(9);
if (!(is_vec_equal(sun_vec, sun_vec_prev)) /*&& (eclipse==0)*/ ){
update_sun_vec =1;
v_copy(sun_vec, sun_vec_prev);
v_copy(sun_vec, v_temp);
normalize_vec(v_temp);
quat_rot_vec(q_s_c, v_temp);
normalize_vec(v_temp);
for(i = 0; i<3;i++){
meas->ve[i] = v_temp->ve[i];
}
}
else{
update_sun_vec =0;
for(i = 0; i<3;i++){
meas->ve[i] = 0;
}
}
if (!(is_vec_equal(mag_vec, mag_vec_prev)) ){
update_mag_vec =1;
v_copy(mag_vec, mag_vec_prev);
v_copy(mag_vec, v_temp);
normalize_vec(v_temp);
quat_rot_vec(q_s_c, v_temp);
normalize_vec(v_temp);
for(i=3; i<6; i++){
meas->ve[i] = v_temp->ve[i-3];
}
}
else{
update_mag_vec =0;
for(i=3; i<6; i++){
meas->ve[i] = 0;//mag_vec_prev->ve[i-3];
}
}
if (!(is_vec_equal(omega, omega_prev) ) ){
update_omega =1;
v_copy(omega, omega_prev);
v_copy(omega, v_temp);
quat_rot_vec(q_s_c, v_temp);
for(i=6; i<9; i++){
meas->ve[i] = v_temp->ve[i-6];
}
}
else{
update_omega =0;
for(i=6; i<9; i++){
meas->ve[i] = 0;
}
}
v_resize(v_temp, 9);
v_resize(v_temp2, n_states);
v_temp3 = v_get(3);
Meas_err_mat = m_get(9, n_sig_pts);
m_zero(Meas_err_mat);
meas_priori = v_get(9);
v_zero(meas_priori);
for(j=0; j<n_sig_pts; j++)
{
get_col( sig_vec_mat, j, v_temp2);
if(update_omega){
for(i=6;i<9;i++){
v_temp->ve[i] = v_temp2->ve[i-2] + x_b_g->ve[i-6];
}
}
else{
for(i=6;i<9;i++){
v_temp->ve[i] = 0;
}
}
v_resize(v_temp2, 4);
if(update_sun_vec){
for(i=0;i<3;i++){
v_temp3->ve[i] = sun_vec_I->ve[i];
}
quat_rot_vec(v_temp2, v_temp3);
normalize_vec(v_temp3);
for(i=0;i<3;i++){
v_temp->ve[i] = v_temp3->ve[i];
}
}
else{
for(i=0;i<3;i++){
v_temp->ve[i] = 0;
}
}
if(update_mag_vec){
for(i=0;i<3;i++){
v_temp3->ve[i] = mag_vec_I->ve[i];
}
normalize_vec(v_temp3);
for(i=0;i<3;i++){
v_temp3->ve[i] = v_temp3->ve[i] + x_b_m->ve[i];
}
quat_rot_vec(v_temp2, v_temp3);
normalize_vec(v_temp3);
for(i=3;i<6;i++){
v_temp->ve[i] = v_temp3->ve[i-3];
}
}
else{
for(i=3;i<6;i++){
v_temp->ve[i] = 0;
}
}
set_col(Meas_err_mat, j, v_temp);
if(j==0){
v_mltadd(meas_priori, v_temp, w_m_0, meas_priori);
}
else{
v_mltadd(meas_priori, v_temp, w_i, meas_priori);
}
}
v_resize(v_temp, 9);
m_resize(result_larger, 9, 9);
m_zero(result_larger);
P_zz = m_get(9, 9);
m_zero(P_zz);
iP_vv = m_get(9, 9);
m_zero(iP_vv);
P_xz = m_get(n_err_states, 9);
m_zero(P_xz);
v_resize(v_temp2, n_err_states);
result1 = m_resize(result1,n_err_states,9);
for (j=0; j<n_sig_pts; j++)
{
get_col( Meas_err_mat, j, v_temp);
get_col( err_sig_pt_mat, j, v_temp2);
v_sub(v_temp, meas_priori, v_temp);
get_dyad(v_temp, v_temp, result_larger);
get_dyad(v_temp2, v_temp, result1);
if(j==0){
sm_mlt(w_c_0, result_larger, result_larger);
sm_mlt(w_c_0, result1, result1);
}
else{
sm_mlt(w_i, result_larger, result_larger);
sm_mlt(w_i, result1, result1);
}
m_add(P_zz, result_larger, P_zz);
m_add(P_xz, result1, P_xz);
}
symmeig(P_zz, result_larger, v_temp);
i = 0;
for (j=0; j<9; j++){
if(v_temp->ve[j]>=0);
else{
i = 1;
}
}
m_add(P_zz, R, P_zz);
m_inverse(P_zz, iP_vv);
K = m_get(n_err_states, 9);
m_zero(K);
m_mlt(P_xz, iP_vv, K);
if(x_posteriori_err == VNULL)
{
x_posteriori_err = v_get(n_err_states);
v_zero(x_posteriori_err);
}
v_resize(v_temp,9);
v_sub(meas, meas_priori, v_temp);
v_copy(v_temp, residual);
mv_mlt(K, v_temp, x_posteriori_err);
v_resize(v_temp2,3);
for(i=0;i<3;i++){
v_temp2->ve[i] = x_posteriori_err->ve[i];
}
for(i=4; i<n_states; i++){
x_prev->ve[i] = (x_posteriori_err->ve[i-1] + x_priori->ve[i]);
}
d_res = v_norm2(v_temp2);
v_resize(v_temp2,4);
if(d_res<=1 /*&& d_res!=0*/){
v_temp2->ve[0] = v_temp2->ve[0];
v_temp2->ve[1] = v_temp2->ve[1];
v_temp2->ve[2] = v_temp2->ve[2];
v_temp2->ve[3] = sqrt(1-d_res);
}
else//baad main daala hai
{
v_temp2->ve[0] = (v_temp2->ve[0])/(sqrt(1+d_res));
v_temp2->ve[1] = (v_temp2->ve[1])/(sqrt(1+d_res));
v_temp2->ve[2] = (v_temp2->ve[2])/(sqrt(1+d_res));
v_temp2->ve[3] = 1/sqrt(1 + d_res);
}
v_resize(x_posteriori_err, n_states);
for(i=(n_states-1); i>3; i--){
x_posteriori_err->ve[i] = x_posteriori_err->ve[i-1];
}
for(i=0; i<4; i++){
x_posteriori_err->ve[i] = v_temp2->ve[i];
}
quat_mul(v_temp2, x_priori, v_temp2);
for(i=0;i<4;i++){
x_prev->ve[i] = v_temp2->ve[i];
}
m_resize(result_larger, n_err_states, 9);
m_mlt(K, P_zz, result_larger);
result2 = m_get(9, n_err_states);
m_transp(K,result2);
m_resize(result1, n_err_states, n_err_states);
m_mlt(result_larger, result2, result1);
v_resize(v_temp, n_err_states);
m_sub(P_priori, result1, Pprev);
symmeig(Pprev, result1 , v_temp);
i = 0;
for (j=0;j<n_err_states;j++){
if(v_temp->ve[j]>=0);
else{
i = 1;
}
}
v_copy(x_prev, v_temp);
v_resize(v_temp,4);
v_copy(x_prev, v_temp2);
v_resize(v_temp2,4);
v_copy(x_prev, x_m_o);
//v_resize(x_m_o, 4);
v_resize(v_temp,3);
quat_inv(q_s_c, v_temp2);
v_copy( x_prev, state);
quat_mul(state, v_temp2, state);
for(i=0; i<3; i++){
v_temp->ve[i] = state->ve[i+4];
}
quat_rot_vec(v_temp2, v_temp);
for(i=0; i<3; i++){
state->ve[i+4] = v_temp->ve[i];
}
v_copy( x_posteriori_err, st_error);
iter_num++;
V_FREE(x);
V_FREE(x_priori);
V_FREE(x_err_priori);
V_FREE(single_sig_pt);
V_FREE(v_temp);
V_FREE(q_err_quat);
V_FREE(err_vec);
V_FREE(v_temp2);
V_FREE(x_ang_vel);
V_FREE(meas);
V_FREE(meas_priori);
V_FREE(v_temp3);
V_FREE(x_posteriori_err);
V_FREE(x_b_m);
V_FREE(x_b_g);
M_FREE(sqrt_P);
M_FREE(P);
M_FREE(P_priori);
M_FREE(sig_pt);
M_FREE(sig_vec_mat);
M_FREE(err_sig_pt_mat);
M_FREE(result);
M_FREE(result_larger);
M_FREE(result1);
M_FREE(Meas_err_mat);
M_FREE(P_zz);
M_FREE(iP_vv);
M_FREE(P_xz);
M_FREE(K);
M_FREE(result2);
M_FREE(result3);
}
void parrinellorahman_pcoupl(FILE *fplog,gmx_step_t step,
t_inputrec *ir,real dt,tensor pres,
tensor box,tensor box_rel,tensor boxv,
tensor M,matrix mu,bool bFirstStep)
{
/* This doesn't do any coordinate updating. It just
* integrates the box vector equations from the calculated
* acceleration due to pressure difference. We also compute
* the tensor M which is used in update to couple the particle
* coordinates to the box vectors.
*
* In Nose and Klein (Mol.Phys 50 (1983) no 5., p 1055) this is
* given as
* -1 . . -1
* M_nk = (h') * (h' * h + h' h) * h
*
* with the dots denoting time derivatives and h is the transformation from
* the scaled frame to the real frame, i.e. the TRANSPOSE of the box.
* This also goes for the pressure and M tensors - they are transposed relative
* to ours. Our equation thus becomes:
*
* -1 . . -1
* M_gmx = M_nk' = b * (b * b' + b * b') * b'
*
* where b is the gromacs box matrix.
* Our box accelerations are given by
* .. ..
* b = vol/W inv(box') * (P-ref_P) (=h')
*/
int d,n;
tensor winv;
real vol=box[XX][XX]*box[YY][YY]*box[ZZ][ZZ];
real atot,arel,change,maxchange,xy_pressure;
tensor invbox,pdiff,t1,t2;
real maxl;
m_inv_ur0(box,invbox);
if (!bFirstStep) {
/* Note that PRESFAC does not occur here.
* The pressure and compressibility always occur as a product,
* therefore the pressure unit drops out.
*/
maxl=max(box[XX][XX],box[YY][YY]);
maxl=max(maxl,box[ZZ][ZZ]);
for(d=0;d<DIM;d++)
for(n=0;n<DIM;n++)
winv[d][n]=
(4*M_PI*M_PI*ir->compress[d][n])/(3*ir->tau_p*ir->tau_p*maxl);
m_sub(pres,ir->ref_p,pdiff);
if(ir->epct==epctSURFACETENSION) {
/* Unlike Berendsen coupling it might not be trivial to include a z
* pressure correction here? On the other hand we don't scale the
* box momentarily, but change accelerations, so it might not be crucial.
*/
xy_pressure=0.5*(pres[XX][XX]+pres[YY][YY]);
for(d=0;d<ZZ;d++)
pdiff[d][d]=(xy_pressure-(pres[ZZ][ZZ]-ir->ref_p[d][d]/box[d][d]));
}
tmmul(invbox,pdiff,t1);
/* Move the off-diagonal elements of the 'force' to one side to ensure
* that we obey the box constraints.
*/
for(d=0;d<DIM;d++) {
for(n=0;n<d;n++) {
t1[d][n] += t1[n][d];
t1[n][d] = 0;
}
}
switch (ir->epct) {
case epctANISOTROPIC:
for(d=0;d<DIM;d++)
for(n=0;n<=d;n++)
t1[d][n] *= winv[d][n]*vol;
break;
case epctISOTROPIC:
/* calculate total volume acceleration */
atot=box[XX][XX]*box[YY][YY]*t1[ZZ][ZZ]+
box[XX][XX]*t1[YY][YY]*box[ZZ][ZZ]+
t1[XX][XX]*box[YY][YY]*box[ZZ][ZZ];
arel=atot/(3*vol);
/* set all RELATIVE box accelerations equal, and maintain total V
* change speed */
for(d=0;d<DIM;d++)
for(n=0;n<=d;n++)
t1[d][n] = winv[0][0]*vol*arel*box[d][n];
break;
case epctSEMIISOTROPIC:
case epctSURFACETENSION:
/* Note the correction to pdiff above for surftens. coupling */
/* calculate total XY volume acceleration */
atot=box[XX][XX]*t1[YY][YY]+t1[XX][XX]*box[YY][YY];
arel=atot/(2*box[XX][XX]*box[YY][YY]);
/* set RELATIVE XY box accelerations equal, and maintain total V
* change speed. Dont change the third box vector accelerations */
for(d=0;d<ZZ;d++)
for(n=0;n<=d;n++)
t1[d][n] = winv[d][n]*vol*arel*box[d][n];
for(n=0;n<DIM;n++)
t1[ZZ][n] *= winv[d][n]*vol;
break;
default:
gmx_fatal(FARGS,"Parrinello-Rahman pressure coupling type %s "
"not supported yet\n",EPCOUPLTYPETYPE(ir->epct));
break;
}
maxchange=0;
for(d=0;d<DIM;d++)
for(n=0;n<=d;n++) {
boxv[d][n] += dt*t1[d][n];
/* We do NOT update the box vectors themselves here, since
* we need them for shifting later. It is instead done last
* in the update() routine.
*/
/* Calculate the change relative to diagonal elements -
* since it's perfectly ok for the off-diagonal ones to
* be zero it doesn't make sense to check the change relative
* to its current size.
*/
change=fabs(dt*boxv[d][n]/box[d][d]);
if(change>maxchange)
maxchange=change;
}
if (maxchange > 0.01 && fplog) {
char buf[22];
fprintf(fplog,"\nStep %s Warning: Pressure scaling more than 1%%.\n",
gmx_step_str(step,buf));
}
}
preserve_box_shape(ir,box_rel,boxv);
mtmul(boxv,box,t1); /* t1=boxv * b' */
mmul(invbox,t1,t2);
mtmul(t2,invbox,M);
/* Determine the scaling matrix mu for the coordinates */
for(d=0;d<DIM;d++)
for(n=0;n<=d;n++)
t1[d][n] = box[d][n] + dt*boxv[d][n];
preserve_box_shape(ir,box_rel,t1);
/* t1 is the box at t+dt, determine mu as the relative change */
mmul_ur0(invbox,t1,mu);
}
int main( void) {
FILE *fp;
MATRIX_T *a, *b, *c, *d, *inverse, *test, *x, *ainv;
double D;
/* initialize all MATRIX_T pointers to NULL */
a = NULL;
b = NULL;
c = NULL;
d = NULL;
inverse = NULL;
test = NULL;
x = NULL;
ainv = NULL;
/* it is good practice to reset the error code
before doing matrix calculations */
m_reseterr();
/* open matrix file to initialize matrix variables */
if ((fp = fopen("mtest1.mat","r"))==NULL) {
printf("cannot open file mtest1.mat\n");
exit(0);
}
/* use m_printf functions here */
/* test matrix addition */
a = m_fnew( fp);
m_printf( "\n# matrix a from file: mtest1.mat", "%6.2f", a);
b = m_fnew( fp);
m_printf( "\n# matrix b from: mtest1.mat", "%6.2f", b);
c = m_new( 3, 2);
c = m_add( c, a, b);
m_printf( "\n# sum of a and b", "%6.2f", c);
/* test matrix subtraction */
c = m_sub( c, a, b);
m_printf( "\n# difference of a and b", "%6.2f", c);
/* change to using comma separated format output */
/* multiply matrix by a constant */
printf( "\ncomma separated format: matrix a\n");
m_fputcsv(stdout,a);
printf( "\ncomma separated format: matrix c\n");
m_fputcsv(stdout,c);
m_mupconst( c, 2.5, a);
printf( "\ncsv format: 2.5 times matrix c\n");
m_fputcsv(stdout,c);
/* find maximum element in matrix */
printf( "\nmax element in c is %f\n", m_max_abs_element(c));
/* test euclidean norm */
d = m_fnew( fp);
printf( "\ncsv format: matrix d\n");
m_fputcsv(stdout,d);
printf( "\neuclidean norm of d is %f\n", m_e_norm( d));
/* test assignment of identity matrix to a square matrix */
inverse = m_new( d->rows, d->cols);
m_assign_identity( inverse);
m_printf( "\nidentity matrix", "%6.2f", inverse);
/* test matrix inversion */
inverse = m_inverse( inverse, d, &D, 0.0001);
test = m_new(d->rows,d->cols);
test = m_mup( test, d, inverse);
m_printf( "\nmatrix d", "%6.2f", d);
m_printf( "\nmatrix inverse", "%6.2f", inverse);
m_printf( "\nproduct of d and d-inverse", "%6.2f", test);
/* test solution of linear equations */
/* start by getting new values for matrices a and b
note: b is a 1 by n vector (as is x) */
if ((a = m_fnew( fp)) == NULL) exit(0);
if ((b = m_fnew( fp)) == NULL) exit(0);
if ((x = m_new(b->rows,b->cols)) == NULL) exit(0);
printf("\ncsv: a\n");
m_fputcsv(stdout,a);
printf("\ncsv: b\n");
m_fputcsv(stdout,b);
x = m_solve( x, a, b, 0.0000001);
printf("\n\nx=ab\ncsv: solution x\n");
m_fputcsv(stdout,x);
/* close files and clean up */
fclose(fp);
m_free(a);
m_free(b);
m_free(c);
m_free(d);
m_free(inverse);
m_free(test);
m_free(x);
m_free(ainv);
}
