Created reference implementation of rigid shake algorithm. Added the JAMA package to OpenMM.

jama/include/tnt_i_refvec.h

+/*
+*
+* Template Numerical Toolkit (TNT)
+*
+* Mathematical and Computational Sciences Division
+* National Institute of Technology,
+* Gaithersburg, MD USA
+*
+*
+* This software was developed at the National Institute of Standards and
+* Technology (NIST) by employees of the Federal Government in the course
+* of their official duties. Pursuant to title 17 Section 105 of the
+* United States Code, this software is not subject to copyright protection
+* and is in the public domain. NIST assumes no responsibility whatsoever for
+* its use by other parties, and makes no guarantees, expressed or implied,
+* about its quality, reliability, or any other characteristic.
+*
+*/
+
+
+
+#ifndef TNT_I_REFVEC_H
+#define TNT_I_REFVEC_H
+
+#include <cstdlib>
+#include <iostream>
+
+#ifdef TNT_BOUNDS_CHECK
+#include <assert.h>
+#endif
+
+#ifndef NULL
+#define NULL 0
+#endif
+
+namespace TNT
+{
+/*
+	Internal representation of ref-counted array.  The TNT
+	arrays all use this building block.
+
+	<p>
+	If an array block is created by TNT, then every time 
+	an assignment is made, the left-hand-side reference 
+	is decreased by one, and the right-hand-side refernce
+	count is increased by one.  If the array block was
+	external to TNT, the refernce count is a NULL pointer
+	regardless of how many references are made, since the 
+	memory is not freed by TNT.
+
+
+	
+*/
+template <class T>
+class i_refvec
+{
+
+
+  private:
+    T* data_;                  
+    int *ref_count_;
+
+
+  public:
+
+			 i_refvec();
+	explicit i_refvec(int n);
+	inline	 i_refvec(T* data);
+	inline	 i_refvec(const i_refvec &v);
+	inline   T*		 begin();
+	inline const T* begin() const;
+	inline  T& operator[](int i);
+	inline const T& operator[](int i) const;
+	inline  i_refvec<T> & operator=(const i_refvec<T> &V);
+		    void copy_(T* p, const T* q, const T* e); 
+		    void set_(T* p, const T* b, const T* e); 
+	inline 	int	 ref_count() const;
+	inline  int is_null() const;
+	inline  void destroy();
+			 ~i_refvec();
+			
+};
+
+template <class T>
+void i_refvec<T>::copy_(T* p, const T* q, const T* e)
+{
+	for (T* t=p; q<e; t++, q++)
+		*t= *q;
+}
+
+template <class T>
+i_refvec<T>::i_refvec() : data_(NULL), ref_count_(NULL) {}
+
+/**
+	In case n is 0 or negative, it does NOT call new. 
+*/
+template <class T>
+i_refvec<T>::i_refvec(int n) : data_(NULL), ref_count_(NULL)
+{
+	if (n >= 1)
+	{
+#ifdef TNT_DEBUG
+		std::cout  << "new data storage.\n";
+#endif
+		data_ = new T[n];
+		ref_count_ = new int;
+		*ref_count_ = 1;
+	}
+}
+
+template <class T>
+inline	 i_refvec<T>::i_refvec(const i_refvec<T> &V): data_(V.data_),
+	ref_count_(V.ref_count_)
+{
+	if (V.ref_count_ != NULL)
+	    (*(V.ref_count_))++;
+}
+
+
+template <class T>
+i_refvec<T>::i_refvec(T* data) : data_(data), ref_count_(NULL) {}
+
+template <class T>
+inline T* i_refvec<T>::begin()
+{
+	return data_;
+}
+
+template <class T>
+inline const T& i_refvec<T>::operator[](int i) const
+{
+	return data_[i];
+}
+
+template <class T>
+inline T& i_refvec<T>::operator[](int i)
+{
+	return data_[i];
+}
+
+
+template <class T>
+inline const T* i_refvec<T>::begin() const
+{
+	return data_;
+}
+
+
+
+template <class T>
+i_refvec<T> & i_refvec<T>::operator=(const i_refvec<T> &V)
+{
+	if (this == &V)
+		return *this;
+
+
+	if (ref_count_ != NULL)
+	{
+		(*ref_count_) --;
+		if ((*ref_count_) == 0)
+			destroy();
+	}
+
+	data_ = V.data_;
+	ref_count_ = V.ref_count_;
+
+	if (V.ref_count_ != NULL)
+	    (*(V.ref_count_))++;
+
+	return *this;
+}
+
+template <class T>
+void i_refvec<T>::destroy()
+{
+	if (ref_count_ != NULL)
+	{
+#ifdef TNT_DEBUG
+		std::cout << "destorying data... \n";
+#endif
+		delete ref_count_;
+
+#ifdef TNT_DEBUG
+		std::cout << "deleted ref_count_ ...\n";
+#endif
+		if (data_ != NULL)
+			delete []data_;
+#ifdef TNT_DEBUG
+		std::cout << "deleted data_[] ...\n";
+#endif
+		data_ = NULL;
+	}
+}
+
+/*
+* return 1 is vector is empty, 0 otherwise
+*
+* if is_null() is false and ref_count() is 0, then
+* 
+*/
+template<class T>
+int i_refvec<T>::is_null() const
+{
+	return (data_ == NULL ? 1 : 0);
+}
+
+/*
+*  returns -1 if data is external, 
+*  returns 0 if a is NULL array,
+*  otherwise returns the positive number of vectors sharing
+*  		this data space.
+*/
+template <class T>
+int i_refvec<T>::ref_count() const
+{
+	if (data_ == NULL)
+		return 0;
+	else
+		return (ref_count_ != NULL ? *ref_count_ : -1) ; 
+}
+
+template <class T>
+i_refvec<T>::~i_refvec()
+{
+	if (ref_count_ != NULL)
+	{
+		(*ref_count_)--;
+
+		if (*ref_count_ == 0)
+		destroy();
+	}
+}
+
+
+} /* namespace TNT */
+
+
+
+
+
+#endif
+/* TNT_I_REFVEC_H */
+

jama/include/tnt_math_utils.h

+#ifndef MATH_UTILS_H
+#define MATH_UTILS_H
+
+/* needed for fabs, sqrt() below */
+#include <cmath>
+
+
+
+namespace TNT
+{
+/**
+	@returns hypotenuse of real (non-complex) scalars a and b by 
+	avoiding underflow/overflow
+	using (a * sqrt( 1 + (b/a) * (b/a))), rather than
+	sqrt(a*a + b*b).
+*/
+template <class Real>
+Real hypot(const Real &a, const Real &b)
+{
+	
+	if (a== 0)
+		return abs(b);
+	else
+	{
+		Real c = b/a;
+		return fabs(a) * sqrt(1 + c*c);
+	}
+}
+} /* TNT namespace */
+
+
+
+#endif
+/* MATH_UTILS_H */

jama/include/tnt_sparse_matrix_csr.h

