Skip to content

Commit

Permalink
Further exploit the symmetry of matrices
Browse files Browse the repository at this point in the history
  • Loading branch information
@HanatoK
HanatoK committed Jul 11, 2024
1 parent 66542b3 commit b1e317d
Showing 1 changed file with 105 additions and 105 deletions.
210 changes: 105 additions & 105 deletions src/colvar_rotation_derivative.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -86,17 +86,17 @@ struct rotation_derivative {
tmp_Q0Q0[0][1] = Q0[0] * Q0[1];
tmp_Q0Q0[0][2] = Q0[0] * Q0[2];
tmp_Q0Q0[0][3] = Q0[0] * Q0[3];
tmp_Q0Q0[1][0] = tmp_Q0Q0[0][1];
// tmp_Q0Q0[1][0] = tmp_Q0Q0[0][1];
tmp_Q0Q0[1][1] = Q0[1] * Q0[1];
tmp_Q0Q0[1][2] = Q0[1] * Q0[2];
tmp_Q0Q0[1][3] = Q0[1] * Q0[3];
tmp_Q0Q0[2][0] = tmp_Q0Q0[0][2];
tmp_Q0Q0[2][1] = tmp_Q0Q0[1][2];
// tmp_Q0Q0[2][0] = tmp_Q0Q0[0][2];
// tmp_Q0Q0[2][1] = tmp_Q0Q0[1][2];
tmp_Q0Q0[2][2] = Q0[2] * Q0[2];
tmp_Q0Q0[2][3] = Q0[2] * Q0[3];
tmp_Q0Q0[3][0] = tmp_Q0Q0[0][3];
tmp_Q0Q0[3][1] = tmp_Q0Q0[1][3];
tmp_Q0Q0[3][2] = tmp_Q0Q0[2][3];
// tmp_Q0Q0[3][0] = tmp_Q0Q0[0][3];
// tmp_Q0Q0[3][1] = tmp_Q0Q0[1][3];
// tmp_Q0Q0[3][2] = tmp_Q0Q0[2][3];
tmp_Q0Q0[3][3] = Q0[3] * Q0[3];
}
if (require_dl_dq & rotation_derivative_dldq::use_dq) {
Expand Down Expand Up @@ -186,10 +186,10 @@ struct rotation_derivative {
(Q20Q0[3] + Q23Q0[0]) * Q2_s[3] +
(Q30Q0[3] + Q33Q0[0]) * Q3_s[3]);

tmp_Q0Q0_L[0][1][0] = tmp_Q0Q0_L[0][0][1];
tmp_Q0Q0_L[1][1][0] = tmp_Q0Q0_L[1][0][1];
tmp_Q0Q0_L[2][1][0] = tmp_Q0Q0_L[2][0][1];
tmp_Q0Q0_L[3][1][0] = tmp_Q0Q0_L[3][0][1];
// tmp_Q0Q0_L[0][1][0] = tmp_Q0Q0_L[0][0][1];
// tmp_Q0Q0_L[1][1][0] = tmp_Q0Q0_L[1][0][1];
// tmp_Q0Q0_L[2][1][0] = tmp_Q0Q0_L[2][0][1];
// tmp_Q0Q0_L[3][1][0] = tmp_Q0Q0_L[3][0][1];

tmp_Q0Q0_L[0][1][1] = Q11Q0[1] * Q1_s[0] +
Q21Q0[1] * Q2_s[0] +
Expand Down Expand Up @@ -231,15 +231,15 @@ struct rotation_derivative {
(Q31Q0[3] + Q33Q0[1]) * Q3_s[3]);


tmp_Q0Q0_L[0][2][0] = tmp_Q0Q0_L[0][0][2];
tmp_Q0Q0_L[1][2][0] = tmp_Q0Q0_L[1][0][2];
tmp_Q0Q0_L[2][2][0] = tmp_Q0Q0_L[2][0][2];
tmp_Q0Q0_L[3][2][0] = tmp_Q0Q0_L[3][0][2];

tmp_Q0Q0_L[0][2][1] = tmp_Q0Q0_L[0][1][2];
tmp_Q0Q0_L[1][2][1] = tmp_Q0Q0_L[1][1][2];
tmp_Q0Q0_L[2][2][1] = tmp_Q0Q0_L[2][1][2];
tmp_Q0Q0_L[3][2][1] = tmp_Q0Q0_L[3][1][2];
// tmp_Q0Q0_L[0][2][0] = tmp_Q0Q0_L[0][0][2];
// tmp_Q0Q0_L[1][2][0] = tmp_Q0Q0_L[1][0][2];
// tmp_Q0Q0_L[2][2][0] = tmp_Q0Q0_L[2][0][2];
// tmp_Q0Q0_L[3][2][0] = tmp_Q0Q0_L[3][0][2];
//
// tmp_Q0Q0_L[0][2][1] = tmp_Q0Q0_L[0][1][2];
// tmp_Q0Q0_L[1][2][1] = tmp_Q0Q0_L[1][1][2];
// tmp_Q0Q0_L[2][2][1] = tmp_Q0Q0_L[2][1][2];
// tmp_Q0Q0_L[3][2][1] = tmp_Q0Q0_L[3][1][2];

tmp_Q0Q0_L[0][2][2] = Q12Q0[2] * Q1_s[0] +
Q22Q0[2] * Q2_s[0] +
Expand Down Expand Up @@ -267,20 +267,20 @@ struct rotation_derivative {
(Q22Q0[3] + Q23Q0[2]) * Q2_s[3] +
(Q32Q0[3] + Q33Q0[2]) * Q3_s[3]);

tmp_Q0Q0_L[0][3][0] = tmp_Q0Q0_L[0][0][3];
tmp_Q0Q0_L[1][3][0] = tmp_Q0Q0_L[1][0][3];
tmp_Q0Q0_L[2][3][0] = tmp_Q0Q0_L[2][0][3];
tmp_Q0Q0_L[3][3][0] = tmp_Q0Q0_L[3][0][3];

tmp_Q0Q0_L[0][3][1] = tmp_Q0Q0_L[0][1][3];
tmp_Q0Q0_L[1][3][1] = tmp_Q0Q0_L[1][1][3];
tmp_Q0Q0_L[2][3][1] = tmp_Q0Q0_L[2][1][3];
tmp_Q0Q0_L[3][3][1] = tmp_Q0Q0_L[3][1][3];

tmp_Q0Q0_L[0][3][2] = tmp_Q0Q0_L[0][2][3];
tmp_Q0Q0_L[1][3][2] = tmp_Q0Q0_L[1][2][3];
tmp_Q0Q0_L[2][3][2] = tmp_Q0Q0_L[2][2][3];
tmp_Q0Q0_L[3][3][2] = tmp_Q0Q0_L[3][2][3];
// tmp_Q0Q0_L[0][3][0] = tmp_Q0Q0_L[0][0][3];
// tmp_Q0Q0_L[1][3][0] = tmp_Q0Q0_L[1][0][3];
// tmp_Q0Q0_L[2][3][0] = tmp_Q0Q0_L[2][0][3];
// tmp_Q0Q0_L[3][3][0] = tmp_Q0Q0_L[3][0][3];
//
// tmp_Q0Q0_L[0][3][1] = tmp_Q0Q0_L[0][1][3];
// tmp_Q0Q0_L[1][3][1] = tmp_Q0Q0_L[1][1][3];
// tmp_Q0Q0_L[2][3][1] = tmp_Q0Q0_L[2][1][3];
// tmp_Q0Q0_L[3][3][1] = tmp_Q0Q0_L[3][1][3];
//
// tmp_Q0Q0_L[0][3][2] = tmp_Q0Q0_L[0][2][3];
// tmp_Q0Q0_L[1][3][2] = tmp_Q0Q0_L[1][2][3];
// tmp_Q0Q0_L[2][3][2] = tmp_Q0Q0_L[2][2][3];
// tmp_Q0Q0_L[3][3][2] = tmp_Q0Q0_L[3][2][3];

tmp_Q0Q0_L[0][3][3] = Q13Q0[3] * Q1_s[0] +
Q23Q0[3] * Q2_s[0] +
Expand Down Expand Up @@ -327,20 +327,20 @@ struct rotation_derivative {
}
*/
*dl0_out = tmp_Q0Q0[0][0] * ds[0][0] +
tmp_Q0Q0[0][1] * ds[0][1] +
tmp_Q0Q0[0][2] * ds[0][2] +
tmp_Q0Q0[0][3] * ds[0][3] +
tmp_Q0Q0[1][0] * ds[1][0] +
tmp_Q0Q0[0][1] * ds[1][0] +
tmp_Q0Q0[0][2] * ds[2][0] +
tmp_Q0Q0[0][3] * ds[3][0] +
tmp_Q0Q0[0][1] * ds[1][0] +
tmp_Q0Q0[1][1] * ds[1][1] +
tmp_Q0Q0[1][2] * ds[1][2] +
tmp_Q0Q0[1][3] * ds[1][3] +
tmp_Q0Q0[2][0] * ds[2][0] +
tmp_Q0Q0[2][1] * ds[2][1] +
tmp_Q0Q0[1][2] * ds[2][1] +
tmp_Q0Q0[1][3] * ds[3][1] +
tmp_Q0Q0[0][2] * ds[2][0] +
tmp_Q0Q0[1][2] * ds[2][1] +
tmp_Q0Q0[2][2] * ds[2][2] +
tmp_Q0Q0[2][3] * ds[2][3] +
tmp_Q0Q0[3][0] * ds[3][0] +
tmp_Q0Q0[3][1] * ds[3][1] +
tmp_Q0Q0[3][2] * ds[3][2] +
tmp_Q0Q0[2][3] * ds[3][2] +
tmp_Q0Q0[0][3] * ds[3][0] +
tmp_Q0Q0[1][3] * ds[3][1] +
tmp_Q0Q0[2][3] * ds[3][2] +
tmp_Q0Q0[3][3] * ds[3][3];
}
if (dq0_out != nullptr) {
Expand All @@ -360,71 +360,71 @@ struct rotation_derivative {
}
*/
(*dq0_out)[0] = tmp_Q0Q0_L[0][0][0] * ds[0][0] +
tmp_Q0Q0_L[0][0][1] * ds[0][1] +
tmp_Q0Q0_L[0][0][2] * ds[0][2] +
tmp_Q0Q0_L[0][0][3] * ds[0][3] +
tmp_Q0Q0_L[0][1][0] * ds[1][0] +
tmp_Q0Q0_L[0][0][1] * ds[1][0] +
tmp_Q0Q0_L[0][0][2] * ds[2][0] +
tmp_Q0Q0_L[0][0][3] * ds[3][0] +
tmp_Q0Q0_L[0][0][1] * ds[1][0] +
tmp_Q0Q0_L[0][1][1] * ds[1][1] +
tmp_Q0Q0_L[0][1][2] * ds[1][2] +
tmp_Q0Q0_L[0][1][3] * ds[1][3] +
tmp_Q0Q0_L[0][2][0] * ds[2][0] +
tmp_Q0Q0_L[0][2][1] * ds[2][1] +
tmp_Q0Q0_L[0][1][2] * ds[2][1] +
tmp_Q0Q0_L[0][1][3] * ds[3][1] +
tmp_Q0Q0_L[0][0][2] * ds[2][0] +
tmp_Q0Q0_L[0][1][2] * ds[2][1] +
tmp_Q0Q0_L[0][2][2] * ds[2][2] +
tmp_Q0Q0_L[0][2][3] * ds[2][3] +
tmp_Q0Q0_L[0][3][0] * ds[3][0] +
tmp_Q0Q0_L[0][3][1] * ds[3][1] +
tmp_Q0Q0_L[0][3][2] * ds[3][2] +
tmp_Q0Q0_L[0][2][3] * ds[3][2] +
tmp_Q0Q0_L[0][0][3] * ds[3][0] +
tmp_Q0Q0_L[0][1][3] * ds[3][1] +
tmp_Q0Q0_L[0][2][3] * ds[3][2] +
tmp_Q0Q0_L[0][3][3] * ds[3][3];

(*dq0_out)[1] = tmp_Q0Q0_L[1][0][0] * ds[0][0] +
tmp_Q0Q0_L[1][0][1] * ds[0][1] +
tmp_Q0Q0_L[1][0][2] * ds[0][2] +
tmp_Q0Q0_L[1][0][3] * ds[0][3] +
tmp_Q0Q0_L[1][1][0] * ds[1][0] +
tmp_Q0Q0_L[1][0][1] * ds[1][0] +
tmp_Q0Q0_L[1][0][2] * ds[2][0] +
tmp_Q0Q0_L[1][0][3] * ds[3][0] +
tmp_Q0Q0_L[1][0][1] * ds[1][0] +
tmp_Q0Q0_L[1][1][1] * ds[1][1] +
tmp_Q0Q0_L[1][1][2] * ds[1][2] +
tmp_Q0Q0_L[1][1][3] * ds[1][3] +
tmp_Q0Q0_L[1][2][0] * ds[2][0] +
tmp_Q0Q0_L[1][2][1] * ds[2][1] +
tmp_Q0Q0_L[1][1][2] * ds[2][1] +
tmp_Q0Q0_L[1][1][3] * ds[3][1] +
tmp_Q0Q0_L[1][0][2] * ds[2][0] +
tmp_Q0Q0_L[1][1][2] * ds[2][1] +
tmp_Q0Q0_L[1][2][2] * ds[2][2] +
tmp_Q0Q0_L[1][2][3] * ds[2][3] +
tmp_Q0Q0_L[1][3][0] * ds[3][0] +
tmp_Q0Q0_L[1][3][1] * ds[3][1] +
tmp_Q0Q0_L[1][3][2] * ds[3][2] +
tmp_Q0Q0_L[1][2][3] * ds[3][2] +
tmp_Q0Q0_L[1][0][3] * ds[3][0] +
tmp_Q0Q0_L[1][1][3] * ds[3][1] +
tmp_Q0Q0_L[1][2][3] * ds[3][2] +
tmp_Q0Q0_L[1][3][3] * ds[3][3];

(*dq0_out)[2] = tmp_Q0Q0_L[2][0][0] * ds[0][0] +
tmp_Q0Q0_L[2][0][1] * ds[0][1] +
tmp_Q0Q0_L[2][0][2] * ds[0][2] +
tmp_Q0Q0_L[2][0][3] * ds[0][3] +
tmp_Q0Q0_L[2][1][0] * ds[1][0] +
tmp_Q0Q0_L[2][0][1] * ds[1][0] +
tmp_Q0Q0_L[2][0][2] * ds[2][0] +
tmp_Q0Q0_L[2][0][3] * ds[3][0] +
tmp_Q0Q0_L[2][0][1] * ds[1][0] +
tmp_Q0Q0_L[2][1][1] * ds[1][1] +
tmp_Q0Q0_L[2][1][2] * ds[1][2] +
tmp_Q0Q0_L[2][1][3] * ds[1][3] +
tmp_Q0Q0_L[2][2][0] * ds[2][0] +
tmp_Q0Q0_L[2][2][1] * ds[2][1] +
tmp_Q0Q0_L[2][1][2] * ds[2][1] +
tmp_Q0Q0_L[2][1][3] * ds[3][1] +
tmp_Q0Q0_L[2][0][2] * ds[2][0] +
tmp_Q0Q0_L[2][1][2] * ds[2][1] +
tmp_Q0Q0_L[2][2][2] * ds[2][2] +
tmp_Q0Q0_L[2][2][3] * ds[2][3] +
tmp_Q0Q0_L[2][3][0] * ds[3][0] +
tmp_Q0Q0_L[2][3][1] * ds[3][1] +
tmp_Q0Q0_L[2][3][2] * ds[3][2] +
tmp_Q0Q0_L[2][2][3] * ds[3][2] +
tmp_Q0Q0_L[2][0][3] * ds[3][0] +
tmp_Q0Q0_L[2][1][3] * ds[3][1] +
tmp_Q0Q0_L[2][2][3] * ds[3][2] +
tmp_Q0Q0_L[2][3][3] * ds[3][3];

(*dq0_out)[3] = tmp_Q0Q0_L[3][0][0] * ds[0][0] +
tmp_Q0Q0_L[3][0][1] * ds[0][1] +
tmp_Q0Q0_L[3][0][2] * ds[0][2] +
tmp_Q0Q0_L[3][0][3] * ds[0][3] +
tmp_Q0Q0_L[3][1][0] * ds[1][0] +
tmp_Q0Q0_L[3][0][1] * ds[1][0] +
tmp_Q0Q0_L[3][0][2] * ds[2][0] +
tmp_Q0Q0_L[3][0][3] * ds[3][0] +
tmp_Q0Q0_L[3][0][1] * ds[1][0] +
tmp_Q0Q0_L[3][1][1] * ds[1][1] +
tmp_Q0Q0_L[3][1][2] * ds[1][2] +
tmp_Q0Q0_L[3][1][3] * ds[1][3] +
tmp_Q0Q0_L[3][2][0] * ds[2][0] +
tmp_Q0Q0_L[3][2][1] * ds[2][1] +
tmp_Q0Q0_L[3][1][2] * ds[2][1] +
tmp_Q0Q0_L[3][1][3] * ds[3][1] +
tmp_Q0Q0_L[3][0][2] * ds[2][0] +
tmp_Q0Q0_L[3][1][2] * ds[2][1] +
tmp_Q0Q0_L[3][2][2] * ds[2][2] +
tmp_Q0Q0_L[3][2][3] * ds[2][3] +
tmp_Q0Q0_L[3][3][0] * ds[3][0] +
tmp_Q0Q0_L[3][3][1] * ds[3][1] +
tmp_Q0Q0_L[3][3][2] * ds[3][2] +
tmp_Q0Q0_L[3][2][3] * ds[3][2] +
tmp_Q0Q0_L[3][0][3] * ds[3][0] +
tmp_Q0Q0_L[3][1][3] * ds[3][1] +
tmp_Q0Q0_L[3][2][3] * ds[3][2] +
tmp_Q0Q0_L[3][3][3] * ds[3][3];
}
}
Expand All @@ -449,19 +449,19 @@ struct rotation_derivative {
cvm::rvector ds_1[4][4];
ds_1[0][0].set( a2x, a2y, a2z);
ds_1[1][0].set( 0.0, a2z, -a2y);
ds_1[0][1] = ds_1[1][0];
// ds_1[0][1] = ds_1[1][0];
ds_1[2][0].set(-a2z, 0.0, a2x);
ds_1[0][2] = ds_1[2][0];
// ds_1[0][2] = ds_1[2][0];
ds_1[3][0].set( a2y, -a2x, 0.0);
ds_1[0][3] = ds_1[3][0];
// ds_1[0][3] = ds_1[3][0];
ds_1[1][1].set( a2x, -a2y, -a2z);
ds_1[2][1].set( a2y, a2x, 0.0);
ds_1[1][2] = ds_1[2][1];
// ds_1[1][2] = ds_1[2][1];
ds_1[3][1].set( a2z, 0.0, a2x);
ds_1[1][3] = ds_1[3][1];
// ds_1[1][3] = ds_1[3][1];
ds_1[2][2].set(-a2x, a2y, -a2z);
ds_1[3][2].set( 0.0, a2z, a2y);
ds_1[2][3] = ds_1[3][2];
// ds_1[2][3] = ds_1[3][2];
ds_1[3][3].set(-a2x, -a2y, a2z);
calc_derivative_impl(ds_1, dl0_1_out, dq0_1_out, ds_1_out);
}
Expand All @@ -486,19 +486,19 @@ struct rotation_derivative {
cvm::rvector ds_2[4][4];
ds_2[0][0].set( a1x, a1y, a1z);
ds_2[1][0].set( 0.0, -a1z, a1y);
ds_2[0][1] = ds_2[1][0];
// ds_2[0][1] = ds_2[1][0];
ds_2[2][0].set( a1z, 0.0, -a1x);
ds_2[0][2] = ds_2[2][0];
// ds_2[0][2] = ds_2[2][0];
ds_2[3][0].set(-a1y, a1x, 0.0);
ds_2[0][3] = ds_2[3][0];
// ds_2[0][3] = ds_2[3][0];
ds_2[1][1].set( a1x, -a1y, -a1z);
ds_2[2][1].set( a1y, a1x, 0.0);
ds_2[1][2] = ds_2[2][1];
// ds_2[1][2] = ds_2[2][1];
ds_2[3][1].set( a1z, 0.0, a1x);
ds_2[1][3] = ds_2[3][1];
// ds_2[1][3] = ds_2[3][1];
ds_2[2][2].set(-a1x, a1y, -a1z);
ds_2[3][2].set( 0.0, a1z, a1y);
ds_2[2][3] = ds_2[3][2];
// ds_2[2][3] = ds_2[3][2];
ds_2[3][3].set(-a1x, -a1y, a1z);
calc_derivative_impl(ds_2, dl0_2_out, dq0_2_out, ds_2_out);
}
Expand Down

0 comments on commit b1e317d

Please sign in to comment.