8 #ifndef IMPALGEBRA_GEOMETRIC_ALIGNMENT_H
9 #define IMPALGEBRA_GEOMETRIC_ALIGNMENT_H
11 #include <IMP/algebra/algebra_config.h>
17 #include "internal/tnt_array2d.h"
18 #include "internal/jama_svd.h"
23 IMPALGEBRA_BEGIN_NAMESPACE
43 template <
class Vector3DsOrXYZs0,
44 class Vector3DsOrXYZs1>
47 const Vector3DsOrXYZs0 &source,
48 const Vector3DsOrXYZs1 &target
55 Vector3D center_source(0,0,0), center_target(0,0,0);
56 for (
unsigned int i=0; i< source.size(); ++i) {
63 center_source = center_source/source.size();
64 center_target = center_target/target.size();
66 IMP_LOG_VERBOSE(
"Centers are (" << center_source <<
") (" << center_target
68 Vector3Ds shifted_source(source.size()), shifted_target(target.size());
69 for (
unsigned int i=0; i< source.size(); ++i) {
75 internal::TNT::Array2D<double> H(3, 3);
76 for (
int i = 0; i < 3; i++) {
77 for (
int j = 0; j < 3; j++) {
81 for (
unsigned int i = 0; i < source.size(); i++) {
82 for (
int j = 0; j < 3; j++) {
83 for (
int k = 0; k < 3; k++) {
84 H[j][k] += shifted_source[i][j]*shifted_target[i][k];
91 internal::JAMA::SVD<double> svd(H);
92 internal::TNT::Array2D<double> U(3, 3), V(3, 3);
98 internal::TNT::Array1D<double> SV;
99 svd.getSingularValues(SV);
100 double det= SV[0]*SV[1]*SV[2];
104 for (
unsigned int i=0; i< source.size(); ++i) {
108 for (
unsigned int i=0; i< source.size(); ++i) {
112 IMP_WARN(
"Degenerate point set. I may not be able to align them."
118 internal::TNT::Array2D<double> Sigma(3,3, 0.0);
120 for (
int i=0; i < 3; ++i) {
125 << internal::TNT::matmult(internal::TNT::matmult(U,Sigma),
126 internal::TNT::transpose(V))
131 internal::TNT::Array2D<double> UT = internal::TNT::transpose(U);
132 internal::TNT::Array2D<double> rot(3, 3);
133 rot = matmult(V, UT);
136 if (determinant(rot) < 0) {
138 internal::TNT::Array2D<double> VT = internal::TNT::transpose(V);
139 internal::TNT::Array2D<double> UVT = internal::TNT::matmult(U, VT);
140 internal::TNT::Array2D<double> S(3, 3);
141 S[0][0] = S[1][1] = 1;
142 S[2][2] = determinant(UVT);
143 S[0][1] = S[0][2] = S[1][0] = S[1][2] = S[2][0] = S[2][1] = 0;
144 rot = internal::TNT::matmult(internal::TNT::matmult(U, S), VT);
151 rot[1][0], rot[1][1], rot[1][2],
152 rot[2][0], rot[2][1], rot[2][2]);
174 IMPALGEBRA_END_NAMESPACE