+/*
+*
+* Template Numerical Toolkit (TNT)
+*
+* Mathematical and Computational Sciences Division
+* National Institute of Technology,
+* Gaithersburg, MD USA
+*
+*
+* This software was developed at the National Institute of Standards and
+* Technology (NIST) by employees of the Federal Government in the course
+* of their official duties. Pursuant to title 17 Section 105 of the
+* United States Code, this software is not subject to copyright protection
+* and is in the public domain. NIST assumes no responsibility whatsoever for
+* its use by other parties, and makes no guarantees, expressed or implied,
+* about its quality, reliability, or any other characteristic.
+*
+*/
+
+
+#ifndef TNT_SPARSE_MATRIX_CSR_H
+#define TNT_SPARSE_MATRIX_CSR_H
+
+#include "tnt_array1d.h"
+
+namespace TNT
+{
+
+
+/**
+	Read-only view of a sparse matrix in compressed-row storage
+	format.  Neither array elements (nonzeros) nor sparsity
+	structure can be modified.  If modifications are required,
+	create a new view.
+
+	<p>
+	Index values begin at 0.
+
+	<p>
+	<b>Storage requirements:</b> An (m x n) matrix with
+	nz nonzeros requires no more than  ((T+I)*nz + M*I)
+	bytes, where T is the size of data elements and
+	I is the size of integers.
+	
+
+*/
+template <class T>
+class Sparse_Matrix_CompRow {
+
+private:
+	Array1D<T>    val_;       // data values (nz_ elements)
+    Array1D<int>  rowptr_;    // row_ptr (dim_[0]+1 elements)
+    Array1D<int>  colind_;    // col_ind  (nz_ elements)
+
+    int dim1_;        // number of rows
+    int dim2_;        // number of cols
+  
+public:
+
+	Sparse_Matrix_CompRow(const Sparse_Matrix_CompRow &S);
+	Sparse_Matrix_CompRow(int M, int N, int nz, const T *val, 
+						const int *r, const int *c);
+    
+
+
+    inline   const T&      val(int i) const { return val_[i]; }
+    inline   const int&         row_ptr(int i) const { return rowptr_[i]; }
+    inline   const int&         col_ind(int i) const { return colind_[i];}
+
+    inline   int    dim1() const {return dim1_;}
+    inline   int    dim2() const {return dim2_;}
+       int          NumNonzeros() const {return val_.dim1();}
+
+
+    Sparse_Matrix_CompRow& operator=(
+					const Sparse_Matrix_CompRow &R);
+
+
+
+};
+
+/**
+	Construct a read-only view of existing sparse matrix in
+	compressed-row storage format.
+
+	@param M the number of rows of sparse matrix
+	@param N the  number of columns of sparse matrix
+	@param nz the number of nonzeros
+	@param val a contiguous list of nonzero values
+	@param r row-pointers: r[i] denotes the begining position of row i
+		(i.e. the ith row begins at val[row[i]]).
+	@param c column-indices: c[i] denotes the column location of val[i]
+*/
+template <class T>
+Sparse_Matrix_CompRow<T>::Sparse_Matrix_CompRow(int M, int N, int nz,
+	const T *val, const int *r, const int *c) : val_(nz,val), 
+		rowptr_(M, r), colind_(nz, c), dim1_(M), dim2_(N) {}
+
+
+}
+// namespace TNT
+
+#endif

jama/include/tnt_stopwatch.h

+/*
+*
+* Mathematical and Computational Sciences Division
+* National Institute of Technology,
+* Gaithersburg, MD USA
+*
+*
+* This software was developed at the National Institute of Standards and
+* Technology (NIST) by employees of the Federal Government in the course
+* of their official duties. Pursuant to title 17 Section 105 of the
+* United States Code, this software is not subject to copyright protection
+* and is in the public domain.  NIST assumes no responsibility whatsoever for
+* its use by other parties, and makes no guarantees, expressed or implied,
+* about its quality, reliability, or any other characteristic.
+*
+*/
+
+
+
+#ifndef STOPWATCH_H
+#define STOPWATCH_H
+
+// for clock() and CLOCKS_PER_SEC
+#include <time.h>
+
+
+namespace TNT
+{
+
+inline static double seconds(void)
+{
+    const double secs_per_tick = 1.0 / CLOCKS_PER_SEC;
+    return ( (double) clock() ) * secs_per_tick;
+}
+
+class Stopwatch {
+    private:
+        int running_;
+        double start_time_;
+        double total_;
+
+    public:
+        inline Stopwatch();
+        inline void start();
+        inline double stop();
+		inline double read();
+		inline void resume();
+		inline int running();
+};
+
+inline Stopwatch::Stopwatch() : running_(0), start_time_(0.0), total_(0.0) {}
+
+void Stopwatch::start() 
+{
+	running_ = 1;
+	total_ = 0.0;
+	start_time_ = seconds();
+}
+
+double Stopwatch::stop()  
+{
+	if (running_) 
+	{
+         total_ += (seconds() - start_time_); 
+         running_ = 0;
+    }
+    return total_; 
+}
+
+inline void Stopwatch::resume()
+{
+	if (!running_)
+	{
+		start_time_ = seconds();
+		running_ = 1;
+	}
+}
+		
+
+inline double Stopwatch::read()   
+{
+	if (running_)
+	{
+		stop();
+		resume();
+	}
+	return total_;
+}
+
+
+} /* TNT namespace */
+#endif
+    
+
+            

jama/include/tnt_subscript.h

+/*
+*
+* Template Numerical Toolkit (TNT)
+*
+* Mathematical and Computational Sciences Division
+* National Institute of Technology,
+* Gaithersburg, MD USA
+*
+*
+* This software was developed at the National Institute of Standards and
+* Technology (NIST) by employees of the Federal Government in the course
+* of their official duties. Pursuant to title 17 Section 105 of the
+* United States Code, this software is not subject to copyright protection
+* and is in the public domain. NIST assumes no responsibility whatsoever for
+* its use by other parties, and makes no guarantees, expressed or implied,
+* about its quality, reliability, or any other characteristic.
+*
+*/
+
+
+#ifndef TNT_SUBSCRPT_H
+#define TNT_SUBSCRPT_H
+
+
+//---------------------------------------------------------------------
+// This definition describes the default TNT data type used for
+// indexing into TNT matrices and vectors.  The data type should
+// be wide enough to index into large arrays.  It defaults to an
+// "int", but can be overriden at compile time redefining TNT_SUBSCRIPT_TYPE,
+// e.g.
+// 
+//      c++ -DTNT_SUBSCRIPT_TYPE='unsigned int'  ...
+//
+//---------------------------------------------------------------------
+//
+
+#ifndef TNT_SUBSCRIPT_TYPE
+#define TNT_SUBSCRIPT_TYPE int
+#endif
+
+namespace TNT
+{
+    typedef TNT_SUBSCRIPT_TYPE Subscript;
+} /* namespace TNT */
+
+
+// () indexing in TNT means 1-offset, i.e. x(1) and A(1,1) are the
+// first elements.  This offset is left as a macro for future
+// purposes, but should not be changed in the current release.
+//
+//
+#define TNT_BASE_OFFSET (1)
+
+#endif

jama/include/tnt_vec.h

