Interpolation/src/LinearGridInterpolator3D.cc

0001 #include "LinearGridInterpolator3D.h"
0002 #include "Grid1D.h"
0003 #include "Grid3D.h"
0004 #include "MagneticField/VolumeGeometry/interface/MagExceptions.h"
0005
0006 void LinearGridInterpolator3D::throwGridInterpolator3DException(void) {
0007   throw GridInterpolator3DException(
0008       grida.lower(), gridb.lower(), gridc.lower(), grida.upper(), gridb.upper(), gridc.upper());
0009 }
0010
0011 LinearGridInterpolator3D::ValueType LinearGridInterpolator3D::interpolate(Scalar a, Scalar b, Scalar c) {
0012   /*
0013   int i = grida.index(a);
0014   int j = gridb.index(b);
0015   int k = gridc.index(c);
0016
0017   if (i==-1 || j==-1 || k==-1) {
0018     // point outside of grid validity!
0019     throwGridInterpolator3DException();
0020   }
0021
0022
0023   Scalar s = (a - grida.node(i)) / grida.step();
0024   Scalar t = (b - gridb.node(j)) / gridb.step();
0025   Scalar u = (c - gridc.node(k)) / gridc.step();
0026
0027   */
0028
0029   Scalar s, t, u;
0030   int i = grida.index(a, s);
0031   int j = gridb.index(b, t);
0032   int k = gridc.index(c, u);
0033
0034   // test range??
0035
0036   grida.normalize(i, s);
0037   gridb.normalize(j, t);
0038   gridc.normalize(k, u);
0039
0040 #ifdef DEBUG_LinearGridInterpolator3D
0041   if (InterpolationDebug::debug) {
0042     using std::cout;
0043     using std::endl;
0044     cout << "LinearGridInterpolator3D called with a,b,c " << a << "," << b << "," << c << endl;
0045     cout << " i,j,k = " << i << "," << j << "," << k << " s,t,u = " << s << "," << t << "," << u << endl;
0046     cout << "Node positions for " << i << "," << j << "," << k << " : " << grida.node(i) << "," << gridb.node(j) << ","
0047          << gridc.node(k) << endl;
0048     cout << "Node positions for " << i + 1 << "," << j + 1 << "," << k + 1 << " : " << grida.node(i + 1) << ","
0049          << gridb.node(j + 1) << "," << gridc.node(k + 1) << endl;
0050     cout << "Grid(" << i << "," << j << "," << k << ") = " << grid(i, j, k) << " ";
0051     cout << "Grid(" << i << "," << j << "," << k + 1 << ") = " << grid(i, j, k + 1) << endl;
0052     cout << "Grid(" << i << "," << j + 1 << "," << k << ") = " << grid(i, j + 1, k) << " ";
0053     cout << "Grid(" << i << "," << j + 1 << "," << k + 1 << ") = " << grid(i, j + 1, k + 1) << endl;
0054     cout << "Grid(" << i + 1 << "," << j << "," << k << ") = " << grid(i + 1, j, k) << " ";
0055     cout << "Grid(" << i + 1 << "," << j << "," << k + 1 << ") = " << grid(i + 1, j, k + 1) << endl;
0056     cout << "Grid(" << i + 1 << "," << j + 1 << "," << k << ") = " << grid(i + 1, j + 1, k) << " ";
0057     cout << "Grid(" << i + 1 << "," << j + 1 << "," << k + 1 << ") = " << grid(i + 1, j + 1, k + 1) << endl;
0058     cout << "number of nodes: " << grida.nodes() << "," << gridb.nodes() << "," << gridc.nodes() << endl;
0059
0060     //     cout << (1-s)*(1-t)*(1-u)*grid(i,  j,  k) << " " << (1-s)*(1-t)*u*grid(i,  j,  k+1) << endl
0061     //       << (1-s)*   t *(1-u)*grid(i,  j+1,k) << " " << (1-s)*   t *u*grid(i,  j+1,k+1) << endl
0062     //       << s    *(1-t)*(1-u)*grid(i+1,j,  k) << " " <<  s    *(1-t)*u*grid(i+1,j,  k+1) << endl
0063     //       << s    *   t *(1-u)*grid(i+1,j+1,k) << " " << s    *   t *u*grid(i+1,j+1,k+1) << endl;
0064   }
0065
0066 #endif
0067
0068   int ind = grid.index(i, j, k);
0069   int s1 = grid.stride1();
0070   int s2 = grid.stride2();
0071   int s3 = grid.stride3();
0072   //chances are this is more numerically precise this way
0073
0074   // this code for test to check  properly inline of wrapped math...
0075 #if defined(CMS_TEST_RAWSSE)
0076
0077   __m128 resultSIMD =
0078       _mm_mul_ps(_mm_set1_ps((1.f - s) * (1.f - t) * u), _mm_sub_ps(grid(ind + s3).v.vec, grid(ind).v.vec));
0079   resultSIMD = _mm_add_ps(
0080       resultSIMD,
0081       _mm_mul_ps(_mm_set1_ps((1.f - s) * t * u), _mm_sub_ps(grid(ind + s2 + s3).v.vec, grid(ind + s2).v.vec)));
0082   resultSIMD = _mm_add_ps(
0083       resultSIMD,
0084       _mm_mul_ps(_mm_set1_ps(s * (1.f - t) * u), _mm_sub_ps(grid(ind + s1 + s3).v.vec, grid(ind + s1).v.vec)));
0085   resultSIMD = _mm_add_ps(
0086       resultSIMD,
0087       _mm_mul_ps(_mm_set1_ps(s * t * u), _mm_sub_ps(grid(ind + s1 + s2 + s3).v.vec, grid(ind + s1 + s2).v.vec)));
0088   resultSIMD =
0089       _mm_add_ps(resultSIMD, _mm_mul_ps(_mm_set1_ps((1.f - s) * t), _mm_sub_ps(grid(ind + s2).v.vec, grid(ind).v.vec)));
0090   resultSIMD = _mm_add_ps(resultSIMD,
0091                           _mm_mul_ps(_mm_set1_ps(s * t), _mm_sub_ps(grid(ind + s1 + s2).v.vec, grid(ind + s1).v.vec)));
0092   resultSIMD = _mm_add_ps(resultSIMD, _mm_mul_ps(_mm_set1_ps(s), _mm_sub_ps(grid(ind + s1).v.vec, grid(ind).v.vec)));
0093   resultSIMD = _mm_add_ps(resultSIMD, grid(ind).v.vec);
0094
0095   ValueType result;
0096   result.v = resultSIMD;
0097
0098 #else
0099
0100   ValueType result = ((1.f - s) * (1.f - t) * u) * (grid(ind + s3) - grid(ind));
0101   result = result + ((1.f - s) * t * u) * (grid(ind + s2 + s3) - grid(ind + s2));
0102   result = result + (s * (1.f - t) * u) * (grid(ind + s1 + s3) - grid(ind + s1));
0103   result = result + (s * t * u) * (grid(ind + s1 + s2 + s3) - grid(ind + s1 + s2));
0104   result = result + ((1.f - s) * t) * (grid(ind + s2) - grid(ind));
0105   result = result + (s * t) * (grid(ind + s1 + s2) - grid(ind + s1));
0106   result = result + (s) * (grid(ind + s1) - grid(ind));
0107   result = result + grid(ind);
0108
0109 #endif
0110
0111   //      (1-s)*(1-t)*(1-u)*grid(i,  j,  k) + (1-s)*(1-t)*u*grid(i,  j,  k+1) +
0112   //      (1-s)*   t *(1-u)*grid(i,  j+1,k) + (1-s)*   t *u*grid(i,  j+1,k+1) +
0113   //      s    *(1-t)*(1-u)*grid(i+1,j,  k) + s    *(1-t)*u*grid(i+1,j,  k+1) +
0114   //      s    *   t *(1-u)*grid(i+1,j+1,k) + s    *   t *u*grid(i+1,j+1,k+1);
0115
0116   return result;
0117 }