Repository: OceanParcels/parcels
Pull request #499 (2018-12-04 08:09:08)

Cgrid full interpolation


Patch:
diff --git a/parcels/field.py b/parcels/field.py
index ec1382dc..1f6f4c5f 100644
--- a/parcels/field.py
+++ b/parcels/field.py
@@ -2,12 +2,12 @@ from parcels.tools.loggers import logger
 from parcels.tools.converters import unitconverters_map, UnitConverter, Geographic, GeographicPolar
 from parcels.tools.converters import TimeConverter
 from parcels.tools.error import FieldSamplingError, TimeExtrapolationError
+import parcels.tools.interpolation_utils as i_u
 from collections import Iterable
 from py import path
 import numpy as np
 from ctypes import Structure, c_int, c_float, POINTER, pointer
 import xarray as xr
-from math import cos
 import datetime
 import math
 from .grid import (RectilinearZGrid, RectilinearSGrid, CurvilinearZGrid,
@@ -628,8 +628,8 @@ class Field(object):
             raise RuntimeError(self.interp_method+" is not implemented for 2D grids")
 
     def interpolator3D(self, ti, z, y, x, time):
-        xi = int(self.grid.xdim / 2)
-        yi = int(self.grid.ydim / 2)
+        xi = int(self.grid.xdim / 2) - 1
+        yi = int(self.grid.ydim / 2) - 1
         (xsi, eta, zeta, xi, yi, zi) = self.search_indices(x, y, z, xi, yi, ti, time)
         if self.interp_method is 'nearest':
             xii = xi if xsi <= .5 else xi+1
@@ -638,8 +638,8 @@ class Field(object):
             return self.data[ti, zii, yii, xii]
         elif self.interp_method is 'cgrid_velocity':
             # evaluating W velocity in c_grid
-            f0 = self.data[ti, zi, yi, xi]
-            f1 = self.data[ti, zi+1, yi, xi]
+            f0 = self.data[ti, zi, yi+1, xi+1]
+            f1 = self.data[ti, zi+1, yi+1, xi+1]
             return (1-zeta) * f0 + zeta * f1
         elif self.interp_method is 'linear':
             data = self.data[ti, zi, :, :]
@@ -654,7 +654,7 @@ class Field(object):
                 (1-xsi)*eta * data[yi+1, xi]
             return (1-zeta) * f0 + zeta * f1
         elif self.interp_method is 'cgrid_tracer':
-            return self.data[ti, zi+1, yi+1, xi+1]
+            return self.data[ti, zi, yi+1, xi+1]
         else:
             raise RuntimeError(self.interp_method+" is not implemented for 3D grids")
 
@@ -965,17 +965,11 @@ class VectorField(object):
             if W:
                 assert self.W.interp_method == 'cgrid_velocity'
 
-    def dist(self, lon1, lon2, lat1, lat2, mesh):
+    def dist(self, lon1, lon2, lat1, lat2, mesh, lat):
         if mesh == 'spherical':
-            r = 360*60*1852/2/np.pi
             rad = np.pi/180.
-            x1 = r*np.cos(rad*lon1) * np.cos(rad*lat1)
-            y1 = r*np.sin(rad*lon1) * np.cos(rad*lat1)
-            z1 = r*np.sin(rad*lat1)
-            x2 = r*np.cos(rad*lon2) * np.cos(rad*lat2)
-            y2 = r*np.sin(rad*lon2) * np.cos(rad*lat2)
-            z2 = r*np.sin(rad*lat2)
-            return np.sqrt((x2-x1)**2 + (y2-y1)**2 + (z2-z1)**2)
+            deg2m = 1852 * 60.
+            return np.sqrt(((lon2-lon1)*deg2m*math.cos(rad * lat))**2 + ((lat2-lat1)*deg2m)**2)
         else:
             return np.sqrt((lon2-lon1)**2 + (lat2-lat1)**2)
 
@@ -992,8 +986,8 @@ class VectorField(object):
 
     def spatial_c_grid_interpolation2D(self, ti, z, y, x, time):
         grid = self.U.grid
-        xi = int(grid.xdim / 2)
-        yi = int(grid.ydim / 2)
+        xi = int(grid.xdim / 2) - 1
+        yi = int(grid.ydim / 2) - 1
         (xsi, eta, zeta, xi, yi, zi) = self.U.search_indices(x, y, z, xi, yi, ti, time)
 
         if grid.gtype in [GridCode.RectilinearSGrid, GridCode.RectilinearZGrid]:
@@ -1010,17 +1004,17 @@ class VectorField(object):
             px[1:] = np.where(-px[1:] + px[0] > 180, px[1:]+360, px[1:])
         xx = (1-xsi)*(1-eta) * px[0] + xsi*(1-eta) * px[1] + xsi*eta * px[2] + (1-xsi)*eta * px[3]
         assert abs(xx-x) < 1e-4
-        c1 = self.dist(px[0], px[1], py[0], py[1], grid.mesh)
-        c2 = self.dist(px[1], px[2], py[1], py[2], grid.mesh)
-        c3 = self.dist(px[2], px[3], py[2], py[3], grid.mesh)
-        c4 = self.dist(px[3], px[0], py[3], py[0], grid.mesh)
+        c1 = self.dist(px[0], px[1], py[0], py[1], grid.mesh, np.dot(i_u.phi2D_lin(xsi, 0.), py))
+        c2 = self.dist(px[1], px[2], py[1], py[2], grid.mesh, np.dot(i_u.phi2D_lin(1., eta), py))
+        c3 = self.dist(px[2], px[3], py[2], py[3], grid.mesh, np.dot(i_u.phi2D_lin(xsi, 1.), py))
+        c4 = self.dist(px[3], px[0], py[3], py[0], grid.mesh, np.dot(i_u.phi2D_lin(0., eta), py))
         if grid.zdim == 1:
             U0 = self.U.data[ti, yi+1, xi] * c4
             U1 = self.U.data[ti, yi+1, xi+1] * c2
             V0 = self.V.data[ti, yi, xi+1] * c1
             V1 = self.V.data[ti, yi+1, xi+1] * c3
         else:
-            U0 = self.U.data[ti, zi, yi+1, xi] * c4  # zi here??
+            U0 = self.U.data[ti, zi, yi+1, xi] * c4
             U1 = self.U.data[ti, zi, yi+1, xi+1] * c2
             V0 = self.V.data[ti, zi, yi, xi+1] * c1
             V1 = self.V.data[ti, zi, yi+1, xi+1] * c3
@@ -1028,7 +1022,7 @@ class VectorField(object):
         V = (1-eta) * V0 + eta * V1
         rad = np.pi/180.
         deg2m = 1852 * 60.
-        meshJac = (deg2m * deg2m * cos(rad * y)) if grid.mesh == 'spherical' else 1
+        meshJac = (deg2m * deg2m * math.cos(rad * y)) if grid.mesh == 'spherical' else 1
         jac = self.jacobian(xsi, eta, px, py) * meshJac
 
         u = ((-(1-eta) * U - (1-xsi) * V) * px[0]
@@ -1041,6 +1035,107 @@ class VectorField(object):
              + (-eta * U + (1-xsi) * V) * py[3]) / jac
         return (u, v)
 
+    def spatial_c_grid_interpolation3D_full(self, ti, z, y, x, time):
+        grid = self.U.grid
+        xi = int(grid.xdim / 2) - 1
+        yi = int(grid.ydim / 2) - 1
+        (xsi, eta, zet, xi, yi, zi) = self.U.search_indices(x, y, z, xi, yi, ti, time)
+
+        if grid.gtype in [GridCode.RectilinearSGrid, GridCode.RectilinearZGrid]:
+            px = np.array([grid.lon[xi], grid.lon[xi+1], grid.lon[xi+1], grid.lon[xi]])
+            py = np.array([grid.lat[yi], grid.lat[yi], grid.lat[yi+1], grid.lat[yi+1]])
+        else:
+            px = np.array([grid.lon[yi, xi], grid.lon[yi, xi+1], grid.lon[yi+1, xi+1], grid.lon[yi+1, xi]])
+            py = np.array([grid.lat[yi, xi], grid.lat[yi, xi+1], grid.lat[yi+1, xi+1], grid.lat[yi+1, xi]])
+
+        if grid.mesh == 'spherical':
+            px[0] = px[0]+360 if px[0] < x-225 else px[0]
+            px[0] = px[0]-360 if px[0] > x+225 else px[0]
+            px[1:] = np.where(px[1:] - px[0] > 180, px[1:]-360, px[1:])
+            px[1:] = np.where(-px[1:] + px[0] > 180, px[1:]+360, px[1:])
+        xx = (1-xsi)*(1-eta) * px[0] + xsi*(1-eta) * px[1] + xsi*eta * px[2] + (1-xsi)*eta * px[3]
+        assert abs(xx-x) < 1e-4
+
+        px = np.concatenate((px, px))
+        py = np.concatenate((py, py))
+        if grid.z4d:
+            pz = np.array([grid.depth[0, zi, yi, xi], grid.depth[0, zi, yi, xi+1], grid.depth[0, zi, yi+1, xi+1], grid.depth[0, zi, yi+1, xi],
+                           grid.depth[0, zi+1, yi, xi], grid.depth[0, zi+1, yi, xi+1], grid.depth[0, zi+1, yi+1, xi+1], grid.depth[0, zi+1, yi+1, xi]])
+        else:
+            pz = np.array([grid.depth[zi, yi, xi], grid.depth[zi, yi, xi+1], grid.depth[zi, yi+1, xi+1], grid.depth[zi, yi+1, xi],
+                           grid.depth[zi+1, yi, xi], grid.depth[zi+1, yi, xi+1], grid.depth[zi+1, yi+1, xi+1], grid.depth[zi+1, yi+1, xi]])
+
+        u0 = self.U.data[ti, zi, yi+1, xi]
+        u1 = self.U.data[ti, zi, yi+1, xi+1]
+        v0 = self.V.data[ti, zi, yi, xi+1]
+        v1 = self.V.data[ti, zi, yi+1, xi+1]
+        w0 = self.W.data[ti, zi, yi+1, xi+1]
+        w1 = self.W.data[ti, zi+1, yi+1, xi+1]
+
+        U0 = u0 * i_u.jacobian3D_lin_face(px, py, pz, 0, eta, zet, 'zonal', grid.mesh)
+        U1 = u1 * i_u.jacobian3D_lin_face(px, py, pz, 1, eta, zet, 'zonal', grid.mesh)
+        V0 = v0 * i_u.jacobian3D_lin_face(px, py, pz, xsi, 0, zet, 'meridional', grid.mesh)
+        V1 = v1 * i_u.jacobian3D_lin_face(px, py, pz, xsi, 1, zet, 'meridional', grid.mesh)
+        W0 = w0 * i_u.jacobian3D_lin_face(px, py, pz, xsi, eta, 0, 'vertical', grid.mesh)
+        W1 = w1 * i_u.jacobian3D_lin_face(px, py, pz, xsi, eta, 1, 'vertical', grid.mesh)
+
+        # Computing fluxes in half left hexahedron -> flux_u05
+        xx = [px[0], (px[0]+px[1])/2, (px[2]+px[3])/2, px[3], px[4], (px[4]+px[5])/2, (px[6]+px[7])/2, px[7]]
+        yy = [py[0], (py[0]+py[1])/2, (py[2]+py[3])/2, py[3], py[4], (py[4]+py[5])/2, (py[6]+py[7])/2, py[7]]
+        zz = [pz[0], (pz[0]+pz[1])/2, (pz[2]+pz[3])/2, pz[3], pz[4], (pz[4]+pz[5])/2, (pz[6]+pz[7])/2, pz[7]]
+        flux_u0 = u0 * i_u.jacobian3D_lin_face(xx, yy, zz, 0, .5, .5, 'zonal', grid.mesh)
+        flux_v0_halfx = v0 * i_u.jacobian3D_lin_face(xx, yy, zz, .5, 0, .5, 'meridional', grid.mesh)
+        flux_v1_halfx = v1 * i_u.jacobian3D_lin_face(xx, yy, zz, .5, 1, .5, 'meridional', grid.mesh)
+        flux_w0_halfx = w0 * i_u.jacobian3D_lin_face(xx, yy, zz, .5, .5, 0, 'vertical', grid.mesh)
+        flux_w1_halfx = w1 * i_u.jacobian3D_lin_face(xx, yy, zz, .5, .5, 1, 'vertical', grid.mesh)
+        flux_u05 = flux_u0 + flux_v0_halfx - flux_v1_halfx + flux_w0_halfx - flux_w1_halfx
+
+        # Computing fluxes in half front hexahedron -> flux_v05
+        xx = [px[0], px[1], (px[1]+px[2])/2, (px[0]+px[3])/2, px[4], px[5], (px[5]+px[6])/2, (px[4]+px[7])/2]
+        yy = [py[0], py[1], (py[1]+py[2])/2, (py[0]+py[3])/2, py[4], py[5], (py[5]+py[6])/2, (py[4]+py[7])/2]
+        zz = [pz[0], pz[1], (pz[1]+pz[2])/2, (pz[0]+pz[3])/2, pz[4], pz[5], (pz[5]+pz[6])/2, (pz[4]+pz[7])/2]
+        flux_u0_halfy = u0 * i_u.jacobian3D_lin_face(xx, yy, zz, 0, .5, .5, 'zonal', grid.mesh)
+        flux_u1_halfy = u1 * i_u.jacobian3D_lin_face(xx, yy, zz, 1, .5, .5, 'zonal', grid.mesh)
+        flux_v0 = v0 * i_u.jacobian3D_lin_face(xx, yy, zz, .5, 0, .5, 'meridional', grid.mesh)
+        flux_w0_halfy = w0 * i_u.jacobian3D_lin_face(xx, yy, zz, .5, .5, 0, 'vertical', grid.mesh)
+        flux_w1_halfy = w1 * i_u.jacobian3D_lin_face(xx, yy, zz, .5, .5, 1, 'vertical', grid.mesh)
+        flux_v05 = flux_u0_halfy - flux_u1_halfy + flux_v0 + flux_w0_halfy - flux_w1_halfy
+
+        # Computing fluxes in half lower hexahedron -> flux_w05
+        xx = [px[0], px[1], px[2], px[3], (px[0]+px[4])/2, (px[1]+px[5])/2, (px[2]+px[6])/2, (px[3]+px[7])/2]
+        yy = [py[0], py[1], py[2], py[3], (py[0]+py[4])/2, (py[1]+py[5])/2, (py[2]+py[6])/2, (py[3]+py[7])/2]
+        zz = [pz[0], pz[1], pz[2], pz[3], (pz[0]+pz[4])/2, (pz[1]+pz[5])/2, (pz[2]+pz[6])/2, (pz[3]+pz[7])/2]
+        flux_u0_halfz = u0 * i_u.jacobian3D_lin_face(xx, yy, zz, 0, .5, .5, 'zonal', grid.mesh)
+        flux_u1_halfz = u1 * i_u.jacobian3D_lin_face(xx, yy, zz, 1, .5, .5, 'zonal', grid.mesh)
+        flux_v0_halfz = v0 * i_u.jacobian3D_lin_face(xx, yy, zz, .5, 0, .5, 'meridional', grid.mesh)
+        flux_v1_halfz = v1 * i_u.jacobian3D_lin_face(xx, yy, zz, .5, 1, .5, 'meridional', grid.mesh)
+        flux_w0 = w0 * i_u.jacobian3D_lin_face(xx, yy, zz, .5, .5, 0, 'vertical', grid.mesh)
+        flux_w05 = flux_u0_halfz - flux_u1_halfz + flux_v0_halfz - flux_v1_halfz + flux_w0
+
+        surf_u05 = i_u.jacobian3D_lin_face(px, py, pz, .5, .5, .5, 'zonal', grid.mesh)
+        jac_u05 = i_u.jacobian3D_lin_face(px, py, pz, .5, eta, zet, 'zonal', grid.mesh)
+        U05 = flux_u05 / surf_u05 * jac_u05
+
+        surf_v05 = i_u.jacobian3D_lin_face(px, py, pz, .5, .5, .5, 'meridional', grid.mesh)
+        jac_v05 = i_u.jacobian3D_lin_face(px, py, pz, xsi, .5, zet, 'meridional', grid.mesh)
+        V05 = flux_v05 / surf_v05 * jac_v05
+
+        surf_w05 = i_u.jacobian3D_lin_face(px, py, pz, .5, .5, .5, 'vertical', grid.mesh)
+        jac_w05 = i_u.jacobian3D_lin_face(px, py, pz, xsi, eta, .5, 'vertical', grid.mesh)
+        W05 = flux_w05 / surf_w05 * jac_w05
+
+        jac = i_u.jacobian3D_lin(px, py, pz, xsi, eta, zet, grid.mesh)
+        dxsidt = i_u.interpolate(i_u.phi1D_quad, [U0, U05, U1], xsi) / jac
+        detadt = i_u.interpolate(i_u.phi1D_quad, [V0, V05, V1], eta) / jac
+        dzetdt = i_u.interpolate(i_u.phi1D_quad, [W0, W05, W1], zet) / jac
+
+        dphidxsi, dphideta, dphidzet = i_u.dphidxsi3D_lin(xsi, eta, zet)
+
+        u = np.dot(dphidxsi, px) * dxsidt + np.dot(dphideta, px) * detadt + np.dot(dphidzet, px) * dzetdt
+        v = np.dot(dphidxsi, py) * dxsidt + np.dot(dphideta, py) * detadt + np.dot(dphidzet, py) * dzetdt
+        w = np.dot(dphidxsi, pz) * dxsidt + np.dot(dphideta, pz) * detadt + np.dot(dphidzet, pz) * dzetdt
+        return (u, v, w)
+
     def spatial_c_grid_interpolation3D(self, ti, z, y, x, time):
         """
           __ V1 __
@@ -1053,10 +1148,11 @@ class VectorField(object):
         Curvilinear grids are treated properly, since the element is projected to a rectilinear parent element.
         """
         if self.U.grid.gtype in [GridCode.RectilinearSGrid, GridCode.CurvilinearSGrid]:
-            raise NotImplementedError('C staggered grid with a s vertical discretisation are not available')
-        (u, v) = self.spatial_c_grid_interpolation2D(ti, z, y, x, time)
-        w = self.W.eval(time, x, y, z, False)
-        w = self.W.units.to_target(w, x, y, z)
+            (u, v, w) = self.spatial_c_grid_interpolation3D_full(ti, z, y, x, time)
+        else:
+            (u, v) = self.spatial_c_grid_interpolation2D(ti, z, y, x, time)
+            w = self.W.eval(time, x, y, z, False)
+            w = self.W.units.to_target(w, x, y, z)
         return (u, v, w)
 
     def eval(self, time, x, y, z):
diff --git a/parcels/fieldset.py b/parcels/fieldset.py
index b691aca6..b2f5af8b 100644
--- a/parcels/fieldset.py
+++ b/parcels/fieldset.py
@@ -276,6 +276,7 @@ class FieldSet(object):
                To interpolate U, V velocities on the C-grid, Parcels needs to read the f-nodes,
                which are located on the corners of the cells.
                (for indexing details: https://www.nemo-ocean.eu/doc/img360.png )
+               In 3D, the depth is the one corresponding to W nodes
         :param indices: Optional dictionary of indices for each dimension
                to read from file(s), to allow for reading of subset of data.
                Default is to read the full extent of each dimension.
diff --git a/parcels/grid.py b/parcels/grid.py
index a2f53377..d9e56ea9 100644
--- a/parcels/grid.py
+++ b/parcels/grid.py
@@ -4,7 +4,7 @@ import numpy as np
 from ctypes import Structure, c_int, c_float, c_double, POINTER, cast, c_void_p, pointer
 from enum import IntEnum
 
-__all__ = ['GridCode', 'RectilinearZGrid', 'RectilinearSGrid', 'CurvilinearZGrid', 'CurvilinearSGrid', 'CGrid']
+__all__ = ['GridCode', 'RectilinearZGrid', 'RectilinearSGrid', 'CurvilinearZGrid', 'CurvilinearSGrid', 'CGrid', 'Grid']
 
 
 class GridCode(IntEnum):
diff --git a/parcels/include/interpolation_utils.h b/parcels/include/interpolation_utils.h
new file mode 100644
index 00000000..34c75f31
--- /dev/null
+++ b/parcels/include/interpolation_utils.h
@@ -0,0 +1,135 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+
+typedef enum
+  {
+    ZONAL=0, MERIDIONAL=1, VERTICAL=2,
+  } Orientation;
+
+
+static inline void phi2D_lin(double xsi, double eta, double *phi)
+{
+    phi[0] = (1-xsi) * (1-eta);
+    phi[1] =    xsi  * (1-eta);
+    phi[2] =    xsi  *    eta ;
+    phi[3] = (1-xsi) *    eta ;
+}
+
+
+static inline void phi1D_quad(double xsi, double *phi)
+{
+    phi[0] = 2*xsi*xsi-3*xsi+1;
+    phi[1] = -4*xsi*xsi+4*xsi;
+    phi[2] = 2*xsi*xsi-xsi;
+}
+
+
+static inline void dphidxsi3D_lin(double xsi, double eta, double zet, double *dphidxsi, double *dphideta, double *dphidzet)
+{
+  dphidxsi[0] = - (1-eta) * (1-zet);
+  dphidxsi[1] =   (1-eta) * (1-zet);
+  dphidxsi[2] =   (  eta) * (1-zet);
+  dphidxsi[3] = - (  eta) * (1-zet);
+  dphidxsi[4] = - (1-eta) * (  zet);
+  dphidxsi[5] =   (1-eta) * (  zet);
+  dphidxsi[6] =   (  eta) * (  zet);
+  dphidxsi[7] = - (  eta) * (  zet);
+
+  dphideta[0] = - (1-xsi) * (1-zet);
+  dphideta[1] = - (  xsi) * (1-zet);
+  dphideta[2] =   (  xsi) * (1-zet);
+  dphideta[3] =   (1-xsi) * (1-zet);
+  dphideta[4] = - (1-xsi) * (  zet);
+  dphideta[5] = - (  xsi) * (  zet);
+  dphideta[6] =   (  xsi) * (  zet);
+  dphideta[7] =   (1-xsi) * (  zet);
+
+  dphidzet[0] = - (1-xsi) * (1-eta);
+  dphidzet[1] = - (  xsi) * (1-eta);
+  dphidzet[2] = - (  xsi) * (  eta);
+  dphidzet[3] = - (1-xsi) * (  eta);
+  dphidzet[4] =   (1-xsi) * (1-eta);
+  dphidzet[5] =   (  xsi) * (1-eta);
+  dphidzet[6] =   (  xsi) * (  eta);
+  dphidzet[7] =   (1-xsi) * (  eta);
+}
+
+static inline void dxdxsi3D_lin(double *px, double *py, double *pz, double xsi, double eta, double zet, double *jacM, int sphere_mesh)
+{
+  double dphidxsi[8], dphideta[8], dphidzet[8];
+  dphidxsi3D_lin(xsi, eta, zet, dphidxsi, dphideta, dphidzet);
+
+  int i;
+  for(i=0; i<9; ++i)
+      jacM[i] = 0;
+
+  double deg2m = 1852 * 60.;
+  double rad = M_PI / 180.;
+  double lat = (1-xsi) * (1-eta) * py[0]+
+                  xsi  * (1-eta) * py[1]+
+                  xsi  *    eta  * py[2]+
+               (1-xsi) *    eta  * py[3];
+  double jac_lon = (sphere_mesh == 1) ? (deg2m * cos(rad * lat) ) : 1;
+  double jac_lat = (sphere_mesh == 1) ? deg2m : 1;
+
+  for(i=0; i<8; ++i){
+    jacM[3*0+0] += px[i] * dphidxsi[i] * jac_lon; // dxdxsi
+    jacM[3*0+1] += px[i] * dphideta[i] * jac_lon; // dxdeta
+    jacM[3*0+2] += px[i] * dphidzet[i] * jac_lon; // dxdzet
+    jacM[3*1+0] += py[i] * dphidxsi[i] * jac_lat; // dydxsi
+    jacM[3*1+1] += py[i] * dphideta[i] * jac_lat; // dydeta
+    jacM[3*1+2] += py[i] * dphidzet[i] * jac_lat; // dydzet
+    jacM[3*2+0] += pz[i] * dphidxsi[i];           // dzdxsi
+    jacM[3*2+1] += pz[i] * dphideta[i];           // dzdeta
+    jacM[3*2+2] += pz[i] * dphidzet[i];           // dzdzet
+  }
+}
+
+static inline double jacobian3D_lin_face(double *px, double *py, double *pz, double xsi, double eta, double zet, Orientation orientation, int sphere_mesh)
+{
+  double jacM[9];
+  dxdxsi3D_lin(px, py, pz, xsi, eta, zet, jacM, sphere_mesh);
+
+  double j[3];
+
+  if (orientation == ZONAL){
+    j[0] = jacM[3*1+1]*jacM[3*2+2]-jacM[3*1+2]*jacM[3*2+1];
+    j[1] =-jacM[3*0+1]*jacM[3*2+2]+jacM[3*0+2]*jacM[3*2+1];
+    j[2] = jacM[3*0+1]*jacM[3*1+2]-jacM[3*0+2]*jacM[3*1+1];
+  }
+  else if (orientation == MERIDIONAL){
+    j[0] = jacM[3*1+0]*jacM[3*2+2]-jacM[3*1+2]*jacM[3*2+0];
+    j[1] =-jacM[3*0+0]*jacM[3*2+2]+jacM[3*0+2]*jacM[3*2+0];
+    j[2] = jacM[3*0+0]*jacM[3*1+2]-jacM[3*0+2]*jacM[3*1+0];
+  }
+  else if (orientation == VERTICAL){
+    j[0] = jacM[3*1+0]*jacM[3*2+1]-jacM[3*1+1]*jacM[3*2+0];
+    j[1] =-jacM[3*0+0]*jacM[3*2+1]+jacM[3*0+1]*jacM[3*2+0];
+    j[2] = jacM[3*0+0]*jacM[3*1+1]-jacM[3*0+1]*jacM[3*1+0];
+  }
+
+  return sqrt(j[0]*j[0]+j[1]*j[1]+j[2]*j[2]);
+}
+
+static inline double jacobian3D_lin(double *px, double *py, double *pz, double xsi, double eta, double zet, int sphere_mesh)
+{
+  double jacM[9];
+  dxdxsi3D_lin(px, py, pz, xsi, eta, zet, jacM, sphere_mesh);
+
+  double jac = jacM[3*0+0] * (jacM[3*1+1]*jacM[3*2+2] - jacM[3*2+1]*jacM[3*1+2])
+             - jacM[3*0+1] * (jacM[3*1+0]*jacM[3*2+2] - jacM[3*2+0]*jacM[3*1+2])
+             + jacM[3*0+2] * (jacM[3*1+0]*jacM[3*2+1] - jacM[3*2+0]*jacM[3*1+1]);
+
+
+  return jac;
+}
+
+static inline double dot_prod(double *a, double *b, size_t n)
+{
+  double val = 0;
+  int i = 0;
+  for(i=0; i<n; ++i)
+    val += a[i]*b[i];
+  return val;
+}
diff --git a/parcels/include/parcels.h b/parcels/include/parcels.h
index 3368d936..2a2fb060 100644
--- a/parcels/include/parcels.h
+++ b/parcels/include/parcels.h
@@ -9,6 +9,7 @@ extern "C" {
 #include <math.h>
 #include "random.h"
 #include "index_search.h"
+#include "interpolation_utils.h"
 
 #define min(X, Y) (((X) < (Y)) ? (X) : (Y))
 #define max(X, Y) (((X) > (Y)) ? (X) : (Y))
@@ -95,7 +96,7 @@ static inline ErrorCode spatial_interpolation_tracer_c_grid_3D(int xi, int yi, i
                                                         int xdim, int ydim, float **f_data, float *value)
 {
   float (*data)[ydim][xdim] = (float (*)[ydim][xdim]) f_data;
-  *value = data[zi+1][yi+1][xi+1];
+  *value = data[zi][yi+1][xi+1];
   return SUCCESS;
 }
 
@@ -124,7 +125,11 @@ static inline ErrorCode temporal_interpolation_structured_grid(float x, float y,
     double t0 = grid->time[ti[igrid]]; double t1 = grid->time[ti[igrid]+1];
     /* Identify grid cell to sample through local linear search */
     err = search_indices(x, y, z, grid, &xi[igrid], &yi[igrid], &zi[igrid], &xsi, &eta, &zeta, gcode, ti[igrid], time, t0, t1); CHECKERROR(err);
-    if (interp_method == LINEAR){
+    if ((interp_method == LINEAR) || (interp_method == CGRID_VELOCITY)){
+      if (interp_method == CGRID_VELOCITY){ // interpolate w
+        xsi = 1;
+        eta = 1;
+      }
       if (grid->zdim==1){
         err = spatial_interpolation_bilinear(xsi, eta, xi[igrid], yi[igrid], grid->xdim, (float**)(data[ti[igrid]]), &f0); CHECKERROR(err);
         err = spatial_interpolation_bilinear(xsi, eta, xi[igrid], yi[igrid], grid->xdim, (float**)(data[ti[igrid]+1]), &f1); CHECKERROR(err);
@@ -163,7 +168,13 @@ static inline ErrorCode temporal_interpolation_structured_grid(float x, float y,
   } else {
     double t0 = grid->time[ti[igrid]];
     err = search_indices(x, y, z, grid, &xi[igrid], &yi[igrid], &zi[igrid], &xsi, &eta, &zeta, gcode, ti[igrid], t0, t0, t0+1); CHECKERROR(err);
-    if (interp_method == LINEAR){
+    if ((interp_method == LINEAR) || (interp_method == CGRID_VELOCITY)){
+      if (interp_method == CGRID_VELOCITY){ // interpolate w
+        xsi = 1;
+        eta = 1;
+        if (grid->zdim==1)
+          return ERROR;
+      }
       if (grid->zdim==1){
         err = spatial_interpolation_bilinear(xsi, eta, xi[igrid], yi[igrid], grid->xdim, (float**)(data[ti[igrid]]), value); CHECKERROR(err);
       }
@@ -198,18 +209,12 @@ static inline ErrorCode temporal_interpolation_structured_grid(float x, float y,
   }
 }
 
-static double dist(float lon1, float lon2, float lat1, float lat2, int sphere_mesh)
+static double dist(double lon1, double lon2, double lat1, double lat2, int sphere_mesh, double lat)
 {
   if (sphere_mesh == 1){
-    double r = 360*60*1852 / (2*M_PI);
     double rad = M_PI / 180.;
-    double x1 = r*cos(rad*lon1) * cos(rad*lat1);
-    double y1 = r*sin(rad*lon1) * cos(rad*lat1);
-    double z1 = r*sin(rad*lat1);
-    double x2 = r*cos(rad*lon2) * cos(rad*lat2);
-    double y2 = r*sin(rad*lon2) * cos(rad*lat2);
-    double z2 = r*sin(rad*lat2);
-    return sqrt((x2-x1)*(x2-x1) + (y2-y1)*(y2-y1) + (z2-z1)*(z2-z1));
+    double deg2m = 1852 * 60.;
+    return sqrt((lon2-lon1)*(lon2-lon1) * deg2m * deg2m * cos(rad * lat) * cos(rad * lat) + (lat2-lat1)*(lat2-lat1) * deg2m * deg2m);
   }
   else{
     return sqrt((lon2-lon1)*(lon2-lon1) + (lat2-lat1)*(lat2-lat1));
@@ -225,8 +230,8 @@ static inline ErrorCode spatial_interpolation_UV_c_grid(double xsi, double eta,
   float (*dataU)[xdim] = (float (*)[xdim]) u_data;
   float (*dataV)[xdim] = (float (*)[xdim]) v_data;
 
-  float xgrid_loc[4];
-  float ygrid_loc[4];
+  double xgrid_loc[4];
+  double ygrid_loc[4];
   int iN;
   if( (gcode == RECTILINEAR_Z_GRID) || (gcode == RECTILINEAR_S_GRID) ){
     float *xgrid = grid->lon;
@@ -250,10 +255,16 @@ static inline ErrorCode spatial_interpolation_UV_c_grid(double xsi, double eta,
     if (xgrid_loc[i4] > xgrid_loc[0] + 180) xgrid_loc[i4] -= 360;
   }
 
-  double U0 = dataU[yi+1][xi]   * dist(xgrid_loc[3], xgrid_loc[0], ygrid_loc[3], ygrid_loc[0], grid->sphere_mesh);
-  double U1 = dataU[yi+1][xi+1] * dist(xgrid_loc[1], xgrid_loc[2], ygrid_loc[1], ygrid_loc[2], grid->sphere_mesh);
-  double V0 = dataV[yi][xi+1]   * dist(xgrid_loc[0], xgrid_loc[1], ygrid_loc[0], ygrid_loc[1], grid->sphere_mesh);
-  double V1 = dataV[yi+1][xi+1] * dist(xgrid_loc[2], xgrid_loc[3], ygrid_loc[2], ygrid_loc[3], grid->sphere_mesh);
+
+  double phi[4];
+  phi2D_lin(0., eta, phi);
+  double U0 = dataU[yi+1][xi]   * dist(xgrid_loc[3], xgrid_loc[0], ygrid_loc[3], ygrid_loc[0], grid->sphere_mesh, dot_prod(phi, ygrid_loc, 4));
+  phi2D_lin(1., eta, phi);
+  double U1 = dataU[yi+1][xi+1] * dist(xgrid_loc[1], xgrid_loc[2], ygrid_loc[1], ygrid_loc[2], grid->sphere_mesh, dot_prod(phi, ygrid_loc, 4));
+  phi2D_lin(xsi, 0., phi);
+  double V0 = dataV[yi][xi+1]   * dist(xgrid_loc[0], xgrid_loc[1], ygrid_loc[0], ygrid_loc[1], grid->sphere_mesh, dot_prod(phi, ygrid_loc, 4));
+  phi2D_lin(xsi, 1., phi);
+  double V1 = dataV[yi+1][xi+1] * dist(xgrid_loc[2], xgrid_loc[3], ygrid_loc[2], ygrid_loc[3], grid->sphere_mesh, dot_prod(phi, ygrid_loc, 4));
   double U = (1-xsi) * U0 + xsi * U1;
   double V = (1-eta) * V0 + eta * V1;
 
@@ -272,12 +283,9 @@ static inline ErrorCode spatial_interpolation_UV_c_grid(double xsi, double eta,
   if (grid->sphere_mesh == 1){
     double deg2m = 1852 * 60.;
     double rad = M_PI / 180.;
-    double lat = (1-xsi) * (1-eta) * ygrid_loc[0]+
-                    xsi  * (1-eta) * ygrid_loc[1]+
-                    xsi  *    eta  * ygrid_loc[2]+
-                 (1-xsi) *    eta  * ygrid_loc[3];
+    phi2D_lin(xsi, eta, phi);
+    double lat = dot_prod(phi, ygrid_loc, 4);
     meshJac = deg2m * deg2m * cos(rad * lat);
-
   }
   double jac = (dxdxsi*dydeta - dxdeta * dydxsi) * meshJac;
 
@@ -295,7 +303,7 @@ static inline ErrorCode spatial_interpolation_UV_c_grid(double xsi, double eta,
 
 
 
-static inline ErrorCode temporal_interpolation_UV_c_grid(float x, float y, float z, double time, CField *U, CField *V,
+static inline ErrorCode temporal_interpolationUV_c_grid(float x, float y, float z, double time, CField *U, CField *V,
                                                          GridCode gcode, int *xi, int *yi, int *zi, int *ti,
                                                          float *u, float *v)
 {
@@ -343,6 +351,194 @@ static inline ErrorCode temporal_interpolation_UV_c_grid(float x, float y, float
   }
 }
 
+/* Quadratic interpolation routine for 3D C grid */
+static inline ErrorCode spatial_interpolation_UVW_c_grid(double xsi, double eta, double zet, int xi, int yi, int zi, int ti, CStructuredGrid *grid,
+                                                        GridCode gcode, float **u_data, float **v_data, float **w_data, float *u, float *v, float *w)
+{
+  /* Cast data array into data[lat][lon] as per NEMO convention */
+  int xdim = grid->xdim;
+  int ydim = grid->ydim;
+  int zdim = grid->zdim;
+  float (*dataU)[ydim][xdim] = (float (*)[ydim][xdim]) u_data;
+  float (*dataV)[ydim][xdim] = (float (*)[ydim][xdim]) v_data;
+  float (*dataW)[ydim][xdim] = (float (*)[ydim][xdim]) w_data;
+
+  float xgrid_loc[4];
+  float ygrid_loc[4];
+  int iN;
+  if( gcode == RECTILINEAR_S_GRID ){
+    float *xgrid = grid->lon;
+    float *ygrid = grid->lat;
+    for (iN=0; iN < 4; ++iN){
+      xgrid_loc[iN] = xgrid[xi+min(1, (iN%3))];
+      ygrid_loc[iN] = ygrid[yi+iN/2];
+    }
+  }
+  else{
+    float (* xgrid)[xdim] = (float (*)[xdim]) grid->lon;
+    float (* ygrid)[xdim] = (float (*)[xdim]) grid->lat;
+    for (iN=0; iN < 4; ++iN){
+      xgrid_loc[iN] = xgrid[yi+iN/2][xi+min(1, (iN%3))];
+      ygrid_loc[iN] = ygrid[yi+iN/2][xi+min(1, (iN%3))];
+    }
+  }
+  int i4;
+  for (i4 = 1; i4 < 4; ++i4){
+    if (xgrid_loc[i4] < xgrid_loc[0] - 180) xgrid_loc[i4] += 360;
+    if (xgrid_loc[i4] > xgrid_loc[0] + 180) xgrid_loc[i4] -= 360;
+  }
+
+  float u0 = dataU[zi][yi+1][xi];
+  float u1 = dataU[zi][yi+1][xi+1];
+  float v0 = dataV[zi][yi][xi+1];
+  float v1 = dataV[zi][yi+1][xi+1];
+  float w0 = dataW[zi][yi+1][xi+1];
+  float w1 = dataW[zi+1][yi+1][xi+1];
+
+  double px[8] = {xgrid_loc[0], xgrid_loc[1], xgrid_loc[2], xgrid_loc[3],
+                  xgrid_loc[0], xgrid_loc[1], xgrid_loc[2], xgrid_loc[3]};
+  double py[8] = {ygrid_loc[0], ygrid_loc[1], ygrid_loc[2], ygrid_loc[3],
+                  ygrid_loc[0], ygrid_loc[1], ygrid_loc[2], ygrid_loc[3]};
+  double pz[8];
+  if (grid->z4d == 1){
+    float (*zvals)[zdim][ydim][xdim] = (float (*)[zdim][ydim][xdim]) grid->depth;
+    for (iN=0; iN < 4; ++iN){
+      pz[iN] = zvals[ti][zi][yi+iN/2][xi+min(1, (iN%3))];
+      pz[iN+4] = zvals[ti][zi+1][yi+iN/2][xi+min(1, (iN%3))];
+    }
+  }
+  else{
+    float (*zvals)[ydim][xdim] = (float (*)[zdim][ydim][xdim]) grid->depth;
+    for (iN=0; iN < 4; ++iN){
+      pz[iN] = zvals[zi][yi+iN/2][xi+min(1, (iN%3))];
+      pz[iN+4] = zvals[zi+1][yi+iN/2][xi+min(1, (iN%3))];
+    }
+  }
+
+  double U0 = u0 * jacobian3D_lin_face(px, py, pz, 0, eta, zet, ZONAL, grid->sphere_mesh);
+  double U1 = u1 * jacobian3D_lin_face(px, py, pz, 1, eta, zet, ZONAL, grid->sphere_mesh);
+  double V0 = v0 * jacobian3D_lin_face(px, py, pz, xsi, 0, zet, MERIDIONAL, grid->sphere_mesh);
+  double V1 = v1 * jacobian3D_lin_face(px, py, pz, xsi, 1, zet, MERIDIONAL, grid->sphere_mesh);
+  double W0 = w0 * jacobian3D_lin_face(px, py, pz, xsi, eta, 0, VERTICAL, grid->sphere_mesh);
+  double W1 = w1 * jacobian3D_lin_face(px, py, pz, xsi, eta, 1, VERTICAL, grid->sphere_mesh);
+
+  // Computing fluxes in half left hexahedron -> flux_u05
+  double xxu[8] = {px[0], (px[0]+px[1])/2, (px[2]+px[3])/2, px[3], px[4], (px[4]+px[5])/2, (px[6]+px[7])/2, px[7]};
+  double yyu[8] = {py[0], (py[0]+py[1])/2, (py[2]+py[3])/2, py[3], py[4], (py[4]+py[5])/2, (py[6]+py[7])/2, py[7]};
+  double zzu[8] = {pz[0], (pz[0]+pz[1])/2, (pz[2]+pz[3])/2, pz[3], pz[4], (pz[4]+pz[5])/2, (pz[6]+pz[7])/2, pz[7]};
+  double flux_u0 = u0 * jacobian3D_lin_face(xxu, yyu, zzu, 0, .5, .5, ZONAL, grid->sphere_mesh);
+  double flux_v0_halfx = v0 * jacobian3D_lin_face(xxu, yyu, zzu, .5, 0, .5, MERIDIONAL, grid->sphere_mesh);
+  double flux_v1_halfx = v1 * jacobian3D_lin_face(xxu, yyu, zzu, .5, 1, .5, MERIDIONAL, grid->sphere_mesh);
+  double flux_w0_halfx = w0 * jacobian3D_lin_face(xxu, yyu, zzu, .5, .5, 0, VERTICAL, grid->sphere_mesh);
+  double flux_w1_halfx = w1 * jacobian3D_lin_face(xxu, yyu, zzu, .5, .5, 1, VERTICAL, grid->sphere_mesh);
+  double flux_u05 = flux_u0 + flux_v0_halfx - flux_v1_halfx + flux_w0_halfx - flux_w1_halfx;
+
+  // Computing fluxes in half front hexahedron -> flux_v05
+  double xxv[8] = {px[0], px[1], (px[1]+px[2])/2, (px[0]+px[3])/2, px[4], px[5], (px[5]+px[6])/2, (px[4]+px[7])/2};
+  double yyv[8] = {py[0], py[1], (py[1]+py[2])/2, (py[0]+py[3])/2, py[4], py[5], (py[5]+py[6])/2, (py[4]+py[7])/2};
+  double zzv[8] = {pz[0], pz[1], (pz[1]+pz[2])/2, (pz[0]+pz[3])/2, pz[4], pz[5], (pz[5]+pz[6])/2, (pz[4]+pz[7])/2};
+  double flux_u0_halfy = u0 * jacobian3D_lin_face(xxv, yyv, zzv, 0, .5, .5, ZONAL, grid->sphere_mesh);
+  double flux_u1_halfy = u1 * jacobian3D_lin_face(xxv, yyv, zzv, 1, .5, .5, ZONAL, grid->sphere_mesh);
+  double flux_v0 = v0 * jacobian3D_lin_face(xxv, yyv, zzv, .5, 0, .5, MERIDIONAL, grid->sphere_mesh);
+  double flux_w0_halfy = w0 * jacobian3D_lin_face(xxv, yyv, zzv, .5, .5, 0, VERTICAL, grid->sphere_mesh);
+  double flux_w1_halfy = w1 * jacobian3D_lin_face(xxv, yyv, zzv, .5, .5, 1, VERTICAL, grid->sphere_mesh);
+  double flux_v05 = flux_u0_halfy - flux_u1_halfy + flux_v0 + flux_w0_halfy - flux_w1_halfy;
+
+  // Computing fluxes in half lower hexahedron -> flux_w05
+  double xx[8] = {px[0], px[1], px[2], px[3], (px[0]+px[4])/2, (px[1]+px[5])/2, (px[2]+px[6])/2, (px[3]+px[7])/2};
+  double yy[8] = {py[0], py[1], py[2], py[3], (py[0]+py[4])/2, (py[1]+py[5])/2, (py[2]+py[6])/2, (py[3]+py[7])/2};
+  double zz[8] = {pz[0], pz[1], pz[2], pz[3], (pz[0]+pz[4])/2, (pz[1]+pz[5])/2, (pz[2]+pz[6])/2, (pz[3]+pz[7])/2};
+  double flux_u0_halfz = u0 * jacobian3D_lin_face(xx, yy, zz, 0, .5, .5, ZONAL, grid->sphere_mesh);
+  double flux_u1_halfz = u1 * jacobian3D_lin_face(xx, yy, zz, 1, .5, .5, ZONAL, grid->sphere_mesh);
+  double flux_v0_halfz = v0 * jacobian3D_lin_face(xx, yy, zz, .5, 0, .5, MERIDIONAL, grid->sphere_mesh);
+  double flux_v1_halfz = v1 * jacobian3D_lin_face(xx, yy, zz, .5, 1, .5, MERIDIONAL, grid->sphere_mesh);
+  double flux_w0 = w0 * jacobian3D_lin_face(xx, yy, zz, .5, .5, 0, VERTICAL, grid->sphere_mesh);
+  double flux_w05 = flux_u0_halfz - flux_u1_halfz + flux_v0_halfz - flux_v1_halfz + flux_w0;
+
+  double surf_u05 = jacobian3D_lin_face(px, py, pz, .5, .5, .5, ZONAL, grid->sphere_mesh);
+  double jac_u05 = jacobian3D_lin_face(px, py, pz, .5, eta, zet, ZONAL, grid->sphere_mesh);
+  double U05 = flux_u05 / surf_u05 * jac_u05;
+
+  double surf_v05 = jacobian3D_lin_face(px, py, pz, .5, .5, .5, MERIDIONAL, grid->sphere_mesh);
+  double jac_v05 = jacobian3D_lin_face(px, py, pz, xsi, .5, zet, MERIDIONAL, grid->sphere_mesh);
+  double V05 = flux_v05 / surf_v05 * jac_v05;
+
+  double surf_w05 = jacobian3D_lin_face(px, py, pz, .5, .5, .5, VERTICAL, grid->sphere_mesh);
+  double jac_w05 = jacobian3D_lin_face(px, py, pz, xsi, eta, .5, VERTICAL, grid->sphere_mesh);
+  double W05 = flux_w05 / surf_w05 * jac_w05;
+
+  double jac = jacobian3D_lin(px, py, pz, xsi, eta, zet, grid->sphere_mesh);
+
+  double phi[3];
+  phi1D_quad(xsi, phi);
+  double uvec[3] = {U0, U05, U1};
+  double dxsidt = dot_prod(phi, uvec, 3) / jac;
+  phi1D_quad(eta, phi);
+  double vvec[3] = {V0, V05, V1};
+  double detadt = dot_prod(phi, vvec, 3) / jac;
+  phi1D_quad(zet, phi);
+  double wvec[3] = {W0, W05, W1};
+  double dzetdt = dot_prod(phi, wvec, 3) / jac;
+
+  double dphidxsi[8], dphideta[8], dphidzet[8];
+  dphidxsi3D_lin(xsi, eta, zet, dphidxsi, dphideta, dphidzet);
+
+  *u = dot_prod(dphidxsi, px, 8) * dxsidt + dot_prod(dphideta, px, 8) * detadt + dot_prod(dphidzet, px, 8) * dzetdt;
+  *v = dot_prod(dphidxsi, py, 8) * dxsidt + dot_prod(dphideta, py, 8) * detadt + dot_prod(dphidzet, py, 8) * dzetdt;
+  *w = dot_prod(dphidxsi, pz, 8) * dxsidt + dot_prod(dphideta, pz, 8) * detadt + dot_prod(dphidzet, pz, 8) * dzetdt;
+
+  return SUCCESS;
+}
+
+static inline ErrorCode temporal_interpolationUVW_c_grid(float x, float y, float z, double time, CField *U, CField *V, CField *W,
+                                                         GridCode gcode, int *xi, int *yi, int *zi, int *ti,
+                                                         float *u, float *v, float *w)
+{
+  ErrorCode err;
+  CStructuredGrid *grid = U->grid->grid;
+  int igrid = U->igrid;
+
+  /* Find time index for temporal interpolation */
+  if (U->time_periodic == 0 && U->allow_time_extrapolation == 0 && (time < grid->time[0] || time > grid->time[grid->tdim-1])){
+    return ERROR_TIME_EXTRAPOLATION;
+  }
+  err = search_time_index(&time, grid->tdim, grid->time, &ti[igrid], U->time_periodic); CHECKERROR(err);
+
+  /* Cast data array intp data[time][depth][lat][lon] as per NEMO convention */
+  float (*dataU)[U->zdim][U->ydim][U->xdim] = (float (*)[U->zdim][U->ydim][U->xdim]) U->data;
+  float (*dataV)[V->zdim][V->ydim][V->xdim] = (float (*)[V->zdim][V->ydim][V->xdim]) V->data;
+  float (*dataW)[W->zdim][W->ydim][W->xdim] = (float (*)[W->zdim][W->ydim][W->xdim]) W->data;
+  double xsi, eta, zet;
+
+
+  if (ti[igrid] < grid->tdim-1 && time > grid->time[ti[igrid]]) {
+    float u0, u1, v0, v1, w0, w1;
+    double t0 = grid->time[ti[igrid]]; double t1 = grid->time[ti[igrid]+1];
+    /* Identify grid cell to sample through local linear search */
+    err = search_indices(x, y, z, grid, &xi[igrid], &yi[igrid], &zi[igrid], &xsi, &eta, &zet, gcode, ti[igrid], time, t0, t1); CHECKERROR(err);
+    if (grid->zdim==1){
+      return ERROR;
+    } else {
+      err = spatial_interpolation_UVW_c_grid(xsi, eta, zet, xi[igrid], yi[igrid], zi[igrid], ti[igrid], grid, gcode, (float**)(dataU[ti[igrid]])  , (float**)(dataV[ti[igrid]]),   (float**)(dataW[ti[igrid]]),   &u0, &v0, &w0); CHECKERROR(err);
+      err = spatial_interpolation_UVW_c_grid(xsi, eta, zet, xi[igrid], yi[igrid], zi[igrid], ti[igrid], grid, gcode, (float**)(dataU[ti[igrid]+1]), (float**)(dataV[ti[igrid]+1]), (float**)(dataW[ti[igrid]+1]), &u1, &v1, &w1); CHECKERROR(err);
+    }
+    *u = u0 + (u1 - u0) * (float)((time - t0) / (t1 - t0));
+    *v = v0 + (v1 - v0) * (float)((time - t0) / (t1 - t0));
+    *w = w0 + (w1 - w0) * (float)((time - t0) / (t1 - t0));
+    return SUCCESS;
+  } else {
+    double t0 = grid->time[ti[igrid]];
+    err = search_indices(x, y, z, grid, &xi[igrid], &yi[igrid], &zi[igrid], &xsi, &eta, &zet, gcode, ti[igrid], t0, t0, t0+1); CHECKERROR(err);
+    if (grid->zdim==1){
+      return ERROR;
+    }
+    else{
+      err = spatial_interpolation_UVW_c_grid(xsi, eta, zet, xi[igrid], yi[igrid], zi[igrid], ti[igrid], grid, gcode, (float**)(dataU[ti[igrid]]), (float**)(dataV[ti[igrid]]), (float**)(dataW[ti[igrid]]), u, v, w); CHECKERROR(err);
+    }
+    return SUCCESS;
+  }
+}
+
 
 static inline ErrorCode temporal_interpolation(float x, float y, float z, double time, CField *f, 
                                                void *vxi, void *vyi, void *vzi, void *vti,
@@ -376,7 +572,7 @@ static inline ErrorCode temporal_interpolationUV(float x, float y, float z, doub
     int *yi = (int *) vyi;
     int *zi = (int *) vzi;
     int *ti = (int *) vti;
-    err = temporal_interpolation_UV_c_grid(x, y, z, time, U, V, gcode, xi, yi, zi, ti, valueU, valueV); CHECKERROR(err);
+    err = temporal_interpolationUV_c_grid(x, y, z, time, U, V, gcode, xi, yi, zi, ti, valueU, valueV); CHECKERROR(err);
     return SUCCESS;
   }
   else{
@@ -396,11 +592,15 @@ static inline ErrorCode temporal_interpolationUVW(float x, float y, float z, dou
     CGrid *_grid = U->grid;
     GridCode gcode = _grid->gtype;
     if (gcode == RECTILINEAR_S_GRID || gcode == CURVILINEAR_S_GRID){
-      printf("C staggered grid with a s vertical discretisation are not available\n");
-      return ERROR;
+      int *xi = (int *) vxi;
+      int *yi = (int *) vyi;
+      int *zi = (int *) vzi;
+      int *ti = (int *) vti;
+      err = temporal_interpolationUVW_c_grid(x, y, z, time, U, V, W, gcode, xi, yi, zi, ti, valueU, valueV, valueW); CHECKERROR(err);
+      return SUCCESS;
     }
   }
-  temporal_interpolationUV(x, y, z, time, U, V, vxi, vyi, vzi, vti, valueU, valueV, interp_method);
+  err = temporal_interpolationUV(x, y, z, time, U, V, vxi, vyi, vzi, vti, valueU, valueV, interp_method); CHECKERROR(err);
   err = temporal_interpolation(x, y, z, time, W, vxi, vyi, vzi, vti, valueW, interp_method); CHECKERROR(err);
   return SUCCESS;
 }
diff --git a/parcels/tools/__init__.py b/parcels/tools/__init__.py
index 3bec7c70..53945e76 100644
--- a/parcels/tools/__init__.py
+++ b/parcels/tools/__init__.py
@@ -2,3 +2,4 @@ from .timer import *  # noqa
 from .converters import *  # noqa
 from .loggers import *  # noqa
 from .error import *  # noqa
+from .interpolation_utils import *  # noqa
diff --git a/parcels/tools/interpolation_utils.py b/parcels/tools/interpolation_utils.py
new file mode 100644
index 00000000..5e1b2a10
--- /dev/null
+++ b/parcels/tools/interpolation_utils.py
@@ -0,0 +1,167 @@
+# flake8: noqa
+import numpy as np
+
+
+def phi1D_lin(xsi):
+    phi = [1-xsi,
+           xsi]
+
+    return phi
+
+
+def phi1D_quad(xsi):
+    phi = [2*xsi**2-3*xsi+1,
+           -4*xsi**2+4*xsi,
+           2*xsi**2-xsi]
+
+    return phi
+
+
+def phi2D_lin(xsi, eta):
+    phi = [(1-xsi) * (1-eta),
+              xsi  * (1-eta),
+              xsi  *    eta ,
+           (1-xsi) *    eta ]
+
+    return phi
+
+
+def phi3D_lin(xsi, eta, zet):
+    phi = [(1-xsi) * (1-eta) * (1-zet),
+              xsi  * (1-eta) * (1-zet),
+              xsi  *    eta  * (1-zet),
+           (1-xsi) *    eta  * (1-zet),
+           (1-xsi) * (1-eta) *    zet ,
+              xsi  * (1-eta) *    zet ,
+              xsi  *    eta  *    zet ,
+           (1-xsi) *    eta  *    zet ]
+
+    return phi
+
+
+def dphidxsi3D_lin(xsi, eta, zet):
+    dphidxsi = [ - (1-eta) * (1-zet),
+                   (1-eta) * (1-zet),
+                   (  eta) * (1-zet),
+                 - (  eta) * (1-zet),
+                 - (1-eta) * (  zet),
+                   (1-eta) * (  zet),
+                   (  eta) * (  zet),
+                 - (  eta) * (  zet)]
+    dphideta = [ - (1-xsi) * (1-zet),
+                 - (  xsi) * (1-zet),
+                   (  xsi) * (1-zet),
+                   (1-xsi) * (1-zet),
+                 - (1-xsi) * (  zet),
+                 - (  xsi) * (  zet),
+                   (  xsi) * (  zet),
+                   (1-xsi) * (  zet)]
+    dphidzet = [ - (1-xsi) * (1-eta),
+                 - (  xsi) * (1-eta),
+                 - (  xsi) * (  eta),
+                 - (1-xsi) * (  eta),
+                   (1-xsi) * (1-eta),
+                   (  xsi) * (1-eta),
+                   (  xsi) * (  eta),
+                   (1-xsi) * (  eta)]
+
+    return dphidxsi, dphideta, dphidzet
+
+
+def dxdxsi3D_lin(hexa_x, hexa_y, hexa_z, xsi, eta, zet, mesh):
+    dphidxsi, dphideta, dphidzet = dphidxsi3D_lin(xsi, eta, zet)
+
+    if mesh is 'spherical':
+        deg2m = 1852 * 60.
+        rad = np.pi / 180.
+        lat = (1-xsi) * (1-eta) * hexa_y[0] + \
+                 xsi  * (1-eta) * hexa_y[1] + \
+                 xsi  *    eta  * hexa_y[2] + \
+              (1-xsi) *    eta  * hexa_y[3]
+        jac_lon = deg2m * np.cos(rad * lat)
+        jac_lat = deg2m
+    else:
+        jac_lon = 1
+        jac_lat = 1
+
+    dxdxsi = np.dot(hexa_x, dphidxsi) * jac_lon
+    dxdeta = np.dot(hexa_x, dphideta) * jac_lon
+    dxdzet = np.dot(hexa_x, dphidzet) * jac_lon
+    dydxsi = np.dot(hexa_y, dphidxsi) * jac_lat
+    dydeta = np.dot(hexa_y, dphideta) * jac_lat
+    dydzet = np.dot(hexa_y, dphidzet) * jac_lat
+    dzdxsi = np.dot(hexa_z, dphidxsi)
+    dzdeta = np.dot(hexa_z, dphideta)
+    dzdzet = np.dot(hexa_z, dphidzet)
+
+    return dxdxsi, dxdeta, dxdzet, dydxsi, dydeta, dydzet, dzdxsi, dzdeta, dzdzet
+
+
+def jacobian3D_lin(hexa_x, hexa_y, hexa_z, xsi, eta, zet, mesh):
+    dxdxsi, dxdeta, dxdzet, dydxsi, dydeta, dydzet, dzdxsi, dzdeta, dzdzet = dxdxsi3D_lin(hexa_x, hexa_y, hexa_z, xsi, eta, zet, mesh)
+
+    jac = dxdxsi * (dydeta*dzdzet - dzdeta*dydzet)\
+        - dxdeta * (dydxsi*dzdzet - dzdxsi*dydzet)\
+        + dxdzet * (dydxsi*dzdeta - dzdxsi*dydeta)
+    return jac
+
+
+def jacobian3D_lin_face(hexa_x, hexa_y, hexa_z, xsi, eta, zet, orientation, mesh):
+    dxdxsi, dxdeta, dxdzet, dydxsi, dydeta, dydzet, dzdxsi, dzdeta, dzdzet = dxdxsi3D_lin(hexa_x, hexa_y, hexa_z, xsi, eta, zet, mesh)
+
+    if orientation is 'zonal':
+        j = [dydeta*dzdzet-dydzet*dzdeta,
+            -dxdeta*dzdzet+dxdzet*dzdeta,
+             dxdeta*dydzet-dxdzet*dydeta]
+    elif orientation is 'meridional':
+        j = [dydxsi*dzdzet-dydzet*dzdxsi,
+            -dxdxsi*dzdzet+dxdzet*dzdxsi,
+             dxdxsi*dydzet-dxdzet*dydxsi]
+    elif orientation is 'vertical':
+        j = [dydxsi*dzdeta-dydeta*dzdxsi,
+            -dxdxsi*dzdeta+dxdeta*dzdxsi,
+             dxdxsi*dydeta-dxdeta*dydxsi]
+
+    jac = np.sqrt(j[0]**2+j[1]**2+j[2]**2)
+    return jac
+
+
+def dphidxsi2D_lin(xsi, eta):
+    dphidxsi = [-(1-eta),
+                  1-eta,
+                    eta,
+                -   eta]
+    dphideta = [-(1-xsi),
+                -   xsi,
+                    xsi,
+                  1-xsi]
+
+    return dphidxsi, dphideta
+
+
+def dxdxsi2D_lin(quad_x, quad_y, xsi, eta,):
+    dphidxsi, dphideta = dphidxsi2D_lin(xsi, eta)
+
+    dxdxsi = np.dot(quad_x, dphidxsi)
+    dxdeta = np.dot(quad_x, dphideta)
+    dydxsi = np.dot(quad_y, dphidxsi)
+    dydeta = np.dot(quad_y, dphideta)
+
+    return dxdxsi, dxdeta, dydxsi, dydeta
+
+
+def jacobian2D_lin(quad_x, quad_y, xsi, eta):
+    dxdxsi, dxdeta, dydxsi, dydeta = dxdxsi2D_lin(quad_x, quad_y, xsi, eta)
+
+    jac = dxdxsi*dydeta - dxdeta*dydxsi
+    return jac
+
+
+def length2d_lin_edge(quad_x, quad_y, ids):
+    xe = [quad_x[ids[0]], quad_x[ids[1]]]
+    ye = [quad_y[ids[0]], quad_y[ids[1]]]
+    return np.sqrt((xe[1]-xe[0])**2+(ye[1]-ye[0])**2)
+
+
+def interpolate(phi, f, xsi):
+    return np.dot(phi(xsi), f)


Test patch:
diff --git a/tests/test_grids.py b/tests/test_grids.py
index 041a60e2..be692a8b 100644
--- a/tests/test_grids.py
+++ b/tests/test_grids.py
@@ -1,6 +1,7 @@
 from parcels import FieldSet, Field, ParticleSet, ScipyParticle, JITParticle, Variable, AdvectionRK4, AdvectionRK4_3D
 from parcels import RectilinearZGrid, RectilinearSGrid, CurvilinearZGrid
 import numpy as np
+import xarray as xr
 import math
 import pytest
 from os import path
@@ -385,3 +386,158 @@ def test_advect_nemo(mode):
     pset = ParticleSet.from_list(field_set, ptype[mode], lon=[lonp], lat=[latp])
     pset.execute(AdvectionRK4, runtime=delta(days=2), dt=delta(hours=6))
     assert abs(pset[0].lat - latp) < 1e-3
+
+
+@pytest.mark.parametrize('mode', ['scipy', 'jit'])
+@pytest.mark.parametrize('time', [True, False])
+def test_cgrid_uniform_2dvel(mode, time):
+    lon = np.array([[0, 2], [.4, 1.5]])
+    lat = np.array([[0, -.5], [.8, .5]])
+    U = np.array([[-99, -99], [4.4721359549995793e-01, 1.3416407864998738e+00]])
+    V = np.array([[-99, 1.2126781251816650e+00], [-99, 1.2278812270298409e+00]])
+
+    if time:
+        U = np.stack((U, U))
+        V = np.stack((V, V))
+        dimensions = {'lat': lat, 'lon': lon, 'time': np.array([0, 10])}
+    else:
+        dimensions = {'lat': lat, 'lon': lon}
+    data = {'U': np.array(U, dtype=np.float32), 'V': np.array(V, dtype=np.float32)}
+    fieldset = FieldSet.from_data(data, dimensions, mesh='flat')
+    fieldset.U.interp_method = 'cgrid_velocity'
+    fieldset.V.interp_method = 'cgrid_velocity'
+
+    def sampleVel(particle, fieldset, time, dt):
+        (particle.zonal, particle.meridional) = fieldset.UV[time, particle.lon, particle.lat, particle.depth]
+
+    class MyParticle(ptype[mode]):
+        zonal = Variable('zonal', dtype=np.float32, initial=0.)
+        meridional = Variable('meridional', dtype=np.float32, initial=0.)
+
+    pset = ParticleSet.from_list(fieldset, MyParticle, lon=.7, lat=.3)
+    pset.execute(pset.Kernel(sampleVel), runtime=0, dt=0)
+    assert abs(pset[0].zonal - 1) < 1e-6
+    assert abs(pset[0].meridional - 1) < 1e-6
+
+
+@pytest.mark.parametrize('mode', ['scipy', 'jit'])
+@pytest.mark.parametrize('vert_mode', ['zlev', 'slev1', 'slev2'])
+@pytest.mark.parametrize('time', [True, False])
+def test_cgrid_uniform_3dvel(mode, vert_mode, time):
+
+    lon = np.array([[0, 2], [.4, 1.5]])
+    lat = np.array([[0, -.5], [.8, .5]])
+
+    u0 = 4.4721359549995793e-01
+    u1 = 1.3416407864998738e+00
+    v0 = 1.2126781251816650e+00
+    v1 = 1.2278812270298409e+00
+    w0 = 1
+    w1 = 1
+
+    if vert_mode == 'zlev':
+        depth = np.array([0, 1])
+    elif vert_mode == 'slev1':
+        depth = np.array([[[0, 0], [0, 0]], [[1, 1], [1, 1]]])
+    elif vert_mode == 'slev2':
+        depth = np.array([[[-1, -.6], [-1.1257142857142859, -.9]],
+                          [[1, 1.5], [0.50857142857142845, .8]]])
+        w0 = 1.0483007922296661e+00
+        w1 = 1.3098951476312375e+00
+
+    U = np.array([[[-99, -99], [u0, u1]],
+                  [[-99, -99], [-99, -99]]])
+    V = np.array([[[-99, v0], [-99, v1]],
+                  [[-99, -99], [-99, -99]]])
+    W = np.array([[[-99, -99], [-99, w0]],
+                  [[-99, -99], [-99, w1]]])
+
+    if time:
+        U = np.stack((U, U))
+        V = np.stack((V, V))
+        W = np.stack((W, W))
+        dimensions = {'lat': lat, 'lon': lon, 'depth': depth, 'time': np.array([0, 10])}
+    else:
+        dimensions = {'lat': lat, 'lon': lon, 'depth': depth}
+    data = {'U': np.array(U, dtype=np.float32),
+            'V': np.array(V, dtype=np.float32),
+            'W': np.array(W, dtype=np.float32)}
+    fieldset = FieldSet.from_data(data, dimensions, mesh='flat')
+    fieldset.U.interp_method = 'cgrid_velocity'
+    fieldset.V.interp_method = 'cgrid_velocity'
+    fieldset.W.interp_method = 'cgrid_velocity'
+
+    def sampleVel(particle, fieldset, time, dt):
+        (particle.zonal, particle.meridional, particle.vertical) = fieldset.UVW[time, particle.lon, particle.lat, particle.depth]
+
+    class MyParticle(ptype[mode]):
+        zonal = Variable('zonal', dtype=np.float32, initial=0.)
+        meridional = Variable('meridional', dtype=np.float32, initial=0.)
+        vertical = Variable('vertical', dtype=np.float32, initial=0.)
+
+    pset = ParticleSet.from_list(fieldset, MyParticle, lon=.7, lat=.3, depth=.2)
+    pset.execute(pset.Kernel(sampleVel), runtime=0, dt=0)
+    assert abs(pset[0].zonal - 1) < 1e-6
+    assert abs(pset[0].meridional - 1) < 1e-6
+    assert abs(pset[0].vertical - 1) < 1e-6
+
+
+@pytest.mark.parametrize('mode', ['scipy', 'jit'])
+@pytest.mark.parametrize('vert_mode', ['zlev', 'slev1'])
+@pytest.mark.parametrize('time', [True, False])
+def test_cgrid_uniform_3dvel_spherical(mode, vert_mode, time):
+    data_path = path.join(path.dirname(__file__), 'test_data/')
+    dim_file = xr.open_dataset(data_path + 'mask_nemo_cross_180lon.nc')
+    u_file = xr.open_dataset(data_path + 'Uu_eastward_nemo_cross_180lon.nc')
+    v_file = xr.open_dataset(data_path + 'Vv_eastward_nemo_cross_180lon.nc')
+    j = 4
+    i = 11
+    lon = np.array(dim_file.glamf[0, j:j+2, i:i+2])
+    lat = np.array(dim_file.gphif[0, j:j+2, i:i+2])
+    U = np.array(u_file.U[0, j:j+2, i:i+2])
+    V = np.array(v_file.V[0, j:j+2, i:i+2])
+    trash = np.zeros((2, 2))
+    U = np.stack((U, trash))
+    V = np.stack((V, trash))
+    w0 = 1
+    w1 = 1
+    W = np.array([[[-99, -99], [-99, w0]],
+                  [[-99, -99], [-99, w1]]])
+
+    if vert_mode == 'zlev':
+        depth = np.array([0, 1])
+    elif vert_mode == 'slev1':
+        depth = np.array([[[0, 0], [0, 0]], [[1, 1], [1, 1]]])
+
+    if time:
+        U = np.stack((U, U))
+        V = np.stack((V, V))
+        W = np.stack((W, W))
+        dimensions = {'lat': lat, 'lon': lon, 'depth': depth, 'time': np.array([0, 10])}
+    else:
+        dimensions = {'lat': lat, 'lon': lon, 'depth': depth}
+    data = {'U': np.array(U, dtype=np.float32),
+            'V': np.array(V, dtype=np.float32),
+            'W': np.array(W, dtype=np.float32)}
+    fieldset = FieldSet.from_data(data, dimensions, mesh='spherical')
+    fieldset.U.interp_method = 'cgrid_velocity'
+    fieldset.V.interp_method = 'cgrid_velocity'
+    fieldset.W.interp_method = 'cgrid_velocity'
+
+    def sampleVel(particle, fieldset, time, dt):
+        (particle.zonal, particle.meridional, particle.vertical) = fieldset.UVW[time, particle.lon, particle.lat, particle.depth]
+
+    class MyParticle(ptype[mode]):
+        zonal = Variable('zonal', dtype=np.float32, initial=0.)
+        meridional = Variable('meridional', dtype=np.float32, initial=0.)
+        vertical = Variable('vertical', dtype=np.float32, initial=0.)
+
+    lonp = 179.8
+    latp = 81.35
+    pset = ParticleSet.from_list(fieldset, MyParticle, lon=lonp, lat=latp, depth=.2)
+    pset.execute(pset.Kernel(sampleVel), runtime=0, dt=0)
+    pset[0].zonal = fieldset.U.units.to_source(pset[0].zonal, lonp, latp, 0)
+    pset[0].meridional = fieldset.V.units.to_source(pset[0].meridional, lonp, latp, 0)
+    assert abs(pset[0].zonal - 1) < 1e-3
+    assert abs(pset[0].meridional) < 1e-3
+    assert abs(pset[0].vertical - 1) < 1e-3