--- trunk/src/math/RMSD.cpp	2009/04/03 17:44:57	1335
+++ branches/development/src/math/RMSD.cpp	2010/07/09 23:08:25	1465
@@ -1,14 +1,13 @@
 #include "math/RMSD.hpp"
 #include "math/SVD.hpp"
 
-using namespace oopse;
+using namespace OpenMD;
 using namespace JAMA;
 
 RealType RMSD::calculate_rmsd(std::vector<Vector3d> mov,
                               Vector3d mov_com,
-                              Vector3d mov_to_ref,
-                              RotMat3x3d U) {
-
+                              Vector3d mov_to_ref) {
+  
   assert(mov.size() == ref_.size());
   int n;
   int n_vec = ref_.size();
@@ -31,7 +30,7 @@ RealType RMSD::calculate_rmsd(std::vector<Vector3d> mo
   }
   
   /* initialize */
-  Mat3x3d R = Mat3x3d(0.0);
+  Mat3x3d R(0.0);
   RealType E0 = 0.0;
   
   for (n=0; n < n_vec; n++) {
@@ -52,14 +51,26 @@ RealType RMSD::calculate_rmsd(std::vector<Vector3d> mo
   }
   E0 *= 0.5;
 
-  RectMatrix<RealType, n_vec, 3> v;
+  // Pack everything into Dynamic arrays for the SVD:
+  DynamicRectMatrix<RealType> Rtmp(3,3,0.0);
+  DynamicRectMatrix<RealType> vtmp(3, 3);
+  DynamicVector<RealType> stmp(3);
+  DynamicRectMatrix<RealType> wtmp(3, 3);
+
+  Rtmp.setSubMatrix(0, 0, R);
+  SVD<RealType> svd(Rtmp);
+
+  svd.getU(vtmp);
+  svd.getSingularValues(stmp);
+  svd.getV(wtmp);
+
+  Mat3x3d v;
   Vector3d s;
   Mat3x3d w;
 
-  SVD<RealType, n_vec, 3> svd = SVD<RealType, n_vec, 3>(R);
-  svd.getU(v);
-  svd.getSingularValues(s);
-  svd.getV(w);
+  vtmp.getSubMatrix(0,0,v);
+  stmp.getSubVector(0,s);
+  wtmp.getSubMatrix(0,0,w);
     
   int is_reflection = (v.determinant() * w.determinant()) < 0.0;
   if (is_reflection)
@@ -71,7 +82,72 @@ RealType RMSD::calculate_rmsd(std::vector<Vector3d> mo
   return rmsd;
 }
                             
+RotMat3x3d RMSD::optimal_superposition(std::vector<Vector3d> mov,
+				       Vector3d mov_com,
+				       Vector3d mov_to_ref) {
+    
+  assert(mov.size() == ref_.size());
+  int n;
+  int n_vec = ref_.size();
 
-RotMat3x3d RMSD::optimal_superposition(std::vector<Vector3d> mov) {
+  /* calculate the centre of mass */
+  mov_com = V3Zero;
+  
+  for (n=0; n < n_vec; n++) {
+    mov_com += mov[n];
+  }
+  
+  mov_com /= (RealType)n_vec;
+  
+  mov_to_ref = ref_com - mov_com;
+  
+  /* shift mov to center of mass */
+
+  for (n=0; n < n_vec; n++) {
+    mov[n] -= mov_com;
+  }
+  
+  /* initialize */
+  Mat3x3d R(0.0);
+  RealType E0 = 0.0;
+  
+  for (n=0; n < n_vec; n++) {
+    
+    /*
+     * correlation matrix R:   
+     *   R(i,j) = sum(over n): y(n,i) * x(n,j)  
+     *   where x(n) and y(n) are two vector sets   
+     */
+
+    R += outProduct(mov[n], ref_[n]);
+  }
+
+  // Pack everything into Dynamic arrays for the SVD:
+  DynamicRectMatrix<RealType> Rtmp(3,3,0.0);
+  DynamicRectMatrix<RealType> vtmp(3, 3);
+  DynamicVector<RealType> stmp(3);
+  DynamicRectMatrix<RealType> wtmp(3, 3);
+
+  Rtmp.setSubMatrix(0, 0, R);
+  SVD<RealType> svd(Rtmp);
+
+  svd.getU(vtmp);
+  svd.getSingularValues(stmp);
+  svd.getV(wtmp);
+
+  Mat3x3d v;
+  Vector3d s;
+  Mat3x3d w;
+
+  vtmp.getSubMatrix(0,0,v);
+  stmp.getSubVector(0,s);
+  wtmp.getSubMatrix(0,0,w);
+    
+  int is_reflection = (v.determinant() * w.determinant()) < 0.0;
+  if (is_reflection)
+    s(2) = -s(2);
+
+  RotMat3x3d U = v*w;
+  return U;
 }