Fixed error in constructing CCMA matrix

platforms/reference/src/SimTKReference/ReferenceCCMAAlgorithm.cpp

@@ -33,21 +33,19 @@
 #include "openmm/Vec3.h"
 #include "openmm/internal/ThreadPool.h"
 #include <map>
+#include <utility>
 
-using std::map;
-using std::pair;
-using std::vector;
-using std::set;
 using namespace OpenMM;
+using namespace std;
 
 // This class extracts columns from the inverse matrix one at a time.  It is done in parallel,
 // since this can be very slow.
 
 class ExtractMatrixTask : public ThreadPool::Task {
 public:
-    ExtractMatrixTask(int numConstraints, vector<vector<pair<int, RealOpenMM> > >& matrix, const vector<RealOpenMM>& distance, RealOpenMM elementCutoff,
+    ExtractMatrixTask(int numConstraints, vector<vector<pair<int, RealOpenMM> > >& transposedMatrix, const vector<RealOpenMM>& distance, RealOpenMM elementCutoff,
                       const int* qRowStart, const int* qColIndex, const int* rRowStart, const int* rColIndex, const double* qValue, const double* rValue) :
-                numConstraints(numConstraints), matrix(matrix), distance(distance), elementCutoff(elementCutoff), qRowStart(qRowStart), qColIndex(qColIndex),
+                numConstraints(numConstraints), transposedMatrix(transposedMatrix), distance(distance), elementCutoff(elementCutoff), qRowStart(qRowStart), qColIndex(qColIndex),
                 rRowStart(rRowStart), rColIndex(rColIndex), qValue(qValue), rValue(rValue) {
     }
 
@@ -61,15 +59,15 @@ public:
             QUERN_multiply_with_q_transpose(numConstraints, qRowStart, qColIndex, qValue, &rhs[0]);
             QUERN_solve_with_r(numConstraints, rRowStart, rColIndex, rValue, &rhs[0], &rhs[0]);
             for (int j = 0; j < numConstraints; j++) {
-                double value = rhs[j]*distance[j]/distance[i];
+                double value = rhs[j]*distance[i]/distance[j];
                 if (FABS((RealOpenMM) value) > elementCutoff)
-                    matrix[i].push_back(pair<int, RealOpenMM>(j, (RealOpenMM) value));
+                    transposedMatrix[i].push_back(pair<int, RealOpenMM>(j, (RealOpenMM) value));
             }
         }
     }
 private:
     int numConstraints;
-    vector<vector<pair<int, RealOpenMM> > >& matrix;
+    vector<vector<pair<int, RealOpenMM> > >& transposedMatrix;
     const vector<RealOpenMM>& distance;
     RealOpenMM elementCutoff;
     const int *qRowStart, *qColIndex, *rRowStart, *rColIndex;
@@ -194,12 +192,21 @@ ReferenceCCMAAlgorithm::ReferenceCCMAAlgorithm(int numberOfAtoms,
         double *qValue, *rValue;
         QUERN_compute_qr(numberOfConstraints, numberOfConstraints, &matrixRowStart[0], &matrixColIndex[0], &matrixValue[0], NULL,
                 &qRowStart, &qColIndex, &qValue, &rRowStart, &rColIndex, &rValue);
-        vector<double> rhs(numberOfConstraints);
+        vector<vector<pair<int, RealOpenMM> > > transposedMatrix(numberOfConstraints);
         _matrix.resize(numberOfConstraints);
         ThreadPool threads;
-        ExtractMatrixTask task(numberOfConstraints, _matrix, _distance, _elementCutoff, qRowStart, qColIndex, rRowStart, rColIndex, qValue, rValue);
+        ExtractMatrixTask task(numberOfConstraints, transposedMatrix, _distance, _elementCutoff, qRowStart, qColIndex, rRowStart, rColIndex, qValue, rValue);
         threads.execute(task);
         threads.waitForThreads();
+
+        // For purposes of thread safety we extracted the matrix in transposed form, so we need to transpose it again.
+
+        for (int i = 0; i < numberOfConstraints; i++) {
+            for (int j = 0; j < transposedMatrix[i].size(); j++) {
+                pair<int, RealOpenMM> value = transposedMatrix[i][j];
+                _matrix[value.first].push_back(make_pair(i, value.second));
+            }
+        }
         QUERN_free_result(qRowStart, qColIndex, qValue);
         QUERN_free_result(rRowStart, rColIndex, rValue);
     }