Created vector classes to simplify SSE code

openmmapi/include/openmm/internal/vectorize.h

+#ifndef OPENMM_VECTORIZE_H_
+#define OPENMM_VECTORIZE_H_
+
+/* -------------------------------------------------------------------------- *
+ *                                   OpenMM                                   *
+ * -------------------------------------------------------------------------- *
+ * This is part of the OpenMM molecular simulation toolkit originating from   *
+ * Simbios, the NIH National Center for Physics-Based Simulation of           *
+ * Biological Structures at Stanford, funded under the NIH Roadmap for        *
+ * Medical Research, grant U54 GM072970. See https://simtk.org.               *
+ *                                                                            *
+ * Portions copyright (c) 2013 Stanford University and the Authors.           *
+ * Authors: Peter Eastman                                                     *
+ * Contributors:                                                              *
+ *                                                                            *
+ * 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 <smmintrin.h>
+
+
+// This file defines classes and functions to simplify vectorizing code with SSE.
+
+class ivec4;
+
+/**
+ * A four element vector of floats.
+ */
+class fvec4 {
+public:
+    __m128 val;
+    
+    fvec4() {}
+    fvec4(float v) : val(_mm_set1_ps(v)) {}
+    fvec4(float v1, float v2, float v3, float v4) : val(_mm_set_ps(v4, v3, v2, v1)) {}
+    fvec4(__m128 v) : val(v) {}
+    fvec4(const float* v) : val(_mm_loadu_ps(v)) {}
+    operator __m128() const {
+        return val;
+    }
+    float operator[](int i) const {
+        int resultBits = _mm_extract_ps(val, i);
+        return *((float*) &resultBits);
+    }
+    void store(float* v) const {
+        _mm_storeu_ps(v, val);
+    }
+    fvec4 operator+(fvec4 other) const {
+        return _mm_add_ps(val, other);
+    }
+    fvec4 operator-(fvec4 other) const {
+        return _mm_sub_ps(val, other);
+    }
+    fvec4 operator*(fvec4 other) const {
+        return _mm_mul_ps(val, other);
+    }
+    fvec4 operator/(fvec4 other) const {
+        return _mm_div_ps(val, other);
+    }
+    void operator+=(fvec4 other) {
+        val = _mm_add_ps(val, other);
+    }
+    void operator-=(fvec4 other) {
+        val = _mm_sub_ps(val, other);
+    }
+    void operator*=(fvec4 other) {
+        val = _mm_mul_ps(val, other);
+    }
+    void operator/=(fvec4 other) {
+        val = _mm_div_ps(val, other);
+    }
+    fvec4 operator-() const {
+        return _mm_sub_ps(_mm_set1_ps(0.0f), val);
+    }
+    operator ivec4() const;
+};
+
+/**
+ * A four element vector of ints.
+ */
+class ivec4 {
+public:
+    __m128i val;
+    
+    ivec4() {}
+    ivec4(int v) : val(_mm_set1_epi32(v)) {}
+    ivec4(int v1, int v2, int v3, int v4) : val(_mm_set_epi32(v4, v3, v2, v1)) {}
+    ivec4(__m128i v) : val(v) {}
+    ivec4(const int* v) : val(_mm_loadu_si128((const __m128i*) v)) {}
+    operator __m128i() const {
+        return val;
+    }
+    int operator[](int i) const {
+        return _mm_extract_epi32(val, i);
+    }
+    void store(int* v) const {
+        _mm_storeu_si128((__m128i*) v, val);
+    }
+    ivec4 operator+(ivec4 other) const {
+        return _mm_add_epi32(val, other);
+    }
+    ivec4 operator-(ivec4 other) const {
+        return _mm_sub_epi32(val, other);
+    }
+    ivec4 operator*(ivec4 other) const {
+        return _mm_mul_epi32(val, other);
+    }
+    void operator+=(ivec4 other) {
+        val = _mm_add_epi32(val, other);
+    }
+    void operator-=(ivec4 other) {
+        val = _mm_sub_epi32(val, other);
+    }
+    void operator*=(ivec4 other) {
+        val = _mm_mul_epi32(val, other);
+    }
+    ivec4 operator-() const {
+        return _mm_sub_epi32(_mm_set1_epi32(0), val);
+    }
+    ivec4 operator&(ivec4 other) const {
+        return _mm_and_si128(val, other);
+    }
+    ivec4 operator==(ivec4 other) const {
+        return _mm_cmpeq_epi32(val, other);
+    }
+    operator fvec4() const;
+};
+
+// Conversion operators.
+
+inline fvec4::operator ivec4() const {
+    return _mm_cvttps_epi32(val);
+}
+
+inline ivec4::operator fvec4() const {
+    return _mm_cvtepi32_ps(val);
+}
+
+// Functions that operate on fvec4s.
+
+static inline fvec4 floor(fvec4 v) {
+    return fvec4(_mm_floor_ps(v.val));
+}
+
+static inline fvec4 ceil(fvec4 v) {
+    return fvec4(_mm_ceil_ps(v.val));
+}
+
+static inline fvec4 round(fvec4 v) {
+    return fvec4(_mm_round_ps(v.val, _MM_FROUND_TO_NEAREST_INT));
+}
+
+static inline fvec4 min(fvec4 v1, fvec4 v2) {
+    return fvec4(_mm_min_ps(v1.val, v2.val));
+}
+
+static inline fvec4 max(fvec4 v1, fvec4 v2) {
+    return fvec4(_mm_max_ps(v1.val, v2.val));
+}
+
+static inline fvec4 sqrt(fvec4 v) {
+    return fvec4(_mm_sqrt_ps(v.val));
+}
+
+static inline float dot3(fvec4 v1, fvec4 v2) {
+    return _mm_cvtss_f32(_mm_dp_ps(v1, v2, 0x71));
+}
+
+// Functions that operate on ivec4s.
+
+static inline ivec4 min(ivec4 v1, ivec4 v2) {
+    return ivec4(_mm_min_epi32(v1.val, v2.val));
+}
+
+static inline ivec4 max(ivec4 v1, ivec4 v2) {
+    return ivec4(_mm_max_epi32(v1.val, v2.val));
+}
+
+#endif /*OPENMM_VECTORIZE_H_*/
+