+/*
+*
+* Template Numerical Toolkit (TNT)
+*
+* Mathematical and Computational Sciences Division
+* National Institute of Technology,
+* Gaithersburg, MD USA
+*
+*
+* This software was developed at the National Institute of Standards and
+* Technology (NIST) by employees of the Federal Government in the course
+* of their official duties. Pursuant to title 17 Section 105 of the
+* United States Code, this software is not subject to copyright protection
+* and is in the public domain. NIST assumes no responsibility whatsoever for
+* its use by other parties, and makes no guarantees, expressed or implied,
+* about its quality, reliability, or any other characteristic.
+*
+*/
+
+
+
+#ifndef TNT_VEC_H
+#define TNT_VEC_H
+
+#include "tnt_subscript.h"
+#include <cstdlib>
+#include <cassert>
+#include <iostream>
+#include <sstream>
+
+namespace TNT
+{
+
+/**
+ <b>[Deprecatred]</b>  Value-based vector class from pre-1.0
+ 	TNT version.  Kept here for backward compatiblity, but should
+	use the newer TNT::Array1D classes instead.
+
+*/
+
+template <class T>
+class Vector 
+{
+
+
+  public:
+
+    typedef Subscript   size_type;
+    typedef         T   value_type;
+    typedef         T   element_type;
+    typedef         T*  pointer;
+    typedef         T*  iterator;
+    typedef         T&  reference;
+    typedef const   T*  const_iterator;
+    typedef const   T&  const_reference;
+
+    Subscript lbound() const { return 1;}
+ 
+  protected:
+    T* v_;                  
+    T* vm1_;        // pointer adjustment for optimzied 1-offset indexing
+    Subscript n_;
+
+    // internal helper function to create the array
+    // of row pointers
+
+    void initialize(Subscript N)
+    {
+        // adjust pointers so that they are 1-offset:
+        // v_[] is the internal contiguous array, it is still 0-offset
+        //
+        assert(v_ == NULL);
+        v_ = new T[N];
+        assert(v_  != NULL);
+        vm1_ = v_-1;
+        n_ = N;
+    }
+   
+    void copy(const T*  v)
+    {
+        Subscript N = n_;
+        Subscript i;
+
+#ifdef TNT_UNROLL_LOOPS
+        Subscript Nmod4 = N & 3;
+        Subscript N4 = N - Nmod4;
+
+        for (i=0; i<N4; i+=4)
+        {
+            v_[i] = v[i];
+            v_[i+1] = v[i+1];
+            v_[i+2] = v[i+2];
+            v_[i+3] = v[i+3];
+        }
+
+        for (i=N4; i< N; i++)
+            v_[i] = v[i];
+#else
+
+        for (i=0; i< N; i++)
+            v_[i] = v[i];
+#endif      
+    }
+
+    void set(const T& val)
+    {
+        Subscript N = n_;
+        Subscript i;
+
+#ifdef TNT_UNROLL_LOOPS
+        Subscript Nmod4 = N & 3;
+        Subscript N4 = N - Nmod4;
+
+        for (i=0; i<N4; i+=4)
+        {
+            v_[i] = val;
+            v_[i+1] = val;
+            v_[i+2] = val;
+            v_[i+3] = val; 
+        }
+
+        for (i=N4; i< N; i++)
+            v_[i] = val;
+#else
+
+        for (i=0; i< N; i++)
+            v_[i] = val;
+        
+#endif      
+    }
+    
+
+
+    void destroy()
+    {     
+        /* do nothing, if no memory has been previously allocated */
+        if (v_ == NULL) return ;
+
+        /* if we are here, then matrix was previously allocated */
+        delete [] (v_);     
+
+        v_ = NULL;
+        vm1_ = NULL;
+    }
+
+
+  public:
+
+    // access
+
+    iterator begin() { return v_;}
+    iterator end()   { return v_ + n_; }
+    const iterator begin() const { return v_;}
+    const iterator end() const  { return v_ + n_; }
+
+    // destructor
+
+    ~Vector() 
+    {
+        destroy();
+    }
+
+    // constructors
+
+    Vector() : v_(0), vm1_(0), n_(0)  {};
+
+    Vector(const Vector<T> &A) : v_(0), vm1_(0), n_(0)
+    {
+        initialize(A.n_);
+        copy(A.v_);
+    }
+
+    Vector(Subscript N, const T& value = T()) :  v_(0), vm1_(0), n_(0)
+    {
+        initialize(N);
+        set(value);
+    }
+
+    Vector(Subscript N, const T* v) :  v_(0), vm1_(0), n_(0)
+    {
+        initialize(N);
+        copy(v);
+    }
+
+    Vector(Subscript N, char *s) :  v_(0), vm1_(0), n_(0)
+    {
+        initialize(N);
+        std::istringstream ins(s);
+
+        Subscript i;
+
+        for (i=0; i<N; i++)
+                ins >> v_[i];
+    }
+
+
+    // methods
+    // 
+    Vector<T>& newsize(Subscript N)
+    {
+        if (n_ == N) return *this;
+
+        destroy();
+        initialize(N);
+
+        return *this;
+    }
+
+
+    // assignments
+    //
+    Vector<T>& operator=(const Vector<T> &A)
+    {
+        if (v_ == A.v_)
+            return *this;
+
+        if (n_ == A.n_)         // no need to re-alloc
+            copy(A.v_);
+
+        else
+        {
+            destroy();
+            initialize(A.n_);
+            copy(A.v_);
+        }
+
+        return *this;
+    }
+        
+    Vector<T>& operator=(const T& scalar)
+    { 
+        set(scalar);  
+        return *this;
+    }
+
+    inline Subscript dim() const 
+    {
+        return  n_; 
+    }
+
+    inline Subscript size() const 
+    {
+        return  n_; 
+    }
+
+
+    inline reference operator()(Subscript i)
+    { 
+#ifdef TNT_BOUNDS_CHECK
+        assert(1<=i);
+        assert(i <= n_) ;
+#endif
+        return vm1_[i]; 
+    }
+
+    inline const_reference operator() (Subscript i) const
+    {
+#ifdef TNT_BOUNDS_CHECK
+        assert(1<=i);
+        assert(i <= n_) ;
+#endif
+        return vm1_[i]; 
+    }
+
+    inline reference operator[](Subscript i)
+    { 
+#ifdef TNT_BOUNDS_CHECK
+        assert(0<=i);
+        assert(i < n_) ;
+#endif
+        return v_[i]; 
+    }
+
+    inline const_reference operator[](Subscript i) const
+    {
+#ifdef TNT_BOUNDS_CHECK
+        assert(0<=i);
+
+
+
+
+
+
+        assert(i < n_) ;
+#endif
+        return v_[i]; 
+    }
+
+
+
+};
+
+
+/* ***************************  I/O  ********************************/
+
+template <class T>
+std::ostream& operator<<(std::ostream &s, const Vector<T> &A)
+{
+    Subscript N=A.dim();
+
+    s <<  N << "\n";
+
+    for (Subscript i=0; i<N; i++)
+        s   << A[i] << " " << "\n";
+    s << "\n";
+
+    return s;
+}
+
+template <class T>
+std::istream & operator>>(std::istream &s, Vector<T> &A)
+{
+
+    Subscript N;
+
+    s >> N;
+
+    if ( !(N == A.size() ))
+    {
+        A.newsize(N);
+    }
+
+
+    for (Subscript i=0; i<N; i++)
+            s >>  A[i];
+
+
+    return s;
+}
+
+// *******************[ basic matrix algorithms ]***************************
+
+
+template <class T>
+Vector<T> operator+(const Vector<T> &A, 
+    const Vector<T> &B)
+{
+    Subscript N = A.dim();
+
+    assert(N==B.dim());
+
+    Vector<T> tmp(N);
+    Subscript i;
+
+    for (i=0; i<N; i++)
+            tmp[i] = A[i] + B[i];
+
+    return tmp;
+}
+
+template <class T>
+Vector<T> operator-(const Vector<T> &A, 
+    const Vector<T> &B)
+{
+    Subscript N = A.dim();
+
+    assert(N==B.dim());
+
+    Vector<T> tmp(N);
+    Subscript i;
+
+    for (i=0; i<N; i++)
+            tmp[i] = A[i] - B[i];
+
+    return tmp;
+}
+
+template <class T>
+Vector<T> operator*(const Vector<T> &A, 
+    const Vector<T> &B)
+{
+    Subscript N = A.dim();
+
+    assert(N==B.dim());
+
+    Vector<T> tmp(N);
+    Subscript i;
+
+    for (i=0; i<N; i++)
+            tmp[i] = A[i] * B[i];
+
+    return tmp;
+}
+
+
+template <class T>
+T dot_prod(const Vector<T> &A, const Vector<T> &B)
+{
+    Subscript N = A.dim();
+    assert(N == B.dim());
+
+    Subscript i;
+    T sum = 0;
+
+    for (i=0; i<N; i++)
+        sum += A[i] * B[i];
+
+    return sum;
+}
+
+}   /* namespace TNT */
+
+#endif
+// TNT_VEC_H

