INT angle_calc( DOUBLE* F, int pos, DOUBLE* _E )
{
DOUBLE phi_0 = angle_phis[ 2*pos ];
DOUBLE k_phi = angle_phis[ 2*pos + 1 ];
DOUBLE* F1 = F + DIMS0 * angle_pairs[ 3 * pos ];
DOUBLE* F2 = F + DIMS0 * angle_pairs[ 3 * pos + 1 ];
DOUBLE* F3 = F + DIMS0 * angle_pairs[ 3 * pos + 2 ];
DOUBLE Fi[3];
DOUBLE Fk[3];
DOUBLE e1[3];
DOUBLE e2[3];
DOUBLE cos_phi;
DOUBLE phi;
get_v3_norms( angle_pairs[ 3 * pos ], angle_pairs[ 3 * pos + 1 ], angle_pairs[ 3 * pos + 2 ], e1, e2 );
cos_phi = cos_vs( e1, e2 );
if(cos_phi>=1-EPSILON || cos_phi<=-1+EPSILON) return 0;
phi = ACOSD( cos_phi );
k_phi *= ( phi - phi_0 );
if(_E) _E[0] += 0.5f * k_phi * ( phi - phi_0 );
k_phi /= SQRTD( 1 - cos_phi*cos_phi );
Fi[0] = -k_phi * (e2[0] - e1[0] * cos_phi);
Fi[1] = -k_phi * (e2[1] - e1[1] * cos_phi);
Fi[2] = -k_phi * (e2[2] - e1[2] * cos_phi);
Fk[0] = -k_phi * (e1[0] - e2[0] * cos_phi);
Fk[1] = -k_phi * (e1[1] - e2[1] * cos_phi);
Fk[2] = -k_phi * (e1[2] - e2[2] * cos_phi);
F1[0] += Fi[0];
F1[1] += Fi[1];
F1[2] += Fi[2];
F2[0] += -Fi[0]-Fk[0];
F2[1] += -Fi[1]-Fk[1];
F2[2] += -Fi[2]-Fk[2];
F3[0] += Fk[0];
F3[1] += Fk[1];
F3[2] += Fk[2];
return isnan(Fk[0]) || isinf(Fk[1]) ||
isnan(Fk[1]) || isinf(Fk[1]) ||
isnan(Fk[2]) || isinf(Fk[2]) ||
isnan(Fi[0]) || isinf(Fi[0]) ||
isnan(Fi[1]) || isinf(Fi[1]) ||
isnan(Fi[2]) || isinf(Fi[2]) ||
( _E ? (isnan( _E[0] ) || isinf( _E[0] )) : 0);
}
DOUBLE check_tilt_pairs(DOUBLE rot1, DOUBLE tilt1, DOUBLE psi1,
DOUBLE &alpha, DOUBLE &tilt_angle, DOUBLE &beta)
{
// Transformation matrices
Matrix1D<DOUBLE> axis(3);
Matrix2D<DOUBLE> E1, E2;
axis.resize(3);
DOUBLE aux, sine_tilt_angle;
DOUBLE rot2 = alpha, tilt2 = tilt_angle, psi2 = beta;
// Calculate the transformation from one setting to the second one.
Euler_angles2matrix(psi1, tilt1, rot1, E1);
Euler_angles2matrix(psi2, tilt2, rot2, E2);
E2 = E2 * E1.inv();
// Get the tilt angle (and its sine)
aux = ( E2(0,0) + E2(1,1) + E2(2,2) - 1. ) / 2.;
if (ABS(aux) - 1. > XMIPP_EQUAL_ACCURACY)
REPORT_ERROR("BUG: aux>1");
tilt_angle = ACOSD(aux);
sine_tilt_angle = 2. * SIND(tilt_angle);
// Get the tilt axis direction in angles alpha and beta
if (sine_tilt_angle > XMIPP_EQUAL_ACCURACY)
{
axis(0) = ( E2(2,1) - E2(1,2) ) / sine_tilt_angle;
axis(1) = ( E2(0,2) - E2(2,0) ) / sine_tilt_angle;
axis(2) = ( E2(1,0) - E2(0,1) ) / sine_tilt_angle;
}
else
{
axis(0) = axis(1) = 0.;
axis(2) = 1.;
}
// Apply E1.inv() to the axis to get everyone in the same coordinate system again
axis = E1.inv() * axis;
// Convert to alpha and beta angle
Euler_direction2angles(axis, alpha, beta);
// Enforce positive beta: choose the other Euler angle combination to express the same direction
if (beta < 0.)
{
beta = -beta;
alpha+= 180.;
}
// Let alpha go from 0 to 360 degrees
alpha = realWRAP(alpha, 0., 360.);
// Return the value that needs to be optimized
DOUBLE minimizer=0.;
if (exp_beta < 999.)
minimizer = ABS(beta - exp_beta);
if (exp_tilt < 999.)
minimizer += ABS(tilt_angle - exp_tilt);
return minimizer;
}
