@@ -1115,60 +1115,111 @@ CMATRIX compute_F_cost_matrix(CMATRIX& F_curr, CMATRIX& F_prev, MATRIX& e_curr,
11151115
11161116 MATRIX vel (ndof, 1 ); vel.dot_product (iM, p);
11171117 MATRIX tmp (ndof, 1 );
1118- tmp = fi_curr.T (); ai.dot_product (iM, tmp);
1119- tmp = fj_curr.T (); aj.dot_product (iM, tmp);
11201118
11211119 double val = 0.0 ;
11221120 for (i=0 ; i<nst; i++){
1123- double sum = 1.0 ;
11241121
1125- for (j=0 ;j<nst; j++){
1122+ fi_curr = F_curr.row (i).real ();
1123+ fi_prev = F_prev.row (i).real ();
1124+ tmp = fi_curr.T (); ai.dot_product (iM, tmp);
1125+ dq_i = (vel * dt + 0.5 * ai * dt * dt);
1126+ double dE_i = -(fi_prev * dq_i).get (0 ,0 );
11261127
1127- fi_curr = F_curr.row (i).real ();
1128- fi_prev = F_prev.row (i).real ();
1128+ for (j=0 ;j<nst; j++){
11291129
11301130 fj_curr = F_curr.row (j).real ();
11311131 fj_prev = F_prev.row (j).real ();
1132-
1133- dq_i = (vel * dt + 0.5 * ai * dt * dt);
1134- double dE = e_prev.get (j,j) - e_prev.get (i,i);
1135- double dE_i = -(fi_prev * dq_i).get (0 ,0 );
1136-
1132+ tmp = fj_curr.T (); aj.dot_product (iM, tmp);
11371133 dq_j = (vel * dt + 0.5 * aj * dt * dt);
11381134 double dE_j = -(fj_prev * dq_j).get (0 ,0 );
1135+
1136+ double dE = e_prev.get (j,j) - e_prev.get (i,i);
11391137 double dE_new = dE + dE_j - dE_i;
11401138
11411139 val = 0.5 *(1.0 - SIGN ( dE ) * SIGN ( dE_new )) ;
1142- /*
1143- if(i==j){ val = 0.0; }
1144- //else{ val = 1.0 - SIGN( dE ) * SIGN( dE_new ); sum -= val; }
1145- else{
1146- double val1;
1147- val = SIGN( dE ) * SIGN( dE_new );
1148- if(val<0.0){ val1 = 1.0; }
1149- else{ val1 = 0.0; }
1150- val = val1;
1151- //sum -= val;
1152- }
1153- */
11541140
11551141 res.set (i, j, complex <double >( val, 0.0 ) );
11561142 }// for j
1157-
1158- // if(sum<=0.0){ sum = 0.0; }
1159- // res.set(i, i, complex<double>(sum, 0.0) );
11601143 }// for i
11611144
11621145 return res;
11631146}
11641147
11651148
1149+
1150+ vector<MATRIX> compute_F_cost_matrix_dof_resolved (CMATRIX& F_curr, CMATRIX& F_prev, MATRIX& e_curr, MATRIX& e_prev, MATRIX& _p, MATRIX& iM, double dt, int act_state){
1151+ /* * F_curr, F_prev - CMATRIX(nadi, ndof)
1152+ p - MATRIX(ndof, 1) - current momentum
1153+ iM - MATRIX(ndof, 1) - inverse matrices
1154+ dt - time step
1155+ act_state - the current active state
1156+ */
1157+ int i,j;
1158+ int ndof = F_curr.n_cols ;
1159+ int nst = F_curr.n_rows ;
1160+ vector<MATRIX> res (ndof, MATRIX (nst, nst) );
1161+ // MATRIX fi_curr(1, ndof);
1162+ // MATRIX fj_curr(1, ndof);
1163+ // MATRIX fi_prev(1, ndof);
1164+ // MATRIX fj_prev(1, ndof);
1165+ // MATRIX ai(ndof, 1);
1166+ // MATRIX aj(ndof, 1);
1167+ // MATRIX dq_i(ndof,1);
1168+ // MATRIX dq_j(ndof,1);
1169+ MATRIX p (_p);
1170+
1171+ // correction, because the current p is already a half-step propagated using old forces
1172+ // p -= dt * F_prev.row(act_state).real().T();
1173+
1174+ MATRIX vel (ndof, 1 ); vel.dot_product (iM, p);
1175+ MATRIX tmp (ndof, 1 );
1176+
1177+ double val = 0.0 ;
1178+ for (int idof=0 ; idof<ndof; idof++){
1179+
1180+ double vel = p.get (idof, 0 ) * iM.get (idof, 0 );
1181+
1182+ for (i=0 ; i<nst; i++){
1183+
1184+ double fi_curr = F_curr.get (i,idof).real ();
1185+ double fi_prev = F_prev.get (i,idof).real ();
1186+ double ai = fi_curr * iM.get (idof,0 );
1187+ double dq_i = vel * dt + 0.5 * ai * dt * dt;
1188+ double dE_i = -fi_prev * dq_i;
1189+
1190+ for (j=0 ;j<nst; j++){
1191+
1192+ double fj_curr = F_curr.get (j,idof).real ();
1193+ double fj_prev = F_prev.get (j,idof).real ();
1194+ double aj = fj_curr * iM.get (idof,0 );
1195+ double dq_j = vel * dt + 0.5 * ai * dt * dt;
1196+ double dE_j = -fj_prev * dq_j;
1197+
1198+ double dE = e_prev.get (j,j) - e_prev.get (i,i);
1199+ double dE_new = dE + dE_j - dE_i;
1200+
1201+ val = SIGN ( dE ) * SIGN ( dE_new ) ;
1202+ res[idof].set (i, j, val);
1203+
1204+ }// for j
1205+ }// for i
1206+ }// for idof
1207+
1208+ return res;
1209+ }
1210+
1211+
1212+
1213+
11661214CMATRIX compute_F_cost_matrix2 (CMATRIX& F_curr, CMATRIX& F_prev, MATRIX& e_curr, MATRIX& e_prev, MATRIX& _p, MATRIX& iM, double dt, int act_state){
11671215/* * F_curr, F_prev - CMATRIX(nadi, ndof)
11681216 p - MATRIX(ndof, 1) - current momentum
11691217 iM - MATRIX(ndof, 1) - inverse matrices
11701218 dt - time step
11711219 act_state - the current active state
1220+
1221+ This is an experimental function - merely a placeholder - not used anywhere yet!
1222+
11721223*/
11731224 int i,j;
11741225 int ndof = F_curr.n_cols ;
0 commit comments