diff --git a/applications/solvers/molecularDynamics/mdWaterTest/mdWaterTest.C b/applications/solvers/molecularDynamics/mdWaterTest/mdWaterTest.C
index 73ddb32349..7bb1acfd91 100644
--- a/applications/solvers/molecularDynamics/mdWaterTest/mdWaterTest.C
+++ b/applications/solvers/molecularDynamics/mdWaterTest/mdWaterTest.C
@@ -25,12 +25,13 @@ License
 Application
     mdWaterTest
 Description
-    Testing TIP4P potential and dynamics test
+    Testing TIP4P potential and dynamics
 
 \*---------------------------------------------------------------------------*/
 
 #include "fvCFD.H"
 #include "md.H"
+#include "diagTensor.H"
 
 int main(int argc, char *argv[])
 {
@@ -57,23 +58,23 @@ int main(int argc, char *argv[])
     qSites[2] = 0.0;
     qSites[3] = -2.0*qSites[0];
 
-    qSites *= 1.602176487e-19;
+    qSites = qSites*1.602176487e-19;
 
     scalar rH = 0.9572e-10;
     scalar rM = 0.15e-10;
-    scalar thetaH = 104.52/2.0;
+    scalar thetaH = mathematicalConstant::pi*104.52/(2.0*180.0);
 
     vectorList pSites(nSites);
 
-    pSites[0] = rH*vector
+    pSites[0] = vector
     (
-        sin(thetaH),
-        cos(thetaH)*(1.0 - 1.0/(1.0 + 0.5*mSites[2]/mSites[0))),
+        rH*Foam::sin(thetaH),
+        rH*Foam::cos(thetaH)*(1.0 - 1.0/(1.0 + 0.5*mSites[2]/mSites[0])),
         0
     );
-    pSites[1] = vector(-pSites[0].x(), pSites[0].x(), 0);
-    pSites[2] = vector(0, -cos(thetaH)*rH/(1.0 +  0.5*mSites[2]/mSites[0]), 0);
-    ps[3] = vector(0, rM + pSites[2].y(), 0);
+    pSites[1] = vector(-pSites[0].x(), pSites[0].y(), 0);
+    pSites[2] = vector(0, -Foam::cos(thetaH)*rH/(1.0 +  0.5*mSites[2]/mSites[0]), 0);
+    pSites[3] = vector(0, rM + pSites[2].y(), 0);
 
     diagTensor I
     (
@@ -86,21 +87,130 @@ int main(int argc, char *argv[])
 
     vector p1(0, 0, 0);
 
-    vector v1(150.0, -23.0, 92);
+    vector v1(150.0, 0, 0);
 
-    tensor R1(tensor::one);
+    tensor R1(tensor(1, 0, 0, 0, 1, 0, 0, 0, 1));
 
-    vector omega1(1.2e13, -2.6e11, 5.9e12);
+    vector omega1(6e12, 3e12, -5e12);
+
+    vectorList pSites1 = R1 & pSites;
 
     vectorList fSites1(nSites, vector::zero);
 
-    Info << "\nStarting time loop\n" << endl;
+    vector a1(vector::zero);
+
+    vector alpha1(vector::zero);
+
+    Info<< nl << p1
+        << nl << v1
+        << nl << R1
+        << nl << omega1
+        << nl << fSites1
+        << nl << pSites1
+        << endl;
+
+    // scalar freq = 1e12;
+    // scalar ampl = 3e7;
+
+    vector eForce(vector::zero);
+
+    Info<< "\nStarting time loop\n" << endl;
+
+    Info<< "3" << nl << "Water test"
+        << nl << "H1" << " " << pSites1[0].x() << " " << pSites1[0].y() << " " << pSites1[0].z()
+        << nl << "H2" << " " << pSites1[1].x() << " " << pSites1[1].y() << " " << pSites1[1].z()
+        << nl << "0"  << " " << pSites1[2].x() << " " << pSites1[2].y() << " " << pSites1[2].z()
+        << nl << "M"  << " " << pSites1[3].x() << " " << pSites1[3].y() << " " << pSites1[3].z()
+        << endl;
 
     while (runTime.run())
     {
         runTime++;
 
-        Info << "Time = " << runTime.timeName() << endl;
+        // Leapfrog part 1
+        v1 += 0.5*runTime.deltaT().value()*a1;
+
+        p1 += runTime.deltaT().value()*v1;
+
+        omega1 += 0.5*runTime.deltaT().value()*alpha1;
+
+        scalar phi1 = 0.5*runTime.deltaT().value()*omega1.x();
+
+        tensor U1
+        (
+            tensor
+            (
+                1, 0, 0,
+                0, Foam::cos(phi1), Foam::sin(phi1),
+                0, -Foam::sin(phi1), Foam::cos(phi1)
+            )
+        );
+
+        scalar phi2 = 0.5*runTime.deltaT().value()*omega1.y();
+
+        tensor U2
+        (
+            tensor
+            (
+                Foam::cos(phi2), 0, -Foam::sin(phi2),
+                0, 1, 0,
+                Foam::sin(phi2), 0, Foam::cos(phi2)
+            )
+        );
+
+        scalar phi3 = runTime.deltaT().value()*omega1.z();
+
+        tensor U3
+        (
+            tensor
+            (
+                Foam::cos(phi3), Foam::sin(phi3), 0,
+                -Foam::sin(phi3), Foam::cos(phi3), 0,
+                0, 0, 1
+            )
+        );
+
+        tensor UT = U1.T() & U2.T() & U3.T() & U2.T() & U1.T();
+
+        R1 = R1 & UT;
+
+        pSites1 = p1 + (R1 & pSites);
+
+        // Force calculation
+
+        a1 = vector::zero;
+
+        //        eForce.z() = ampl*Foam::sin(2*mathematicalConstant::pi*freq*runTime.timeOutputValue());
+
+        //        fSites1 = qSites * eForce;
+
+        vector tau1(vector::zero);
+
+        forAll(fSites1, fS)
+        {
+            const vector& f = fSites1[fS];
+
+            a1 += f/m;
+
+            tau1 += (pSites1[fS] - p1) ^ f;
+        }
+
+        alpha1 = R1 & inv(I) & R1.T() & tau1;
+
+        // Leapfrog part 2
+
+        v1 += 0.5*runTime.deltaT().value()*a1;
+
+        omega1 += 0.5*runTime.deltaT().value()*alpha1;
+
+        Info << "3" << nl << "Water test"
+            << nl << "H1" << " " << pSites1[0].x() << " " << pSites1[0].y() << " " << pSites1[0].z()
+            << nl << "H2" << " " << pSites1[1].x() << " " << pSites1[1].y() << " " << pSites1[1].z()
+            << nl << "0"  << " " << pSites1[2].x() << " " << pSites1[2].y() << " " << pSites1[2].z()
+            << nl << "M"  << " " << pSites1[3].x() << " " << pSites1[3].y() << " " << pSites1[3].z()
+            << endl;
+
+        // Info<< "Time = " << runTime.timeName() << endl;
     }
 
     Info << "End\n" << endl;