Finished reference implementation of GayBerneForce

openmmapi/include/openmm/GayBerneForce.h

@@ -44,20 +44,19 @@ namespace OpenMM {
 /**
  * This class implements the Gay-Berne anisotropic potential.  This is similar to a Lennard-Jones potential,
  * but it represents the particles as ellipsoids rather than point particles.  In addition to the standard
- * sigma and epsilon parameters, each particle has three radii r_x, r_y, and r_z that give the size of the
- * ellipsoid along each axis.  It also has three scale factors e_x, e_y, and e_z that scale the strength
+ * sigma and epsilon parameters, each particle has three widths sx, sy, and sz that give the diameter of the
+ * ellipsoid along each axis.  It also has three scale factors ex, ey, and ez that scale the strength
  * of the interaction along each axis.  You can think of this force as a Lennard-Jones interaction computed
- * based on the distance between the nearest points on two ellipsoids.  The interpretation of the scale
- * factors is slightly complicated, but roughly speaking, the strength of the interaction along the ellipsoid's
- * x axis is divided by e_x^2, and likewise for the other axes.  If two particles each have all their
- * radii set to sigma/2 and all their scale factors set 1, the interaction simplifies to a standard Lennard-Jones
- * force between point particles.
+ * based on the distance between the nearest points on two ellipsoids.  The scale factors act as multipliers
+ * for epsilon along each axis, so the strength of the interaction along the ellipsoid's x axis is multiplied by
+ * ex, and likewise for the other axes.  If two particles each have all their widths set to sigma and all their
+ * scale factors set to 1, the interaction simplifies to a standard Lennard-Jones force between point particles.
  *
  * The orientation of a particle's ellipsoid is determined based on the positions of two other particles.
  * The vector to the first particle sets the direction of the x axis.  The vector to the second particle
  * (after subtracting out any x component) sets the direction of the y axis.  If the ellipsoid is axially
- * symmetric (r_y=r_z and e_y=e_z), you can omit the second particle and define only an x axis direction.
- * If the ellipsoid is a sphere (all three radii and all three scale factors are equal), both particles
+ * symmetric (sy=sz and ey=ez), you can omit the second particle and define only an x axis direction.
+ * If the ellipsoid is a sphere (all three widths and all three scale factors are equal), both particles
  * can be omitted.
  *
  * To determine the values of sigma and epsilon for an interaction, this class uses Lorentz-Berthelot
@@ -166,15 +165,15 @@ public:
      * @param epsilon   the epsilon parameter (corresponding to the well depth of the van der Waals interaction), measured in kJ/mol
      * @param xparticle the index of the particle whose position defines the ellipsoid's x axis, or -1 if the ellipsoid is a sphere
      * @param yparticle the index of the particle whose position defines the ellipsoid's y axis, or -1 if the ellipsoid is axially symmetric
-     * @param rx        the radius of the ellipsoid along its x axis
-     * @param ry        the radius of the ellipsoid along its y axis
-     * @param rz        the radius of the ellipsoid along its z axis
+     * @param sx        the diameter of the ellipsoid along its x axis
+     * @param sy        the diameter of the ellipsoid along its y axis
+     * @param sz        the diameter of the ellipsoid along its z axis
      * @param ex        the factor by which epsilon is scaled along the ellipsoid's x axis
      * @param ey        the factor by which epsilon is scaled along the ellipsoid's y axis
      * @param ez        the factor by which epsilon is scaled along the ellipsoid's z axis
      * @return the index of the particle that was added
      */
-    int addParticle(double sigma, double epsilon, int xparticle, int yparticle, double rx, double ry, double rz, double ex, double ey, double ez);
+    int addParticle(double sigma, double epsilon, int xparticle, int yparticle, double sx, double sy, double sz, double ex, double ey, double ez);
     /**
      * Get the parameters for a particle.
      *
@@ -183,14 +182,14 @@ public:
      * @param[out] epsilon   the epsilon parameter (corresponding to the well depth of the van der Waals interaction), measured in kJ/mol
      * @param[out] xparticle the index of the particle whose position defines the ellipsoid's x axis, or -1 if the ellipsoid is a sphere
      * @param[out] yparticle the index of the particle whose position defines the ellipsoid's y axis, or -1 if the ellipsoid is axially symmetric
-     * @param[out] rx        the radius of the ellipsoid along its x axis
-     * @param[out] ry        the radius of the ellipsoid along its y axis
-     * @param[out] rz        the radius of the ellipsoid along its z axis
+     * @param[out] sx        the diameter of the ellipsoid along its x axis
+     * @param[out] sy        the diameter of the ellipsoid along its y axis
+     * @param[out] sz        the diameter of the ellipsoid along its z axis
      * @param[out] ex        the factor by which epsilon is scaled along the ellipsoid's x axis
      * @param[out] ey        the factor by which epsilon is scaled along the ellipsoid's y axis
      * @param[out] ez        the factor by which epsilon is scaled along the ellipsoid's z axis
      */
-    void getParticleParameters(int index, double& sigma, double& epsilon, int& xparticle, int& yparticle, double& rx, double& ry, double& rz, double& ex, double& ey, double& ez) const;
+    void getParticleParameters(int index, double& sigma, double& epsilon, int& xparticle, int& yparticle, double& sx, double& sy, double& sz, double& ex, double& ey, double& ez) const;
     /**
      * Set the parameters for a particle.
      *
@@ -199,14 +198,14 @@ public:
      * @param epsilon   the epsilon parameter (corresponding to the well depth of the van der Waals interaction), measured in kJ/mol
      * @param xparticle the index of the particle whose position defines the ellipsoid's x axis, or -1 if the ellipsoid is a sphere
      * @param yparticle the index of the particle whose position defines the ellipsoid's y axis, or -1 if the ellipsoid is axially symmetric
-     * @param rx        the radius of the ellipsoid along its x axis
-     * @param ry        the radius of the ellipsoid along its y axis
-     * @param rz        the radius of the ellipsoid along its z axis
+     * @param sx        the diameter of the ellipsoid along its x axis
+     * @param sy        the diameter of the ellipsoid along its y axis
+     * @param sz        the diameter of the ellipsoid along its z axis
      * @param ex        the factor by which epsilon is scaled along the ellipsoid's x axis
      * @param ey        the factor by which epsilon is scaled along the ellipsoid's y axis
      * @param ez        the factor by which epsilon is scaled along the ellipsoid's z axis
      */
-    void setParticleParameters(int index, double sigma, double epsilon, int xparticle, int yparticle, double rx, double ry, double rz, double ex, double ey, double ez);
+    void setParticleParameters(int index, double sigma, double epsilon, int xparticle, int yparticle, double sx, double sy, double sz, double ex, double ey, double ez);
     /**
      * Add an interaction to the list of exceptions that should be calculated differently from other interactions.  If
      * epsilon is equal to 0, this will cause the interaction to be completely omitted from force and energy calculations.
@@ -284,13 +283,13 @@ private:
 class GayBerneForce::ParticleInfo {
 public:
     int xparticle, yparticle;
-    double sigma, epsilon, rx, ry, rz, ex, ey, ez;
+    double sigma, epsilon, sx, sy, sz, ex, ey, ez;
     ParticleInfo() {
         xparticle = yparticle = -1;
-        sigma = epsilon = rx = ry = rz = ex = ey = ez = 0.0;
+        sigma = epsilon = sx = sy = sz = ex = ey = ez = 0.0;
     }
-    ParticleInfo(double sigma, double epsilon, int xparticle, int yparticle, double rx, double ry, double rz, double ex, double ey, double ez) :
-        sigma(sigma), epsilon(epsilon), xparticle(xparticle), yparticle(yparticle), rx(rx), ry(ry), rz(rz), ex(ex), ey(ey), ez(ez) {
+    ParticleInfo(double sigma, double epsilon, int xparticle, int yparticle, double sx, double sy, double sz, double ex, double ey, double ez) :
+        sigma(sigma), epsilon(epsilon), xparticle(xparticle), yparticle(yparticle), sx(sx), sy(sy), sz(sz), ex(ex), ey(ey), ez(ez) {
     }
 };
 

openmmapi/src/GayBerneForce.cpp

@@ -82,36 +82,36 @@ void GayBerneForce::setSwitchingDistance(double distance) {
     switchingDistance = distance;
 }
 
-int GayBerneForce::addParticle(double sigma, double epsilon, int xparticle, int yparticle, double rx, double ry, double rz, double ex, double ey, double ez) {
-    if (yparticle == -1 && (ry != rz || ey != ez))
+int GayBerneForce::addParticle(double sigma, double epsilon, int xparticle, int yparticle, double sx, double sy, double sz, double ex, double ey, double ez) {
+    if (yparticle == -1 && (sy != sz || ey != ez))
         throw OpenMMException("GayBerneForce: yparticle is -1 for a particle that is not axially symmetric");
-    if (xparticle == -1 && (rx != rz || ex != ez))
+    if (xparticle == -1 && (sx != sz || ex != ez))
         throw OpenMMException("GayBerneForce: xparticle is -1 for a particle that is not spherical");
     if (xparticle == -1 && yparticle != -1)
         throw OpenMMException("GayBerneForce: xparticle cannot be -1 if yparticle is not also -1");
-    particles.push_back(ParticleInfo(sigma, epsilon, xparticle, yparticle, rx, ry, rz, ex, ey, ez));
+    particles.push_back(ParticleInfo(sigma, epsilon, xparticle, yparticle, sx, sy, sz, ex, ey, ez));
     return particles.size()-1;
 }
 
-void GayBerneForce::getParticleParameters(int index, double& sigma, double& epsilon, int& xparticle, int& yparticle, double& rx, double& ry, double& rz, double& ex, double& ey, double& ez) const {
+void GayBerneForce::getParticleParameters(int index, double& sigma, double& epsilon, int& xparticle, int& yparticle, double& sx, double& sy, double& sz, double& ex, double& ey, double& ez) const {
     ASSERT_VALID_INDEX(index, particles);
     sigma = particles[index].sigma;
     epsilon = particles[index].epsilon;
     xparticle = particles[index].xparticle;
     yparticle = particles[index].yparticle;
-    rx = particles[index].rx;
-    ry = particles[index].ry;
-    rz = particles[index].rz;
+    sx = particles[index].sx;
+    sy = particles[index].sy;
+    sz = particles[index].sz;
     ex = particles[index].ex;
     ey = particles[index].ey;
     ez = particles[index].ez;
 }
 
-void GayBerneForce::setParticleParameters(int index, double sigma, double epsilon, int xparticle, int yparticle, double rx, double ry, double rz, double ex, double ey, double ez) {
+void GayBerneForce::setParticleParameters(int index, double sigma, double epsilon, int xparticle, int yparticle, double sx, double sy, double sz, double ex, double ey, double ez) {
     ASSERT_VALID_INDEX(index, particles);
-    if (yparticle == -1 && (ry != rz || ey != ez))
+    if (yparticle == -1 && (sy != sz || ey != ez))
         throw OpenMMException("GayBerneForce: yparticle is -1 for a particle that is not axially symmetric");
-    if (xparticle == -1 && (rx != rz || ex != ez))
+    if (xparticle == -1 && (sx != sz || ex != ez))
         throw OpenMMException("GayBerneForce: xparticle is -1 for a particle that is not spherical");
     if (xparticle == -1 && yparticle != -1)
         throw OpenMMException("GayBerneForce: xparticle cannot be -1 if yparticle is not also -1");
@@ -119,9 +119,9 @@ void GayBerneForce::setParticleParameters(int index, double sigma, double epsilo
     particles[index].epsilon = epsilon;
     particles[index].xparticle = xparticle;
     particles[index].yparticle = yparticle;
-    particles[index].rx = rx;
-    particles[index].ry = ry;
-    particles[index].rz = rx;
+    particles[index].sx = sx;
+    particles[index].sy = sy;
+    particles[index].sz = sz;
     particles[index].ex = ex;
     particles[index].ey = ey;
     particles[index].ez = ez;

platforms/reference/include/ReferenceGayBerneForce.h

@@ -71,6 +71,8 @@ private:
 
     void computeEllipsoidFrames(const std::vector<RealVec>& positions);
     
+    void applyTorques(const std::vector<RealVec>& positions, std::vector<RealVec>& forces, const std::vector<RealVec>& torques);
+
     RealOpenMM computeOneInteraction(int particle1, int particle2, RealOpenMM sigma, RealOpenMM epsilon, const std::vector<RealVec>& positions,
             std::vector<RealVec>& forces, std::vector<RealVec>& torques, const RealVec* boxVectors);
 };
@@ -134,7 +136,7 @@ struct ReferenceGayBerneForce::Matrix {
 };
 
 static RealVec operator*(const RealVec& r, ReferenceGayBerneForce::Matrix& m) {
-    return RealVec(m.v[0][0]*r[0] + m.v[1][1]*r[1] + m.v[2][2]*r[2],
+    return RealVec(m.v[0][0]*r[0] + m.v[1][0]*r[1] + m.v[2][0]*r[2],
                    m.v[0][1]*r[0] + m.v[1][1]*r[1] + m.v[2][1]*r[2],
                    m.v[0][2]*r[0] + m.v[1][2]*r[1] + m.v[2][2]*r[2]);
 }

platforms/reference/src/SimTKReference/ReferenceGayBerneForce.cpp

@@ -44,7 +44,11 @@ ReferenceGayBerneForce::ReferenceGayBerneForce(const GayBerneForce& force) {
     particles.resize(numParticles);
     for (int i = 0; i < numParticles; i++) {
         ParticleInfo& p = particles[i];
-        force.getParticleParameters(i, p.sigma, p.epsilon, p.xparticle, p.yparticle, p.rx, p.ry, p.rz, p.ex, p.ey, p.ez);
+        double sx, sy, sz;
+        force.getParticleParameters(i, p.sigma, p.epsilon, p.xparticle, p.yparticle, sx, sy, sz, p.ex, p.ey, p.ez);
+        p.rx = 0.5*sx;
+        p.ry = 0.5*sy;
+        p.rz = 0.5*sz;
     }
     int numExceptions = force.getNumExceptions();
     exceptions.resize(numExceptions);
@@ -110,6 +114,10 @@ RealOpenMM ReferenceGayBerneForce::calculateForce(const vector<RealVec>& positio
         ExceptionInfo& e = exceptions[i];
         energy += computeOneInteraction(e.particle1, e.particle2, e.sigma, e.epsilon, positions, forces, torques, boxVectors);
     }
+    
+    // Apply torques.
+    
+    applyTorques(positions, forces, torques);
     return energy;
 }
 
@@ -156,7 +164,7 @@ void ReferenceGayBerneForce::computeEllipsoidFrames(const vector<RealVec>& posit
         a[2][1] = zdir[1];
         a[2][2] = zdir[2];
         RealVec r2(p.rx*p.rx, p.ry*p.ry, p.rz*p.rz);
-        RealVec e2(p.ex*p.ex, p.ey*p.ey, p.ez*p.ez);
+        RealVec e2(1/sqrt(p.ex), 1/sqrt(p.ey), 1/sqrt(p.ez));
         for (int i = 0; i < 3; i++)
             for (int j = 0; j < 3; j++) {
                 b[i][j] = 0;
@@ -169,6 +177,34 @@ void ReferenceGayBerneForce::computeEllipsoidFrames(const vector<RealVec>& posit
     }
 }
 
+void ReferenceGayBerneForce::applyTorques(const vector<RealVec>& positions, vector<RealVec>& forces, const vector<RealVec>& torques) {
+    int numParticles = particles.size();
+    for (int particle = 0; particle < numParticles; particle++) {
+        ParticleInfo& p = particles[particle];
+        RealVec pos = positions[particle];
+        if (p.xparticle != -1) {
+            // Apply a force to the x particle.
+            
+            RealVec dx = positions[p.xparticle]-pos;
+            double dx2 = dx.dot(dx);
+            RealVec f = torques[particle].cross(dx)/dx2;
+            forces[p.xparticle] += f;
+            forces[particle] -= f;
+            if (p.yparticle != -1) {
+                // Apply a force to the y particle.  This is based on the component of the torque
+                // that was not already applied to the x particle.
+                
+                RealVec dy = positions[p.yparticle]-pos;
+                double dy2 = dy.dot(dy);
+                RealVec torque = dx*(torques[particle].dot(dx)/dx2);
+                f = torque.cross(dy)/dy2;
+                forces[p.yparticle] += f;
+                forces[particle] -= f;
+            }
+        }
+    }
+}
+
 RealOpenMM ReferenceGayBerneForce::computeOneInteraction(int particle1, int particle2, RealOpenMM sigma, RealOpenMM epsilon, const vector<RealVec>& positions,
         vector<RealVec>& forces, vector<RealVec>& torques, const RealVec* boxVectors) {
     // Compute the displacement and check against the cutoff.
@@ -195,8 +231,6 @@ RealOpenMM ReferenceGayBerneForce::computeOneInteraction(int particle1, int part
 
     // Estimate the distance between the ellipsoids and compute the first terms needed for the energy.
 
-    ParticleInfo& p1 = particles[particle1];
-    ParticleInfo& p2 = particles[particle2];
     RealOpenMM sigma12 = 1/SQRT(0.5*drUnit.dot(G12inv*drUnit));
     RealOpenMM h12 = r - sigma12;
     RealOpenMM rho = sigma/(h12+sigma);
@@ -224,11 +258,12 @@ RealOpenMM ReferenceGayBerneForce::computeOneInteraction(int particle1, int part
 
     for (int j = 0; j < 2; j++) {
         int particle = (j == 0 ? particle1 : particle2);
-        RealVec dudq = A[particle]*((kappa*G[particle]).cross(kappa*temp));
-        RealVec dchidq = (A[particle]*((iota*B[particle]).cross(iota)))*(-4*rInv2);
+        RealVec dudq = (kappa*G[particle]).cross(kappa*(temp*dUSLJdr));
+        RealVec dchidq = (iota*B[particle]).cross(iota)*(-4*rInv2);
         RealOpenMM (&g12)[3][3] = G12.v;
         RealOpenMM (&a)[3][3] = A[particle].v;
-        RealVec scale = RealVec(particles[particle].rx, particles[particle].ry, particles[particle].rz)*(-eta/detG12);
+        ParticleInfo& p = particles[particle];
+        RealVec scale = RealVec(p.rx*p.rx, p.ry*p.ry, p.rz*p.rz)*(-0.5*eta/detG12);
         Matrix D;
         RealOpenMM (&d)[3][3] = D.v;
         d[0][0] = scale[0]*(g12[1][2]*g12[0][1]*a[0][2] + 2*g12[1][1]*g12[2][2]*a[0][0] -
@@ -280,7 +315,7 @@ RealOpenMM ReferenceGayBerneForce::computeOneInteraction(int particle1, int part
         for (int i = 0; i < 3; i++)
             detadq += RealVec(a[i][0], a[i][1], a[i][2]).cross(RealVec(d[i][0], d[i][1], d[i][2]));
         RealVec torque = dchidq*(u*eta) + detadq*(u*chi) + dudq*(eta*chi);
-        torques[particle] += torque;
+        torques[particle] -= torque;
     }
     return u*eta*chi;
 }

tests/TestGayBerneForce.h

@@ -64,7 +64,7 @@ void testPointParticles() {
     init_gen_rand(0, sfmt);
     for (int i = 0; i < numParticles; i++) {
         system.addParticle(1.0);
-        gb->addParticle(sigma, epsilon, -1, -1, sigma/2, sigma/2, sigma/2, 1, 1, 1);
+        gb->addParticle(sigma, epsilon, -1, -1, sigma, sigma, sigma, 1, 1, 1);
         nb->addParticle(0, sigma, epsilon);
         positions.push_back(Vec3(2.0*genrand_real2(sfmt), 2.0*genrand_real2(sfmt), 2.0*genrand_real2(sfmt)));
     }
@@ -91,10 +91,10 @@ void testEnergyScales() {
         system.addParticle(1.0);
     GayBerneForce* gb = new GayBerneForce();
     system.addForce(gb);
-    gb->addParticle(sigma, epsilon, 1, 2, sigma/2, sigma/2, sigma/2, 1.1, 1.5, 1.8);
+    gb->addParticle(sigma, epsilon, 1, 2, sigma, sigma, sigma, 1.1, 1.5, 1.8);
     gb->addParticle(1, 0, -1, -1, 1, 1, 1, 1, 1, 1);
     gb->addParticle(1, 0, -1, -1, 1, 1, 1, 1, 1, 1);
-    gb->addParticle(sigma, epsilon, 4, 5, sigma/2, sigma/2, sigma/2, 1.2, 1.6, 1.7);
+    gb->addParticle(sigma, epsilon, 4, 5, sigma, sigma, sigma, 1.2, 1.6, 1.7);
     gb->addParticle(1, 0, -1, -1, 1, 1, 1, 1, 1, 1);
     gb->addParticle(1, 0, -1, -1, 1, 1, 1, 1, 1, 1);
     vector<Vec3> positions(6);
@@ -113,7 +113,7 @@ void testEnergyScales() {
 
     double expectedEnergy = 4*epsilon*(pow(sigma, 12.0)-pow(sigma, 6.0));
     double expectedForce = 4*epsilon*(12*pow(sigma, 12.0)-6*pow(sigma, 6.0));
-    double expectedScale = pow(2.0/(1.1*1.1+1.2*1.2), 2.0);
+    double expectedScale = pow(2.0/(1/sqrt(1.1) + 1/sqrt(1.2)), 2.0);
     State state = context.getState(State::Forces | State::Energy);
     ASSERT_EQUAL_TOL(expectedEnergy*expectedScale, state.getPotentialEnergy(), 1e-5);
     ASSERT_EQUAL_VEC(Vec3(expectedForce*expectedScale, 0, 0), state.getForces()[3], 1e-5);
@@ -121,7 +121,7 @@ void testEnergyScales() {
     positions[4] = Vec3(1, 1, 0);
     positions[5] = Vec3(0, 2, 0);
     context.setPositions(positions);
-    expectedScale = pow(2.0/(1.5*1.5+1.6*1.6), 2.0);
+    expectedScale = pow(2.0/(1/sqrt(1.5) + 1/sqrt(1.6)), 2.0);
     state = context.getState(State::Forces | State::Energy);
     ASSERT_EQUAL_TOL(expectedEnergy*expectedScale, state.getPotentialEnergy(), 1e-5);
     ASSERT_EQUAL_VEC(Vec3(0, expectedForce*expectedScale, 0), state.getForces()[3], 1e-5);
@@ -129,7 +129,7 @@ void testEnergyScales() {
     positions[4] = Vec3(1, 1, 0);
     positions[5] = Vec3(0, 1, 1);
     context.setPositions(positions);
-    expectedScale = pow(2.0/(1.5*1.5+1.7*1.7), 2.0);
+    expectedScale = pow(2.0/(1/sqrt(1.5) + 1/sqrt(1.7)), 2.0);
     state = context.getState(State::Forces | State::Energy);
     ASSERT_EQUAL_TOL(expectedEnergy*expectedScale, state.getPotentialEnergy(), 1e-5);
     ASSERT_EQUAL_VEC(Vec3(0, expectedForce*expectedScale, 0), state.getForces()[3], 1e-5);
@@ -154,7 +154,7 @@ void testEnergyConservation() {
                 system.addParticle(10.0);
                 system.addParticle(1.0);
                 system.addParticle(1.0);
-                gb->addParticle(0.2, 2.0, first+1, first+2, 0.2, 0.25, 0.3, 0.9, 1.0, 1.1);
+                gb->addParticle(0.2, 10.0, first+1, first+2, 0.2, 0.25, 0.3, 0.9, 1.0, 1.1);
                 gb->addParticle(1.0, 0.0, -1, -1, 1, 1, 1, 1, 1, 1);
                 gb->addParticle(1.0, 0.0, -1, -1, 1, 1, 1, 1, 1, 1);
                 positions.push_back(Vec3(x, y, z));
@@ -166,22 +166,19 @@ void testEnergyConservation() {
             }
         }
     }
-    VerletIntegrator integ(0.001);
+    VerletIntegrator integ(0.0005);
     Context context(system, integ, platform);
     context.setPositions(positions);
     context.setVelocitiesToTemperature(300.0);
+    double initialEnergy;
     for (int i = 0; i < 100; i++) {
+        integ.step(5);
         State state = context.getState(State::Energy);
-        printf("%d %g\n", i, state.getPotentialEnergy()+state.getKineticEnergy());
-        integ.step(10);
-    }
-
-    State state = context.getState(State::Positions);
-    for (int i = 0; i < 9; i++) {
-        Vec3 p1 = state.getPositions()[3*i];
-        Vec3 p2 = state.getPositions()[3*i+1];
-        Vec3 p3 = state.getPositions()[3*i+2];
-        printf("%g %g %g, %g %g %g, %g %g %g\n", p1[0], p1[1], p1[2], p2[0]-p1[0], p2[1]-p1[1], p2[2]-p1[2], p3[0]-p1[0], p3[1]-p1[1], p3[2]-p1[2]);
+        double energy = state.getPotentialEnergy()+state.getKineticEnergy();
+        if (i == 0)
+            initialEnergy = energy;
+        else
+            ASSERT_EQUAL_TOL(initialEnergy, energy, 1e-3);
     }
 }