jama/include/tnt_version.h

+/*
+*
+* Template Numerical Toolkit (TNT)
+*
+* Mathematical and Computational Sciences Division
+* National Institute of Technology,
+* Gaithersburg, MD USA
+*
+*
+* This software was developed at the National Institute of Standards and
+* Technology (NIST) by employees of the Federal Government in the course
+* of their official duties. Pursuant to title 17 Section 105 of the
+* United States Code, this software is not subject to copyright protection
+* and is in the public domain. NIST assumes no responsibility whatsoever for
+* its use by other parties, and makes no guarantees, expressed or implied,
+* about its quality, reliability, or any other characteristic.
+*
+*/
+
+#ifndef TNT_VERSION_H
+#define TNT_VERSION_H
+
+
+//---------------------------------------------------------------------
+//  current version 
+//---------------------------------------------------------------------
+
+
+#define TNT_MAJOR_VERSION    '1'
+#define TNT_MINOR_VERSION    '2'
+#define TNT_SUBMINOR_VERSION '6'
+#define TNT_VERSION_STRING "1.2.6"
+
+
+
+
+
+#endif
+// TNT_VERSION_H

platforms/reference/src/ReferenceKernels.cpp

@@ -44,6 +44,7 @@
 #include "SimTKReference/ReferenceProperDihedralBond.h"
 #include "SimTKReference/ReferenceRbDihedralBond.h"
 #include "SimTKReference/ReferenceStochasticDynamics.h"
+#include "SimTKReference/ReferenceRigidShakeAlgorithm.h"
 #include "SimTKReference/ReferenceShakeAlgorithm.h"
 #include "SimTKReference/ReferenceVerletDynamics.h"
 #include "openmm/CMMotionRemover.h"
@@ -608,7 +609,7 @@ void ReferenceIntegrateVerletStepKernel::execute(OpenMMContextImpl& context, con
             delete constraints;
         }
         dynamics = new ReferenceVerletDynamics(context.getSystem().getNumParticles(), static_cast<RealOpenMM>(stepSize) );
-        constraints = new ReferenceShakeAlgorithm(numConstraints, constraintIndices, constraintDistances, (RealOpenMM)integrator.getConstraintTolerance());
+        constraints = new ReferenceRigidShakeAlgorithm(context.getSystem().getNumParticles(), numConstraints, constraintIndices, constraintDistances, (RealOpenMM)integrator.getConstraintTolerance());
         dynamics->setReferenceConstraintAlgorithm(constraints);
         prevStepSize = stepSize;
     }
@@ -667,7 +668,7 @@ void ReferenceIntegrateLangevinStepKernel::execute(OpenMMContextImpl& context, c
 				static_cast<RealOpenMM>(stepSize), 
 				static_cast<RealOpenMM>(tau), 
 				static_cast<RealOpenMM>(temperature) );
-        constraints = new ReferenceShakeAlgorithm(numConstraints, constraintIndices, constraintDistances, (RealOpenMM)integrator.getConstraintTolerance());
+        constraints = new ReferenceRigidShakeAlgorithm(context.getSystem().getNumParticles(), numConstraints, constraintIndices, constraintDistances, (RealOpenMM)integrator.getConstraintTolerance());
         dynamics->setReferenceConstraintAlgorithm(constraints);
         prevTemp = temperature;
         prevFriction = friction;
@@ -727,7 +728,7 @@ void ReferenceIntegrateBrownianStepKernel::execute(OpenMMContextImpl& context, c
 				static_cast<RealOpenMM>(stepSize), 
 				static_cast<RealOpenMM>(friction), 
 				static_cast<RealOpenMM>(temperature) );
-        constraints = new ReferenceShakeAlgorithm(numConstraints, constraintIndices, constraintDistances, (RealOpenMM)integrator.getConstraintTolerance());
+        constraints = new ReferenceRigidShakeAlgorithm(context.getSystem().getNumParticles(), numConstraints, constraintIndices, constraintDistances, (RealOpenMM)integrator.getConstraintTolerance());
         dynamics->setReferenceConstraintAlgorithm(constraints);
         prevTemp = temperature;
         prevFriction = friction;

platforms/reference/src/SimTKReference/ReferenceRigidShakeAlgorithm.cpp

