diff --git a/image_analysis/geom_refinement/XtalOptimizer.cpp b/image_analysis/geom_refinement/XtalOptimizer.cpp index 7bd37af4..b7cdb04d 100644 --- a/image_analysis/geom_refinement/XtalOptimizer.cpp +++ b/image_analysis/geom_refinement/XtalOptimizer.cpp @@ -355,17 +355,18 @@ void LatticeToRodriguesAndLengths_Hex(const CrystalLattice &latt, double rod[3], // Extract rotation (Rodrigues), lengths (a,b,c) and beta (rad) for monoclinic (unique axis b). // Frame choice: e2 aligned with b; e1 from a projected orthogonal to e2; e3 = e1 x e2. void LatticeToRodriguesLengthsBeta_Mono(const CrystalLattice &latt, - double rod[3], - double lengths[3], - double &beta_rad) { + double rod[3], + double lengths[3], + double &beta_rad) { const Coord a = latt.Vec0(); const Coord b = latt.Vec1(); const Coord c = latt.Vec2(); - Eigen::Vector3d A(a[0], a[1], a[2]); - Eigen::Vector3d Bv(b[0], b[1], b[2]); - Eigen::Vector3d C(c[0], c[1], c[2]); + const Eigen::Vector3d A(a[0], a[1], a[2]); + const Eigen::Vector3d Bv(b[0], b[1], b[2]); + const Eigen::Vector3d C(c[0], c[1], c[2]); + // Unit cell lengths const double a_len = A.norm(); const double b_len = Bv.norm(); const double c_len = C.norm(); @@ -374,41 +375,63 @@ void LatticeToRodriguesLengthsBeta_Mono(const CrystalLattice &latt, lengths[1] = b_len; lengths[2] = c_len; - // beta = angle between a and c + // Monoclinic beta = angle(a, c) double cos_beta = 0.0; - if (a_len > 0.0 && c_len > 0.0) - cos_beta = std::max(-1.0, std::min(1.0, A.dot(C) / (a_len * c_len))); + if (a_len > 1e-15 && c_len > 1e-15) { + cos_beta = A.dot(C) / (a_len * c_len); + cos_beta = std::clamp(cos_beta, -1.0, 1.0); + } + beta_rad = std::acos(cos_beta); - // Recover R from the same forward model used in refinement: - // L ≈ R * B(beta) * D(a,b,c) => R ≈ L * (B*D)^-1 + // Protect against singular construction + const double sin_beta = std::max(std::abs(std::sin(beta_rad)), 1e-12); + + // Canonical monoclinic basis: + // + // B = + // [ 1 0 cos(beta) ] + // [ 0 1 0 ] + // [ 0 0 sin(beta) ] + // + Eigen::Matrix3d Bmono = Eigen::Matrix3d::Zero(); + Bmono(0,0) = 1.0; + Bmono(1,1) = 1.0; + Bmono(0,2) = std::cos(beta_rad); + Bmono(2,2) = sin_beta; + + // Scale by lengths + Eigen::DiagonalMatrix D(a_len, b_len, c_len); + + // Ideal body-frame lattice + const Eigen::Matrix3d M = Bmono * D; + + // Observed lattice Eigen::Matrix3d L; L.col(0) = A; L.col(1) = Bv; L.col(2) = C; - Eigen::Matrix3d Bmono = Eigen::Matrix3d::Identity(); - Bmono(0, 2) = std::cos(beta_rad); - Bmono(2, 2) = std::sin(beta_rad); - - Eigen::DiagonalMatrix D(lengths[0], lengths[1], lengths[2]); - Eigen::Matrix3d M = Bmono * D; - - Eigen::Matrix3d R_est = Eigen::Matrix3d::Identity(); - if (std::abs(M.determinant()) > 1e-15) { - R_est = L * M.inverse(); - } + // Estimate rotation: + // R ≈ L * M^{-1} + Eigen::Matrix3d R_est = L * M.inverse(); + // Project to nearest proper rotation matrix Eigen::JacobiSVD svd(R_est, Eigen::ComputeFullU | Eigen::ComputeFullV); + Eigen::Matrix3d R = svd.matrixU() * svd.matrixV().transpose(); + + // Enforce det(R)=+1 if (R.determinant() < 0.0) { Eigen::Matrix3d U = svd.matrixU(); U.col(2) *= -1.0; R = U * svd.matrixV().transpose(); } + // Rodrigues vector Eigen::AngleAxisd aa(R); - Eigen::Vector3d r = aa.angle() * aa.axis(); + const Eigen::Vector3d r = aa.angle() * aa.axis(); + rod[0] = r.x(); rod[1] = r.y(); rod[2] = r.z();