platforms/cpu/include/CpuNonbondedForce.h

@@ -26,11 +26,11 @@
 #define OPENMM_CPU_NONBONDED_FORCE_H__
 
 #include "ReferencePairIxn.h"
+#include "openmm/internal/vectorize.h"
 #include <pthread.h>
 #include <set>
 #include <utility>
 #include <vector>
-#include <smmintrin.h>
 // ---------------------------------------------------------------------------------------
 
 class CpuNonbondedForce {
@@ -195,7 +195,7 @@ private:
             
          --------------------------------------------------------------------------------------- */
           
-      void calculateOneIxn(int atom1, int atom2, float* forces, double* totalEnergy, const __m128& boxSize, const __m128& invBoxSize);
+      void calculateOneIxn(int atom1, int atom2, float* forces, double* totalEnergy, const fvec4& boxSize, const fvec4& invBoxSize);
             
       /**---------------------------------------------------------------------------------------
       
@@ -208,13 +208,13 @@ private:
             
          --------------------------------------------------------------------------------------- */
           
-      void calculateOneEwaldIxn(int atom1, int atom2, float* forces, double* totalEnergy, const __m128& boxSize, const __m128& invBoxSize);
+      void calculateOneEwaldIxn(int atom1, int atom2, float* forces, double* totalEnergy, const fvec4& boxSize, const fvec4& invBoxSize);
 
       /**
        * Compute the displacement and squared distance between two points, optionally using
        * periodic boundary conditions.
        */
-      void getDeltaR(const __m128& posI, const __m128& posJ, __m128& deltaR, float& r2, bool periodic, const __m128& boxSize, const __m128& invBoxSize) const;
+      void getDeltaR(const fvec4& posI, const fvec4& posJ, fvec4& deltaR, float& r2, bool periodic, const fvec4& boxSize, const fvec4& invBoxSize) const;
 
       /**
        * Compute a fast approximation to erfc(x).

platforms/cpu/src/CpuNeighborList.cpp

 #include "CpuNeighborList.h"
 #include "openmm/internal/hardware.h"
+#include "openmm/internal/vectorize.h"
+#include <algorithm>
 #include <set>
 #include <map>
 #include <cmath>
@@ -82,10 +84,9 @@ public:
 
         const int atomI = referencePoint.second;
         const float* locationI = referencePoint.first;
-        __m128 posI = _mm_loadu_ps(locationI);
-        __m128 boxSize = _mm_set_ps(0, periodicBoxSize[2], periodicBoxSize[1], periodicBoxSize[0]);
-        __m128 invBoxSize = _mm_set_ps(0, (1/periodicBoxSize[2]), (1/periodicBoxSize[1]), (1/periodicBoxSize[0]));
-        __m128 half = _mm_set1_ps(0.5);
+        fvec4 posI(locationI);
+        fvec4 boxSize(periodicBoxSize[0], periodicBoxSize[1], periodicBoxSize[2], 0);
+        fvec4 invBoxSize(1/periodicBoxSize[0], 1/periodicBoxSize[1], 1/periodicBoxSize[2], 0);
         
         float maxDistanceSquared = maxDistance * maxDistance;
 
@@ -125,13 +126,13 @@ public:
                         if (atomJ >= atomI)
                             break;
                         
-                        __m128 posJ = _mm_loadu_ps(itemIter->first);
-                        __m128 delta = _mm_sub_ps(posJ, posI);
+                        fvec4 posJ(itemIter->first);
+                        fvec4 delta = posJ-posI;
                         if (usePeriodic) {
-                            __m128 base = _mm_mul_ps(_mm_floor_ps(_mm_add_ps(_mm_mul_ps(delta, invBoxSize), half)), boxSize);
-                            delta = _mm_sub_ps(delta, base);
+                            fvec4 base = round(delta*invBoxSize)*boxSize;
+                            delta = delta-base;
                         }
-                        float dSquared = _mm_cvtss_f32(_mm_dp_ps(delta, delta, 0x71));
+                        float dSquared = dot3(delta, delta);
                         if (dSquared > maxDistanceSquared)
                             continue;
                         

platforms/cpu/src/CpuNonbondedForce.cpp

@@ -32,6 +32,7 @@
 #include "ReferencePME.h"
 #include "openmm/internal/hardware.h"
 #include "openmm/internal/SplineFitter.h"
+#include "openmm/internal/vectorize.h"
 
 // In case we're using some primitive version of Visual Studio this will
 // make sure that erf() and erfc() are defined.
@@ -358,25 +359,25 @@ void CpuNonbondedForce::calculateDirectIxn(int numberOfAtoms, float* posq, const
     for (int i = 0; i < numThreads; i++)
         directEnergy += threadData[i]->threadEnergy;
     for (int i = 0; i < numberOfAtoms; i++) {
-        __m128 f = _mm_loadu_ps(forces+4*i);
+        fvec4 f(forces+4*i);
         for (int j = 0; j < numThreads; j++)
-            f = _mm_add_ps(f, _mm_loadu_ps(&threadData[j]->threadForce[4*i]));
-        _mm_storeu_ps(forces+4*i, f);
+            f += fvec4(&threadData[j]->threadForce[4*i]);
+        f.store(forces+4*i);
     }
 
     if (ewald || pme) {
         // Now subtract off the exclusions, since they were implicitly included in the reciprocal space sum.
 
-        __m128 boxSize = _mm_set_ps(0, periodicBoxSize[2], periodicBoxSize[1], periodicBoxSize[0]);
-        __m128 invBoxSize = _mm_set_ps(0, (1/periodicBoxSize[2]), (1/periodicBoxSize[1]), (1/periodicBoxSize[0]));
+        fvec4 boxSize(periodicBoxSize[0], periodicBoxSize[1], periodicBoxSize[2], 0);
+        fvec4 invBoxSize((1/periodicBoxSize[0]), (1/periodicBoxSize[1]), (1/periodicBoxSize[2]), 0);
         for (int i = 0; i < numberOfAtoms; i++)
             for (set<int>::const_iterator iter = exclusions[i].begin(); iter != exclusions[i].end(); ++iter) {
                 if (*iter > i) {
                     int ii = i;
                     int jj = *iter;
-                    __m128 deltaR;
-                    __m128 posI = _mm_loadu_ps(posq+4*ii);
-                    __m128 posJ = _mm_loadu_ps(posq+4*jj);
+                    fvec4 deltaR;
+                    fvec4 posI(posq+4*ii);
+                    fvec4 posJ(posq+4*jj);
                     float r2;
                     getDeltaR(posJ, posI, deltaR, r2, false, boxSize, invBoxSize);
                     float r         = sqrtf(r2);
@@ -386,9 +387,9 @@ void CpuNonbondedForce::calculateDirectIxn(int numberOfAtoms, float* posq, const
                     float erfcAlphaR = erfcApprox(alphaR);
                     float dEdR      = (float) (chargeProd * inverseR * inverseR * inverseR);
                           dEdR      = (float) (dEdR * (1.0f-erfcAlphaR-TWO_OVER_SQRT_PI*alphaR*exp(-alphaR*alphaR)));
-                    __m128 result = _mm_mul_ps(deltaR, _mm_set1_ps(dEdR));
-                    _mm_storeu_ps(forces+4*ii, _mm_sub_ps(_mm_loadu_ps(forces+4*ii), result));
-                    _mm_storeu_ps(forces+4*jj, _mm_add_ps(_mm_loadu_ps(forces+4*jj), result));
+                    fvec4 result = deltaR*dEdR;
+                    (fvec4(forces+4*ii)-result).store(forces+4*ii);
+                    (fvec4(forces+4*jj)+result).store(forces+4*jj);
                     if (includeEnergy)
                         directEnergy -= chargeProd*inverseR*(1.0f-erfcAlphaR);
                 }
@@ -418,8 +419,8 @@ void CpuNonbondedForce::runThread(int index, vector<float>& threadForce, double&
         threadForce.resize(4*numberOfAtoms, 0.0f);
         for (int i = 0; i < 4*numberOfAtoms; i++)
             threadForce[i] = 0.0f;
-        __m128 boxSize = _mm_set_ps(0, periodicBoxSize[2], periodicBoxSize[1], periodicBoxSize[0]);
-        __m128 invBoxSize = _mm_set_ps(0, (1/periodicBoxSize[2]), (1/periodicBoxSize[1]), (1/periodicBoxSize[0]));
+        fvec4 boxSize(periodicBoxSize[0], periodicBoxSize[1], periodicBoxSize[2], 0);
+        fvec4 invBoxSize((1/periodicBoxSize[0]), (1/periodicBoxSize[1]), (1/periodicBoxSize[2]), 0);
         if (ewald || pme) {
             // Compute the interactions from the neighbor list.
 
@@ -448,12 +449,12 @@ void CpuNonbondedForce::runThread(int index, vector<float>& threadForce, double&
     }
 }
 
-void CpuNonbondedForce::calculateOneIxn(int ii, int jj, float* forces, double* totalEnergy, const __m128& boxSize, const __m128& invBoxSize) {
+void CpuNonbondedForce::calculateOneIxn(int ii, int jj, float* forces, double* totalEnergy, const fvec4& boxSize, const fvec4& invBoxSize) {
     // get deltaR, R2, and R between 2 atoms
 
-    __m128 deltaR;
-    __m128 posI = _mm_loadu_ps(posq+4*ii);
-    __m128 posJ = _mm_loadu_ps(posq+4*jj);
+    fvec4 deltaR;
+    fvec4 posI(posq+4*ii);
+    fvec4 posJ(posq+4*jj);
     float r2;
     getDeltaR(posJ, posI, deltaR, r2, periodic, boxSize, invBoxSize);
     if (cutoff && r2 >= cutoffDistance*cutoffDistance)
@@ -497,15 +498,15 @@ void CpuNonbondedForce::calculateOneIxn(int ii, int jj, float* forces, double* t
 
     // accumulate forces
 
-    __m128 result = _mm_mul_ps(deltaR, _mm_set1_ps(dEdR));
-    _mm_storeu_ps(forces+4*ii, _mm_add_ps(_mm_loadu_ps(forces+4*ii), result));
-    _mm_storeu_ps(forces+4*jj, _mm_sub_ps(_mm_loadu_ps(forces+4*jj), result));
+    fvec4 result = deltaR*dEdR;
+    (fvec4(forces+4*ii)+result).store(forces+4*ii);
+    (fvec4(forces+4*jj)-result).store(forces+4*jj);
   }
 
-void CpuNonbondedForce::calculateOneEwaldIxn(int ii, int jj, float* forces, double* totalEnergy, const __m128& boxSize, const __m128& invBoxSize) {
-    __m128 deltaR;
-    __m128 posI = _mm_loadu_ps(posq+4*ii);
-    __m128 posJ = _mm_loadu_ps(posq+4*jj);
+void CpuNonbondedForce::calculateOneEwaldIxn(int ii, int jj, float* forces, double* totalEnergy, const fvec4& boxSize, const fvec4& invBoxSize) {
+    fvec4 deltaR;
+    fvec4 posI(posq+4*ii);
+    fvec4 posJ(posq+4*jj);
     float r2;
     getDeltaR(posJ, posI, deltaR, r2, true, boxSize, invBoxSize);
     if (r2 < cutoffDistance*cutoffDistance) {
@@ -534,9 +535,9 @@ void CpuNonbondedForce::calculateOneEwaldIxn(int ii, int jj, float* forces, doub
 
         // accumulate forces
 
-        __m128 result = _mm_mul_ps(deltaR, _mm_set1_ps(dEdR));
-        _mm_storeu_ps(forces+4*ii, _mm_add_ps(_mm_loadu_ps(forces+4*ii), result));
-        _mm_storeu_ps(forces+4*jj, _mm_sub_ps(_mm_loadu_ps(forces+4*jj), result));
+        fvec4 result = deltaR*dEdR;
+        (fvec4(forces+4*ii)+result).store(forces+4*ii);
+        (fvec4(forces+4*jj)-result).store(forces+4*jj);
 
         // accumulate energies
 
@@ -547,13 +548,13 @@ void CpuNonbondedForce::calculateOneEwaldIxn(int ii, int jj, float* forces, doub
     }
 }
 
-void CpuNonbondedForce::getDeltaR(const __m128& posI, const __m128& posJ, __m128& deltaR, float& r2, bool periodic, const __m128& boxSize, const __m128& invBoxSize) const {
-    deltaR = _mm_sub_ps(posJ, posI);
+void CpuNonbondedForce::getDeltaR(const fvec4& posI, const fvec4& posJ, fvec4& deltaR, float& r2, bool periodic, const fvec4& boxSize, const fvec4& invBoxSize) const {
+    deltaR = posJ-posI;
     if (periodic) {
-        __m128 base = _mm_mul_ps(_mm_floor_ps(_mm_add_ps(_mm_mul_ps(deltaR, invBoxSize), _mm_set1_ps(0.5))), boxSize);
-        deltaR = _mm_sub_ps(deltaR, base);
+        fvec4 base = round(deltaR*invBoxSize)*boxSize;
+        deltaR = deltaR-base;
     }
-    r2 = _mm_cvtss_f32(_mm_dp_ps(deltaR, deltaR, 0x71));
+    r2 = dot3(deltaR, deltaR);
 }
 
 float CpuNonbondedForce::erfcApprox(float x) {

plugins/cpupme/src/CpuPmeKernels.cpp

@@ -35,9 +35,9 @@
 #include "CpuPmeKernels.h"
 #include "SimTKOpenMMRealType.h"
 #include "openmm/internal/hardware.h"
+#include "openmm/internal/vectorize.h"
 #include <cmath>
 #include <cstring>
-#include <smmintrin.h>
 
 using namespace OpenMM;
 using namespace std;
@@ -47,52 +47,50 @@ static const int PME_ORDER = 5;
 bool CpuCalcPmeReciprocalForceKernel::hasInitializedThreads = false;
 int CpuCalcPmeReciprocalForceKernel::numThreads = 0;
 
-#define EXTRACT_FLOAT(v, element) _mm_cvtss_f32(_mm_shuffle_ps(v, v, _MM_SHUFFLE(0, 0, 0, element)))
-
 static void spreadCharge(int start, int end, float* posq, float* grid, int gridx, int gridy, int gridz, int numParticles, Vec3 periodicBoxSize) {
     float temp[4];
-    __m128 boxSize = _mm_set_ps(0, (float) periodicBoxSize[2], (float) periodicBoxSize[1], (float) periodicBoxSize[0]);
-    __m128 invBoxSize = _mm_set_ps(0, (float) (1/periodicBoxSize[2]), (float) (1/periodicBoxSize[1]), (float) (1/periodicBoxSize[0]));
-    __m128 gridSize = _mm_set_ps(0, gridz, gridy, gridx);
-    __m128i gridSizeInt = _mm_set_epi32(0, gridz, gridy, gridx);
-    __m128 one  = _mm_set1_ps(1);
-    __m128 scale = _mm_set1_ps(1.0f/(PME_ORDER-1));
+    fvec4 boxSize((float) periodicBoxSize[0], (float) periodicBoxSize[1], (float) periodicBoxSize[2], 0);
+    fvec4 invBoxSize((float) (1/periodicBoxSize[0]), (float) (1/periodicBoxSize[1]), (float) (1/periodicBoxSize[2]), 0);
+    fvec4 gridSize(gridx, gridy, gridz, 0);
+    ivec4 gridSizeInt(gridx, gridy, gridz, 0);
+    fvec4 one(1);
+    fvec4 scale(1.0f/(PME_ORDER-1));
     const float epsilonFactor = sqrt(ONE_4PI_EPS0);
     memset(grid, 0, sizeof(float)*gridx*gridy*gridz);
     for (int i = start; i < end; i++) {
         // Find the position relative to the nearest grid point.
         
-        __m128 pos = _mm_loadu_ps(&posq[4*i]);
-        __m128 posFloor = _mm_floor_ps(_mm_mul_ps(pos, invBoxSize));
-        __m128 posInBox = _mm_sub_ps(pos, _mm_mul_ps(boxSize, posFloor));
-        __m128 t = _mm_mul_ps(_mm_mul_ps(posInBox, invBoxSize), gridSize);
-        __m128i ti = _mm_cvttps_epi32(t);
-        __m128 dr = _mm_sub_ps(t, _mm_cvtepi32_ps(ti));
-        __m128i gridIndex = _mm_sub_epi32(ti, _mm_and_si128(gridSizeInt, _mm_cmpeq_epi32(ti, gridSizeInt)));
+        fvec4 pos(&posq[4*i]);
+        fvec4 posFloor = floor(pos*invBoxSize);
+        fvec4 posInBox = pos-boxSize*posFloor;
+        fvec4 t = posInBox*invBoxSize*gridSize;
+        ivec4 ti = t;
+        fvec4 dr = t-ti;
+        ivec4 gridIndex = ti-(gridSizeInt&ti==gridSizeInt);
         
         // Compute the B-spline coefficients.
         
-        __m128 data[PME_ORDER];
-        data[PME_ORDER-1] = _mm_setzero_ps();
+        fvec4 data[PME_ORDER];
+        data[PME_ORDER-1] = 0.0f;
         data[1] = dr;
-        data[0] = _mm_sub_ps(one, dr);
+        data[0] = one-dr;
         for (int j = 3; j < PME_ORDER; j++) {
-            __m128 div = _mm_set1_ps(1.0f/(j-1));
-            data[j-1] = _mm_mul_ps(_mm_mul_ps(div, dr), data[j-2]);
+            fvec4 div(1.0f/(j-1));
+            data[j-1] = div*dr*data[j-2];
             for (int k = 1; k < j-1; k++)
-                data[j-k-1] = _mm_mul_ps(div, _mm_add_ps(_mm_mul_ps(_mm_add_ps(dr, _mm_set1_ps(k)), data[j-k-2]), _mm_mul_ps(_mm_sub_ps(_mm_set1_ps(j-k), dr), data[j-k-1])));
-            data[0] = _mm_mul_ps(_mm_mul_ps(div, _mm_sub_ps(one, dr)), data[0]);
+                data[j-k-1] = div*((dr+k)*data[j-k-2]+(fvec4(j-k)-dr)*data[j-k-1]);
+            data[0] = div*(one-dr)*data[0];
         }
-        data[PME_ORDER-1] = _mm_mul_ps(_mm_mul_ps(scale, dr), data[PME_ORDER-2]);
+        data[PME_ORDER-1] = scale*dr*data[PME_ORDER-2];
         for (int j = 1; j < (PME_ORDER-1); j++)
-            data[PME_ORDER-j-1] = _mm_mul_ps(scale, _mm_add_ps(_mm_mul_ps(_mm_add_ps(dr, _mm_set1_ps(j)), data[PME_ORDER-j-2]), _mm_mul_ps(_mm_sub_ps(_mm_set1_ps(PME_ORDER-j), dr), data[PME_ORDER-j-1])));
-        data[0] = _mm_mul_ps(_mm_mul_ps(scale, _mm_sub_ps(one, dr)), data[0]);
+            data[PME_ORDER-j-1] = scale*((dr+j)*data[PME_ORDER-j-2]+(fvec4(PME_ORDER-j)-dr)*data[PME_ORDER-j-1]);
+        data[0] = scale*(one-dr)*data[0];
         
         // Spread the charges.
         
-        int gridIndexX = _mm_extract_epi32(gridIndex, 0);
-        int gridIndexY = _mm_extract_epi32(gridIndex, 1);
-        int gridIndexZ = _mm_extract_epi32(gridIndex, 2);
+        int gridIndexX = gridIndex[0];
+        int gridIndexY = gridIndex[1];
+        int gridIndexZ = gridIndex[2];
         if (gridIndexX < 0)
             return; // This happens when a simulation blows up and coordinates become NaN.
         int zindex[PME_ORDER];
@@ -101,21 +99,21 @@ static void spreadCharge(int start, int end, float* posq, float* grid, int gridx
             zindex[j] -= (zindex[j] >= gridz ? gridz : 0);
         }
         float charge = epsilonFactor*posq[4*i+3];
-        __m128 zdata0to3 = _mm_set_ps(EXTRACT_FLOAT(data[3], 2), EXTRACT_FLOAT(data[2], 2), EXTRACT_FLOAT(data[1], 2), EXTRACT_FLOAT(data[0], 2));
-        float zdata4 = EXTRACT_FLOAT(data[4], 2);
+        fvec4 zdata0to3(data[0][2], data[1][2], data[2][2], data[3][2]);
+        float zdata4 = data[4][2];
         if (gridIndexZ+4 < gridz) {
             for (int ix = 0; ix < PME_ORDER; ix++) {
                 int xbase = gridIndexX+ix;
                 xbase -= (xbase >= gridx ? gridx : 0);
                 xbase = xbase*gridy*gridz;
-                float xdata = charge*EXTRACT_FLOAT(data[ix], 0);
+                float xdata = charge*data[ix][0];
                 for (int iy = 0; iy < PME_ORDER; iy++) {
                     int ybase = gridIndexY+iy;
                     ybase -= (ybase >= gridy ? gridy : 0);
                     ybase = xbase + ybase*gridz;
-                    float multiplier = xdata*EXTRACT_FLOAT(data[iy], 1);
-                    __m128 add0to3 = _mm_mul_ps(zdata0to3, _mm_set1_ps(multiplier));
-                    _mm_storeu_ps(&grid[ybase+gridIndexZ], _mm_add_ps(_mm_loadu_ps(&grid[ybase+gridIndexZ]), add0to3));
+                    float multiplier = xdata*data[iy][1];
+                    fvec4 add0to3 = zdata0to3*multiplier;
+                    (fvec4(&grid[ybase+gridIndexZ])+add0to3).store(&grid[ybase+gridIndexZ]);
                     grid[ybase+zindex[4]] += multiplier*zdata4;
                 }
             }
@@ -125,14 +123,14 @@ static void spreadCharge(int start, int end, float* posq, float* grid, int gridx
                 int xbase = gridIndexX+ix;
                 xbase -= (xbase >= gridx ? gridx : 0);
                 xbase = xbase*gridy*gridz;
-                float xdata = charge*EXTRACT_FLOAT(data[ix], 0);
+                float xdata = charge*data[ix][0];
                 for (int iy = 0; iy < PME_ORDER; iy++) {
                     int ybase = gridIndexY+iy;
                     ybase -= (ybase >= gridy ? gridy : 0);
                     ybase = xbase + ybase*gridz;
-                    float multiplier = xdata*EXTRACT_FLOAT(data[iy], 1);
-                    __m128 add0to3 = _mm_mul_ps(zdata0to3, _mm_set1_ps(multiplier));
-                    _mm_store_ps(temp, add0to3);
+                    float multiplier = xdata*data[iy][1];
+                    fvec4 add0to3 = zdata0to3*multiplier;
+                    add0to3.store(temp);
                     grid[ybase+zindex[0]] += temp[0];
                     grid[ybase+zindex[1]] += temp[1];
                     grid[ybase+zindex[2]] += temp[2];
@@ -245,51 +243,51 @@ static void reciprocalConvolution(int start, int end, fftwf_complex* grid, int g
 }
 
 static void interpolateForces(int start, int end, float* posq, float* force, float* grid, int gridx, int gridy, int gridz, int numParticles, Vec3 periodicBoxSize) {
-    __m128 boxSize = _mm_set_ps(0, (float) periodicBoxSize[2], (float) periodicBoxSize[1], (float) periodicBoxSize[0]);
-    __m128 invBoxSize = _mm_set_ps(0, (float) (1/periodicBoxSize[2]), (float) (1/periodicBoxSize[1]), (float) (1/periodicBoxSize[0]));
-    __m128 gridSize = _mm_set_ps(0, gridz, gridy, gridx);
-    __m128i gridSizeInt = _mm_set_epi32(0, gridz, gridy, gridx);
-    __m128 one  = _mm_set1_ps(1);
-    __m128 scale = _mm_set1_ps(1.0f/(PME_ORDER-1));
+    fvec4 boxSize((float) periodicBoxSize[0], (float) periodicBoxSize[1], (float) periodicBoxSize[2], 0);
+    fvec4 invBoxSize((float) (1/periodicBoxSize[0]), (float) (1/periodicBoxSize[1]), (float) (1/periodicBoxSize[2]), 0);
+    fvec4 gridSize(gridx, gridy, gridz, 0);
+    ivec4 gridSizeInt(gridx, gridy, gridz, 0);
+    fvec4 one(1);
+    fvec4 scale(1.0f/(PME_ORDER-1));
     const float epsilonFactor = sqrt(ONE_4PI_EPS0);
     for (int i = start; i < end; i++) {
         // Find the position relative to the nearest grid point.
         
-        __m128 pos = _mm_loadu_ps(&posq[4*i]);
-        __m128 posFloor = _mm_floor_ps(_mm_mul_ps(pos, invBoxSize));
-        __m128 posInBox = _mm_sub_ps(pos, _mm_mul_ps(boxSize, posFloor));
-        __m128 t = _mm_mul_ps(_mm_mul_ps(posInBox, invBoxSize), gridSize);
-        __m128i ti = _mm_cvttps_epi32(t);
-        __m128 dr = _mm_sub_ps(t, _mm_cvtepi32_ps(ti));
-        __m128i gridIndex = _mm_sub_epi32(ti, _mm_and_si128(gridSizeInt, _mm_cmpeq_epi32(ti, gridSizeInt)));
+        fvec4 pos(&posq[4*i]);
+        fvec4 posFloor = floor(pos*invBoxSize);
+        fvec4 posInBox = pos-boxSize*posFloor;
+        fvec4 t = posInBox*invBoxSize*gridSize;
+        ivec4 ti = t;
+        fvec4 dr = t-ti;
+        ivec4 gridIndex = ti-(gridSizeInt&ti==gridSizeInt);
         
         // Compute the B-spline coefficients.
         
-        __m128 data[PME_ORDER];
-        __m128 ddata[PME_ORDER];
-        data[PME_ORDER-1] = _mm_setzero_ps();
+        fvec4 data[PME_ORDER];
+        fvec4 ddata[PME_ORDER];
+        data[PME_ORDER-1] = 0.0f;
         data[1] = dr;
-        data[0] = _mm_sub_ps(one, dr);
+        data[0] = one-dr;
         for (int j = 3; j < PME_ORDER; j++) {
-            __m128 div = _mm_set1_ps(1.0f/(j-1));
-            data[j-1] = _mm_mul_ps(_mm_mul_ps(div, dr), data[j-2]);
+            fvec4 div(1.0f/(j-1));
+            data[j-1] = div*dr*data[j-2];
             for (int k = 1; k < j-1; k++)
-                data[j-k-1] = _mm_mul_ps(div, _mm_add_ps(_mm_mul_ps(_mm_add_ps(dr, _mm_set1_ps(k)), data[j-k-2]), _mm_mul_ps(_mm_sub_ps(_mm_set1_ps(j-k), dr), data[j-k-1])));
-            data[0] = _mm_mul_ps(_mm_mul_ps(div, _mm_sub_ps(one, dr)), data[0]);
+                data[j-k-1] = div*((dr+k)*data[j-k-2]+(fvec4(j-k)-dr)*data[j-k-1]);
+            data[0] = div*(one-dr)*data[0];
         }
-        ddata[0] = _mm_sub_ps(_mm_set1_ps(0), data[0]);
+        ddata[0] = -data[0];
         for (int j = 1; j < PME_ORDER; j++)
-            ddata[j] = _mm_sub_ps(data[j-1], data[j]);
-        data[PME_ORDER-1] = _mm_mul_ps(_mm_mul_ps(scale, dr), data[PME_ORDER-2]);
+            ddata[j] = data[j-1]-data[j];
+        data[PME_ORDER-1] = scale*dr*data[PME_ORDER-2];
         for (int j = 1; j < (PME_ORDER-1); j++)
-            data[PME_ORDER-j-1] = _mm_mul_ps(scale, _mm_add_ps(_mm_mul_ps(_mm_add_ps(dr, _mm_set1_ps(j)), data[PME_ORDER-j-2]), _mm_mul_ps(_mm_sub_ps(_mm_set1_ps(PME_ORDER-j), dr), data[PME_ORDER-j-1])));
-        data[0] = _mm_mul_ps(_mm_mul_ps(scale, _mm_sub_ps(one, dr)), data[0]);
+            data[PME_ORDER-j-1] = scale*((dr+j)*data[PME_ORDER-j-2]+(fvec4(PME_ORDER-j)-dr)*data[PME_ORDER-j-1]);
+        data[0] = scale*(one-dr)*data[0];
                 
         // Compute the force on this atom.
         
-        int gridIndexX = _mm_extract_epi32(gridIndex, 0);
-        int gridIndexY = _mm_extract_epi32(gridIndex, 1);
-        int gridIndexZ = _mm_extract_epi32(gridIndex, 2);
+        int gridIndexX = gridIndex[0];
+        int gridIndexY = gridIndex[1];
+        int gridIndexZ = gridIndex[2];
         if (gridIndexX < 0)
             return; // This happens when a simulation blows up and coordinates become NaN.
         int zindex[PME_ORDER];
@@ -297,34 +295,34 @@ static void interpolateForces(int start, int end, float* posq, float* force, flo
             zindex[j] = gridIndexZ+j;
             zindex[j] -= (zindex[j] >= gridz ? gridz : 0);
         }
-        __m128 zdata[PME_ORDER];
+        fvec4 zdata[PME_ORDER];
         for (int j = 0; j < PME_ORDER; j++)
-            zdata[j] = _mm_set_ps(0, EXTRACT_FLOAT(ddata[j], 2), EXTRACT_FLOAT(data[j], 2), EXTRACT_FLOAT(data[j], 2));
-        __m128 f = _mm_set1_ps(0);
+            zdata[j] = fvec4(data[j][2], data[j][2], ddata[j][2], 0);
+        fvec4 f = 0.0f;
         for (int ix = 0; ix < PME_ORDER; ix++) {
             int xbase = gridIndexX+ix;
             xbase -= (xbase >= gridx ? gridx : 0);
             xbase = xbase*gridy*gridz;
-            float dx = EXTRACT_FLOAT(data[ix], 0);
-            float ddx = EXTRACT_FLOAT(ddata[ix], 0);
-            __m128 xdata = _mm_set_ps(0, dx, dx, ddx);
+            float dx = data[ix][0];
+            float ddx = ddata[ix][0];
+            fvec4 xdata(ddx, dx, dx, 0);
 
             for (int iy = 0; iy < PME_ORDER; iy++) {
                 int ybase = gridIndexY+iy;
                 ybase -= (ybase >= gridy ? gridy : 0);
                 ybase = xbase + ybase*gridz;
-                float dy = EXTRACT_FLOAT(data[iy], 1);
-                float ddy = EXTRACT_FLOAT(ddata[iy], 1);
-                __m128 xydata = _mm_mul_ps(xdata, _mm_set_ps(0, dy, ddy, dy));
+                float dy = data[iy][1];
+                float ddy = ddata[iy][1];
+                fvec4 xydata = xdata*fvec4(dy, ddy, dy, 0);
 
                 for (int iz = 0; iz < PME_ORDER; iz++) {
-                    __m128 gridValue = _mm_set1_ps(grid[ybase+zindex[iz]]);
-                    f = _mm_add_ps(f, _mm_mul_ps(xydata, _mm_mul_ps(zdata[iz], gridValue)));
+                    fvec4 gridValue(grid[ybase+zindex[iz]]);
+                    f = f+xydata*zdata[iz]*gridValue;
                 }
             }
         }
-        f = _mm_mul_ps(invBoxSize, _mm_mul_ps(gridSize, _mm_mul_ps(f, _mm_set1_ps(-epsilonFactor*posq[4*i+3]))));
-        _mm_store_ps(&force[4*i], f);        
+        f = invBoxSize*gridSize*f*(-epsilonFactor*posq[4*i+3]);
+        f.store(&force[4*i]);
     }
 }
 
@@ -509,10 +507,10 @@ void CpuCalcPmeReciprocalForceKernel::runThread(int index) {
             threadWait();
             int numGrids = threadData.size();
             for (int i = gridStart; i < gridEnd; i += 4) {
-                __m128 sum = _mm_load_ps(&realGrid[i]);
+                fvec4 sum(&realGrid[i]);
                 for (int j = 1; j < numGrids; j++)
-                    sum = _mm_add_ps(sum, _mm_load_ps(&threadData[j]->tempGrid[i]));
-                _mm_store_ps(&realGrid[i], sum);
+                    sum += fvec4(&threadData[j]->tempGrid[i]);
+                sum.store(&realGrid[i]);
             }
             threadWait();
             if (lastBoxSize != periodicBoxSize) {