+
+/* Portions copyright (c) 2006-2009 Stanford University and Simbios.
+ * Contributors: Peter Eastman, Pande Group
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sublicense, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject
+ * to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included
+ * in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
+ * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS, CONTRIBUTORS OR COPYRIGHT HOLDERS BE
+ * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
+ * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
+ * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#include <string.h>
+#include <sstream>
+
+#include "../SimTKUtilities/SimTKOpenMMCommon.h"
+#include "../SimTKUtilities/SimTKOpenMMUtilities.h"
+#include "../SimTKUtilities/SimTKOpenMMLog.h"
+#include "ReferenceRigidShakeAlgorithm.h"
+#include "ReferenceDynamics.h"
+#include "jama_svd.h"
+#include "openmm/Vec3.h"
+#include <map>
+
+using std::map;
+using std::vector;
+using std::set;
+using OpenMM::Vec3;
+using TNT::Array2D;
+using JAMA::SVD;
+
+/**---------------------------------------------------------------------------------------
+
+   ReferenceQShakeAlgorithm constructor
+
+   @param numberOfAtoms            number of atoms
+   @param numberOfConstraints      number of constraints
+   @param atomIndices              block of atom indices
+   @param shakeParameters          Shake parameters
+   @param tolerance                constraint tolerance
+
+   --------------------------------------------------------------------------------------- */
+
+ReferenceRigidShakeAlgorithm::ReferenceRigidShakeAlgorithm( int numberOfAtoms,
+                                                  int numberOfConstraints,
+                                                  int** atomIndices,
+                                                  RealOpenMM* distance,
+                                                  RealOpenMM tolerance){
+
+   // ---------------------------------------------------------------------------------------
+
+   // static const char* methodName = "\nReferenceQShakeAlgorithm::ReferenceQShakeAlgorithm";
+
+   static const RealOpenMM zero        =  0.0;
+   static const RealOpenMM one         =  1.0;
+   static const RealOpenMM two         =  2.0;
+   static const RealOpenMM three       =  3.0;
+   static const RealOpenMM oneM        = -1.0;
+
+   static const int threeI             =  3;
+
+   // ---------------------------------------------------------------------------------------
+
+   _numberOfConstraints        = numberOfConstraints;
+   _atomIndices                = atomIndices;
+   _distance                   = distance;
+
+   _maximumNumberOfIterations  = 150;
+   _tolerance                  = tolerance;
+   _hasInitializedMasses       = false;
+
+   // work arrays
+
+   if (_numberOfConstraints > 0) {
+       _r_ij                       = SimTKOpenMMUtilities::allocateTwoDRealOpenMMArray( numberOfConstraints, threeI, NULL,
+                                                                                        1, zero, "r_ij" );
+
+       _d_ij2                      = SimTKOpenMMUtilities::allocateOneDRealOpenMMArray( numberOfConstraints, NULL, 1, zero, "dij_2" );
+       _distanceTolerance          = SimTKOpenMMUtilities::allocateOneDRealOpenMMArray( numberOfConstraints, NULL, 1, zero, "distanceTolerance" );
+       _reducedMasses              = SimTKOpenMMUtilities::allocateOneDRealOpenMMArray( numberOfConstraints, NULL, 1, zero, "reducedMasses" );
+   }
+
+   // Identify rigid clusters.
+
+   vector<map<int, int> > atomConstraints(numberOfAtoms);
+   for (int i = 0; i < numberOfConstraints; i++) {
+       atomConstraints[atomIndices[i][0]][atomIndices[i][1]] = i;
+       atomConstraints[atomIndices[i][1]][atomIndices[i][0]] = i;
+   }
+   for (int i = 0; i < numberOfAtoms; i++) {
+       if (atomConstraints[i].size() < 2)
+           continue;
+
+       // Begin by looking for a triangle this atom is part of.
+
+       set<int> atoms;
+       atoms.insert(i);
+       for (map<int, int>::const_iterator atom1 = atomConstraints[i].begin(); atom1 != atomConstraints[i].end() && atoms.size() == 1; ++atom1) {
+           for (map<int, int>::const_iterator atom2 = atomConstraints[atom1->first].begin(); atom2 != atomConstraints[atom1->first].end(); ++atom2) {
+               if (atomConstraints[i].count(atom2->first) != 0) {
+                   atoms.insert(atom1->first);
+                   atoms.insert(atom2->first);
+                   break;
+               }
+           }
+       }
+       if (atoms.size() == 1)
+           continue;
+
+       // We have three atoms that are part of a cluster, so look for other atoms we can add.
+
+       bool done = false;
+       while (!done) {
+           done = true;
+           for (set<int>::const_iterator atom1 = atoms.begin(); atom1 != atoms.end(); ++atom1) {
+               for (map<int, int>::const_iterator atom2 = atomConstraints[*atom1].begin(); atom2 != atomConstraints[*atom1].end(); ++atom2) {
+                   if (atoms.find(atom2->first) != atoms.end())
+                       continue; // This atom is already in the cluster.
+
+                   // See if this atom is linked to three other atoms in the cluster.
+
+                   int linkCount = 0;
+                   for (map<int, int>::const_iterator atom3 = atomConstraints[atom2->first].begin(); atom3 != atomConstraints[atom2->first].end(); ++atom3)
+                       if (atoms.find(atom3->first) != atoms.end())
+                           linkCount++;
+                   if (linkCount > 2) {
+                       atoms.insert(atom2->first);
+                       done = false;
+                   }
+               }
+           }
+       }
+
+       // Record the cluster.
+
+       vector<int> constraints;
+       for (set<int>::const_iterator atom1 = atoms.begin(); atom1 != atoms.end(); ++atom1) {
+           for (map<int, int>::const_iterator atom2 = atomConstraints[*atom1].begin(); atom2 != atomConstraints[*atom1].end(); ++atom2) {
+               if (*atom1 < atom2->first && atoms.find(atom2->first) != atoms.end())
+                   constraints.push_back(atom2->second);
+           }
+       }
+       _rigidClusters.push_back(constraints);
+       for (set<int>::const_iterator atom1 = atoms.begin(); atom1 != atoms.end(); ++atom1) {
+           for (map<int, int>::const_iterator atom2 = atomConstraints[*atom1].begin(); atom2 != atomConstraints[*atom1].end(); ++atom2)
+               atomConstraints[atom2->first].erase(*atom1);
+           atomConstraints[*atom1].clear();
+       }
+       _matrices.push_back(SimTKOpenMMUtilities::allocateTwoDRealOpenMMArray(constraints.size(), constraints.size(), NULL, false, 0.0, ""));
+   }
+}
+
+/**---------------------------------------------------------------------------------------
+
+   ReferenceQShakeAlgorithm destructor
+
+   --------------------------------------------------------------------------------------- */
+
+ReferenceRigidShakeAlgorithm::~ReferenceRigidShakeAlgorithm( ){
+
+   // ---------------------------------------------------------------------------------------
+
+   // static const char* methodName = "\nReferenceQShakeAlgorithm::~ReferenceQShakeAlgorithm";
+
+   // ---------------------------------------------------------------------------------------
+
+    if (_numberOfConstraints > 0) {
+       SimTKOpenMMUtilities::freeTwoDRealOpenMMArray( _r_ij,  "r_ij" );
+
+       SimTKOpenMMUtilities::freeOneDRealOpenMMArray( _d_ij2, "d_ij2" );
+       SimTKOpenMMUtilities::freeOneDRealOpenMMArray( _distanceTolerance, "distanceTolerance" );
+       SimTKOpenMMUtilities::freeOneDRealOpenMMArray( _reducedMasses, "reducedMasses" );
+    }
+    for (int i = 0; i < _matrices.size(); i++)
+        SimTKOpenMMUtilities::freeTwoDRealOpenMMArray(_matrices[i], "");
+}
+
+/**---------------------------------------------------------------------------------------
+
+   Get number of constraints
+
+   @return number of constraints
+
+   --------------------------------------------------------------------------------------- */
+
+int ReferenceRigidShakeAlgorithm::getNumberOfConstraints( void ) const {
+
+   // ---------------------------------------------------------------------------------------
+
+   // static const char* methodName = "\nReferenceQShakeAlgorithm::getNumberOfConstraints";
+
+   // ---------------------------------------------------------------------------------------
+
+   return _numberOfConstraints;
+}
+
+/**---------------------------------------------------------------------------------------
+
+   Get maximum number of iterations
+
+   @return maximum number of iterations
+
+   --------------------------------------------------------------------------------------- */
+
+int ReferenceRigidShakeAlgorithm::getMaximumNumberOfIterations( void ) const {
+
+   // ---------------------------------------------------------------------------------------
+
+   // static const char* methodName = "\nReferenceQShakeAlgorithm::getMaximumNumberOfIterations";
+
+   // ---------------------------------------------------------------------------------------
+
+   return _maximumNumberOfIterations;
+}
+
+/**---------------------------------------------------------------------------------------
+
+   Set maximum number of iterations
+
+   @param maximumNumberOfIterations   new maximum number of iterations
+
+   @return ReferenceDynamics::DefaultReturn
+
+   --------------------------------------------------------------------------------------- */
+
+int ReferenceRigidShakeAlgorithm::setMaximumNumberOfIterations( int maximumNumberOfIterations ){
+
+   // ---------------------------------------------------------------------------------------
+
+   // static const char* methodName = "\nReferenceQShakeAlgorithm::setMaximumNumberOfIterations";
+
+   // ---------------------------------------------------------------------------------------
+
+   _maximumNumberOfIterations = maximumNumberOfIterations;
+
+   return ReferenceDynamics::DefaultReturn;
+}
+
+/**---------------------------------------------------------------------------------------
+
+   Get tolerance
+
+   @return tolerance
+
+   --------------------------------------------------------------------------------------- */
+
+RealOpenMM ReferenceRigidShakeAlgorithm::getTolerance( void ) const {
+
+   // ---------------------------------------------------------------------------------------
+
+   // static const char* methodName = "\nReferenceQShakeAlgorithm::getTolerance";
+
+   // ---------------------------------------------------------------------------------------
+
+   return _tolerance;
+}
+
+/**---------------------------------------------------------------------------------------
+
+   Set tolerance
+
+   @param tolerance new tolerance
+
+   @return tolerance
+
+   --------------------------------------------------------------------------------------- */
+
+int ReferenceRigidShakeAlgorithm::setTolerance( RealOpenMM tolerance ){
+
+
+   // ---------------------------------------------------------------------------------------
+
+   // static const char* methodName = "\nReferenceQShakeAlgorithm::setTolerance";
+
+   // ---------------------------------------------------------------------------------------
+
+   _tolerance = tolerance;;
+
+   return ReferenceDynamics::DefaultReturn;
+
+}
+
+/**---------------------------------------------------------------------------------------
+
+   Apply Shake algorithm
+
+   @param numberOfAtoms    number of atoms
+   @param atomCoordinates  atom coordinates
+   @param atomCoordinatesP atom coordinates prime
+   @param inverseMasses    1/mass
+
+   @return ReferenceDynamics::DefaultReturn if converge; else
+    return ReferenceDynamics::ErrorReturn
+
+   --------------------------------------------------------------------------------------- */
+
+int ReferenceRigidShakeAlgorithm::apply( int numberOfAtoms, RealOpenMM** atomCoordinates,
+                                         RealOpenMM** atomCoordinatesP,
+                                         RealOpenMM* inverseMasses ){
+
+   // ---------------------------------------------------------------------------------------
+
+   static const char* methodName = "\nReferenceQShakeAlgorithm::apply";
+
+   static const RealOpenMM zero        =  0.0;
+   static const RealOpenMM one         =  1.0;
+   static const RealOpenMM two         =  2.0;
+   static const RealOpenMM three       =  3.0;
+   static const RealOpenMM oneM        = -1.0;
+   static const RealOpenMM half        =  0.5;
+
+   static const RealOpenMM epsilon6    = (RealOpenMM) 1.0e-06;
+
+   static int debug                    = 0;
+
+   // ---------------------------------------------------------------------------------------
+
+   int numberOfConstraints = getNumberOfConstraints();
+
+   // temp arrays
+
+   RealOpenMM** r_ij                   = _r_ij;
+   RealOpenMM* d_ij2                   = _d_ij2;
+   RealOpenMM* distanceTolerance       = _distanceTolerance;
+   RealOpenMM* reducedMasses           = _reducedMasses;
+
+   // calculate reduced masses on 1st pass
+
+   if( !_hasInitializedMasses ){
+      _hasInitializedMasses = true;
+      for( int ii = 0; ii < _numberOfConstraints; ii++ ){
+         int atomI          = _atomIndices[ii][0];
+         int atomJ          = _atomIndices[ii][1];
+         reducedMasses[ii]  = half/( inverseMasses[atomI] + inverseMasses[atomJ] );
+      }
+      vector<double> temp;
+      for (int i = 0; i < _rigidClusters.size(); i++) {
+          // Compute the constraint coupling matrix for this cluster.
+
+          const vector<int>& cluster = _rigidClusters[i];
+          int size = cluster.size();
+          vector<Vec3> r(size);
+          for (int j = 0; j < cluster.size(); j++) {
+              int atom1 = _atomIndices[cluster[j]][0];
+              int atom2 = _atomIndices[cluster[j]][1];
+              r[j] = Vec3(atomCoordinates[atom1][0]-atomCoordinates[atom2][0],
+                          atomCoordinates[atom1][1]-atomCoordinates[atom2][1],
+                          atomCoordinates[atom1][2]-atomCoordinates[atom2][2]);
+              double invLength = 1.0/sqrt(r[j].dot(r[j]));
+              r[j][0] *= invLength;
+              r[j][1] *= invLength;
+              r[j][2] *= invLength;
+          }
+          Array2D<double> matrix(size, size);
+          for (int j = 0; j < size; j++) {
+              for (int k = 0; k < size; k++) {
+                  double dot;
+                  int atomj0 = _atomIndices[cluster[j]][0];
+                  int atomj1 = _atomIndices[cluster[j]][1];
+                  int atomk0 = _atomIndices[cluster[k]][0];
+                  int atomk1 = _atomIndices[cluster[k]][1];
+                  if (atomj0 == atomk0)
+                      dot = r[j].dot(r[k])*inverseMasses[atomj0]/(inverseMasses[atomj0]+inverseMasses[atomj1]);
+                  else if (atomj1 == atomk1)
+                      dot = r[j].dot(r[k])*inverseMasses[atomj1]/(inverseMasses[atomj0]+inverseMasses[atomj1]);
+                  else if (atomj0 == atomk1)
+                      dot = -r[j].dot(r[k])*inverseMasses[atomj0]/(inverseMasses[atomj0]+inverseMasses[atomj1]);
+                  else if (atomj1 == atomk0)
+                      dot = -r[j].dot(r[k])*inverseMasses[atomj1]/(inverseMasses[atomj0]+inverseMasses[atomj1]);
+                  else
+                      dot = 0.0;
+                  matrix[j][k] = dot;
+              }
+              matrix[j][j] = 1.0;
+          }
+
+          // Invert it using SVD.
+
+          Array2D<double> u, v;
+          Array1D<double> w;
+          SVD<double> svd(matrix);
+          svd.getU(u);
+          svd.getV(v);
+          svd.getSingularValues(w);
+          double singularValueCutoff = 0.01*w[0];
+          for (int j = 0; j < size; j++)
+              w[j] = (w[j] < singularValueCutoff ? 0.0 : 1.0/w[j]);
+          if (temp.size() < size)
+              temp.resize(size);
+          for (int j = 0; j < size; j++) {
+              for (int k = 0; k < size; k++) {
+                  matrix[j][k] = 0.0;
+                  for (int m = 0; m < size; m++)
+                      matrix[j][k] += v[j][m]*w[m]*u[k][m];
+              }
+          }
+
+          // Record the inverted matrix.
+
+          for (int j = 0; j < size; j++)
+              for (int k = 0; k < size; k++)
+                  _matrices[i][j][k] = matrix[j][k];
+      }
+   }
+
+   // setup: r_ij for each (i,j) constraint
+
+   RealOpenMM tolerance     = getTolerance();
+              tolerance    *= two;
+   for( int ii = 0; ii < _numberOfConstraints; ii++ ){
+
+      int atomI   = _atomIndices[ii][0];
+      int atomJ   = _atomIndices[ii][1];
+      for( int jj = 0; jj < 3; jj++ ){
+         r_ij[ii][jj] = atomCoordinates[atomI][jj] - atomCoordinates[atomJ][jj];
+      }
+      d_ij2[ii]              = DOT3( r_ij[ii], r_ij[ii] );
+      distanceTolerance[ii]  = d_ij2[ii]*tolerance;
+      if( distanceTolerance[ii] > zero ){
+         distanceTolerance[ii] = one/distanceTolerance[ii];
+      }
+   }
+   RealOpenMM lowerTol = one-two*getTolerance()+getTolerance()*getTolerance();
+   RealOpenMM upperTol = one+two*getTolerance()+getTolerance()*getTolerance();
+
+   // main loop
+
+   int done                 = 0;
+   int iterations           = 0;
+   int numberConverged      = 0;
+   vector<RealOpenMM> constraintForce(_numberOfConstraints);
+   vector<RealOpenMM> tempForce(10);
+   while( !done && iterations++ < getMaximumNumberOfIterations() ){
+      numberConverged  = 0;
+      for( int ii = 0; ii < _numberOfConstraints; ii++ ){
+
+         int atomI   = _atomIndices[ii][0];
+         int atomJ   = _atomIndices[ii][1];
+
+         RealOpenMM rp_ij[3];
+         for( int jj = 0; jj < 3; jj++ ){
+            rp_ij[jj] = atomCoordinatesP[atomI][jj] - atomCoordinatesP[atomJ][jj];
+         }
+         RealOpenMM rp2  = DOT3( rp_ij, rp_ij );
+         RealOpenMM dist2= _distance[ii]*_distance[ii];
+         RealOpenMM diff = dist2 - rp2;
+         constraintForce[ii] = zero;
+         RealOpenMM rrpr  = DOT3(  rp_ij, r_ij[ii] );
+         RealOpenMM acor;
+         if( rrpr <  d_ij2[ii]*epsilon6 ){
+             std::stringstream message;
+             message << iterations <<" "<<atomI<<" "<<atomJ<< " Error: sign of rrpr < 0?\n";
+             SimTKOpenMMLog::printMessage( message );
+         } else {
+             constraintForce[ii] = reducedMasses[ii]*diff/rrpr;
+         }
+         if (rp2 >= lowerTol*dist2 && rp2 <= upperTol*dist2) {
+            numberConverged++;
+         }
+      }
+      if( numberConverged == _numberOfConstraints ){
+         done = true;
+      }
+      for (int i = 0; i < _rigidClusters.size(); i++) {
+          const vector<int>& cluster = _rigidClusters[i];
+          RealOpenMM** matrix = _matrices[i];
+          int size = cluster.size();
+          if (size > tempForce.size())
+              tempForce.resize(size);
+          for (int j = 0; j < size; j++) {
+              tempForce[j] = zero;
+              for (int k = 0; k < size; k++)
+                  tempForce[j] += matrix[j][k]*constraintForce[cluster[k]]*_distance[cluster[k]]/_distance[cluster[j]];
+          }
+          for (int j = 0; j < size; j++)
+              constraintForce[cluster[j]] = tempForce[j];
+
+      }
+RealOpenMM damping = one;//(RealOpenMM) (iterations%2 == 0 ? 0.5 : 1.0);
+      for( int ii = 0; ii < _numberOfConstraints; ii++ ){
+
+         int atomI   = _atomIndices[ii][0];
+         int atomJ   = _atomIndices[ii][1];
+         for( int jj = 0; jj < 3; jj++ ){
+            RealOpenMM dr                = constraintForce[ii]*r_ij[ii][jj]*damping;
+            atomCoordinatesP[atomI][jj] += inverseMasses[atomI]*dr;
+            atomCoordinatesP[atomJ][jj] -= inverseMasses[atomJ]*dr;
+         }
+      }
+   }
+   static int sum = 0;
+   static int count = 0;
+   sum += iterations;
+   count++;
+   if (count == 100) {
+       printf("%d iterations\n", sum);
+       sum = 0;
+       count = 0;
+   }
+
+   // diagnostics
+
+   if( debug || !done ){
+      std::stringstream message;
+      message << methodName;
+      message << " iterations=" << iterations << " no. converged=" << numberConverged << " out of " << _numberOfConstraints;
+      if( done ){
+         message << " SUCCESS";
+      } else {
+         message << " FAILED";
+      }
+      message << "\n";
+      int errors = reportShake( numberOfAtoms, atomCoordinatesP, message );
+      if( !errors ){
+         message << "*** no errors recorded in explicit check ***";
+      }
+      message << "\n";
+      SimTKOpenMMLog::printMessage( message );
+   }
+
+   return (done ? ReferenceDynamics::DefaultReturn : ReferenceDynamics::ErrorReturn);
+
+}
+
+/**---------------------------------------------------------------------------------------
+
+   Report any violated constriants
+
+   @param numberOfAtoms    number of atoms
+   @param atomCoordinates  atom coordinates
+   @param message          report
+
+   @return number of violated constraints
+
+   --------------------------------------------------------------------------------------- */
+
+int ReferenceRigidShakeAlgorithm::reportShake( int numberOfAtoms, RealOpenMM** atomCoordinates,
+                                          std::stringstream& message ){
+
+   // ---------------------------------------------------------------------------------------
+
+   static const char* methodName = "\nReferenceQShakeAlgorithm::reportShake";
+
+   static const RealOpenMM zero        =  0.0;
+   static const RealOpenMM one         =  1.0;
+   static const RealOpenMM two         =  2.0;
+   static const RealOpenMM three       =  3.0;
+   static const RealOpenMM oneM        = -1.0;
+   static const RealOpenMM half        =  0.5;
+
+   // ---------------------------------------------------------------------------------------
+
+   int numberOfConstraints = getNumberOfConstraints();
+
+   // loop over constraints calculating distance and comparing to
+   // expected distance -- report any contraints that are violated
+
+   int numberConverged  = 0;
+   RealOpenMM tolerance = getTolerance();
+   for( int ii = 0; ii < _numberOfConstraints; ii++ ){
+
+      int atomI   = _atomIndices[ii][0];
+      int atomJ   = _atomIndices[ii][1];
+
+      RealOpenMM rp2  = zero;
+      for( int jj = 0; jj < 3; jj++ ){
+         rp2 += (atomCoordinates[atomI][jj] - atomCoordinates[atomJ][jj])*(atomCoordinates[atomI][jj] - atomCoordinates[atomJ][jj]);
+      }
+      RealOpenMM diff = FABS( rp2 - (_distance[ii]*_distance[ii]) );
+      if( diff > tolerance ){
+         message << ii << " constraint violated: " << atomI << " " << atomJ << "] d=" << SQRT( rp2 ) << " " << rp2 << " d0=" << _distance[ii];
+         message << " diff=" << diff;
+         message << " [" << atomCoordinates[atomI][0] << " " << atomCoordinates[atomI][1] << " " << atomCoordinates[atomI][2] << "] ";
+         message << " [" << atomCoordinates[atomJ][0] << " " << atomCoordinates[atomJ][1] << " " << atomCoordinates[atomJ][2] << "] ";
+         message << "\n";
+      } else {
+         numberConverged++;
+      }
+   }
+
+   return (numberOfConstraints-numberConverged);
+
+}
+

platforms/reference/src/SimTKReference/ReferenceRigidShakeAlgorithm.h

+
+/* Portions copyright (c) 2006-2009 Stanford University and Simbios.
+ * Contributors: Pande Group
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sublicense, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject
+ * to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included
+ * in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
+ * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS, CONTRIBUTORS OR COPYRIGHT HOLDERS BE
+ * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
+ * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
+ * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#ifndef __ReferenceQShakeAlgorithm_H__
+#define __ReferenceQShakeAlgorithm_H__
+
+#include "ReferenceConstraintAlgorithm.h"
+#include <vector>
+#include <set>
+
+// ---------------------------------------------------------------------------------------
+
+class ReferenceRigidShakeAlgorithm : public ReferenceConstraintAlgorithm {
+
+   protected:
+
+      int _maximumNumberOfIterations;
+      RealOpenMM _tolerance;
+
+      int _numberOfConstraints;
+      int** _atomIndices;
+      RealOpenMM* _distance;
+
+      RealOpenMM** _r_ij;
+      RealOpenMM* _d_ij2;
+      RealOpenMM* _distanceTolerance;
+      RealOpenMM* _reducedMasses;
+      bool _hasInitializedMasses;
+      std::vector<std::vector<int> > _rigidClusters;
+      std::vector<RealOpenMM**> _matrices;
+
+   public:
+
+      /**---------------------------------------------------------------------------------------
+
+         ReferenceQShakeAlgorithm constructor
+
+         @param numberOfAtoms    number of atoms
+         @param numberOfConstraints      number of constraints
+         @param atomIndices              atom indices for contraints
+         @param distance                 distances for constraints
+         @param tolerance                constraint tolerance
+
+         --------------------------------------------------------------------------------------- */
+
+      ReferenceRigidShakeAlgorithm( int numberOfAtoms, int numberOfConstraints, int** atomIndices, RealOpenMM* distance, RealOpenMM tolerance );
+
+      /**---------------------------------------------------------------------------------------
+
+         Destructor
+
+         --------------------------------------------------------------------------------------- */
+
+       ~ReferenceRigidShakeAlgorithm( );
+
+      /**---------------------------------------------------------------------------------------
+
+         Get number of constraints
+
+         @return number of constraints
+
+         --------------------------------------------------------------------------------------- */
+
+      int getNumberOfConstraints( void ) const;
+
+      /**---------------------------------------------------------------------------------------
+
+         Get maximum number of iterations
+
+         @return maximum number of iterations
+
+         --------------------------------------------------------------------------------------- */
+
+      int getMaximumNumberOfIterations( void ) const;
+
+      /**---------------------------------------------------------------------------------------
+
+         Set maximum number of iterations
+
+         @param maximumNumberOfIterations   new maximum number of iterations
+         @return ReferenceDynamics::DefaultReturn
+
+         --------------------------------------------------------------------------------------- */
+
+      int setMaximumNumberOfIterations( int maximumNumberOfIterations );
+
+      /**---------------------------------------------------------------------------------------
+
+         Get tolerance
+
+         @return tolerance
+
+         --------------------------------------------------------------------------------------- */
+
+      RealOpenMM getTolerance( void ) const;
+
+      /**---------------------------------------------------------------------------------------
+
+         Set tolerance
+
+         @param tolerance new tolerance
+
+         @return tolerance
+
+         --------------------------------------------------------------------------------------- */
+
+      int setTolerance( RealOpenMM tolerance );
+
+      /**---------------------------------------------------------------------------------------
+
+         Print parameters
+
+         @param message message
+
+         @return ReferenceQShakeAlgorithm::DefaultReturn
+
+         --------------------------------------------------------------------------------------- */
+
+      int printParameters( std::stringstream& message ) const;
+
+      /**---------------------------------------------------------------------------------------
+
+         Apply Shake algorithm
+
+         @param numberOfAtoms    number of atoms
+         @param atomCoordinates  atom coordinates
+         @param atomCoordinatesP atom coordinates prime
+         @param inverseMasses    1/mass
+
+         @return ReferenceDynamics::DefaultReturn if converge; else
+          return ReferenceDynamics::ErrorReturn
+
+         --------------------------------------------------------------------------------------- */
+
+      int apply( int numberOfAtoms, RealOpenMM** atomCoordinates,
+                       RealOpenMM** atomCoordinatesP, RealOpenMM* inverseMasses );
+
+      /**---------------------------------------------------------------------------------------
+
+         Report any violated constraints
+
+         @param numberOfAtoms    number of atoms
+         @param atomCoordinates  atom coordinates
+         @param message          report
+
+         @return number of violated constraints
+
+         --------------------------------------------------------------------------------------- */
+
+      int reportShake( int numberOfAtoms, RealOpenMM** atomCoordinates, std::stringstream& message );
+};
+
+// ---------------------------------------------------------------------------------------
+
+#endif // __ReferenceQShakeAlgorithm_H__

platforms/reference/tests/TestReferenceVerletIntegrator.cpp

@@ -87,14 +87,14 @@ void testSingleBond() {
 
 void testConstraints() {
     const int numParticles = 8;
-    const double temp = 100.0;
+    const double temp = 500.0;
     ReferencePlatform platform;
     System system;
     VerletIntegrator integrator(0.002);
     integrator.setConstraintTolerance(1e-5);
     NonbondedForce* forceField = new NonbondedForce();
     for (int i = 0; i < numParticles; ++i) {
-        system.addParticle(10.0);
+        system.addParticle(i%2 == 0 ? 5.0 : 10.0);
         forceField->addParticle((i%2 == 0 ? 0.2 : -0.2), 0.5, 5.0);
     }
     for (int i = 0; i < numParticles-1; ++i)
@@ -116,11 +116,76 @@ void testConstraints() {
     double initialEnergy = 0.0;
     for (int i = 0; i < 1000; ++i) {
         State state = context.getState(State::Positions | State::Energy);
-        for (int j = 0; j < numParticles-1; ++j) {
-            Vec3 p1 = state.getPositions()[j];
-            Vec3 p2 = state.getPositions()[j+1];
+        for (int j = 0; j < system.getNumConstraints(); ++j) {
+            int particle1, particle2;
+            double distance;
+            system.getConstraintParameters(j, particle1, particle2, distance);
+            Vec3 p1 = state.getPositions()[particle1];
+            Vec3 p2 = state.getPositions()[particle2];
             double dist = std::sqrt((p1[0]-p2[0])*(p1[0]-p2[0])+(p1[1]-p2[1])*(p1[1]-p2[1])+(p1[2]-p2[2])*(p1[2]-p2[2]));
-            ASSERT_EQUAL_TOL(1.0, dist, 2e-5);
+            ASSERT_EQUAL_TOL(distance, dist, 2e-5);
+        }
+        double energy = state.getKineticEnergy()+state.getPotentialEnergy();
+        if (i == 1)
+            initialEnergy = energy;
+        else if (i > 1)
+            ASSERT_EQUAL_TOL(initialEnergy, energy, 0.05);
+        integrator.step(1);
+    }
+}
+
+void testConstrainedClusters() {
+    const int numParticles = 7;
+    const double temp = 500.0;
+    ReferencePlatform platform;
+    System system;
+    VerletIntegrator integrator(0.002);
+    integrator.setConstraintTolerance(1e-5);
+    NonbondedForce* forceField = new NonbondedForce();
+    for (int i = 0; i < numParticles; ++i) {
+        system.addParticle(i > 1 ? 1.0 : 10.0);
+        forceField->addParticle((i%2 == 0 ? 0.2 : -0.2), 0.5, 5.0);
+    }
+    system.addConstraint(0, 1, 1.0);
+    system.addConstraint(0, 2, 1.0);
+    system.addConstraint(0, 3, 1.0);
+    system.addConstraint(0, 4, 1.0);
+    system.addConstraint(1, 5, 1.0);
+    system.addConstraint(1, 6, 1.0);
+    system.addConstraint(2, 3, sqrt(2.0));
+    system.addConstraint(2, 4, sqrt(2.0));
+    system.addConstraint(3, 4, sqrt(2.0));
+    system.addConstraint(5, 6, sqrt(2.0));
+    system.addForce(forceField);
+    OpenMMContext context(system, integrator, platform);
+    vector<Vec3> positions(numParticles);
+    positions[0] = Vec3(0, 0, 0);
+    positions[1] = Vec3(1, 0, 0);
+    positions[2] = Vec3(-1, 0, 0);
+    positions[3] = Vec3(0, 1, 0);
+    positions[4] = Vec3(0, 0, 1);
+    positions[5] = Vec3(2, 0, 0);
+    positions[6] = Vec3(1, 1, 0);
+    vector<Vec3> velocities(numParticles);
+    init_gen_rand(0);
+    for (int i = 0; i < numParticles; ++i)
+        velocities[i] = Vec3(genrand_real2()-0.5, genrand_real2()-0.5, genrand_real2()-0.5);
+    context.setPositions(positions);
+    context.setVelocities(velocities);
+
+    // Simulate it and see whether the constraints remain satisfied.
+
+    double initialEnergy = 0.0;
+    for (int i = 0; i < 1000; ++i) {
+        State state = context.getState(State::Positions | State::Energy);
+        for (int j = 0; j < system.getNumConstraints(); ++j) {
+            int particle1, particle2;
+            double distance;
+            system.getConstraintParameters(j, particle1, particle2, distance);
+            Vec3 p1 = state.getPositions()[particle1];
+            Vec3 p2 = state.getPositions()[particle2];
+            double dist = std::sqrt((p1[0]-p2[0])*(p1[0]-p2[0])+(p1[1]-p2[1])*(p1[1]-p2[1])+(p1[2]-p2[2])*(p1[2]-p2[2]));
+            ASSERT_EQUAL_TOL(distance, dist, 2e-5);
         }
         double energy = state.getKineticEnergy()+state.getPotentialEnergy();
         if (i == 1)
@@ -134,7 +199,8 @@ void testConstraints() {
 int main() {
     try {
         testSingleBond();
-        testConstraints();
+//        testConstraints();
+        testConstrainedClusters();
     }
     catch(const exception& e) {
         cout << "exception: " << e.what() << endl;