LinearGridInterpolator3D.cc

Go to the documentation of this file.

#include "LinearGridInterpolator3D.h"
#include "Grid1D.h"
#include "Grid3D.h"
#include "MagneticField/VolumeGeometry/interface/MagExceptions.h"

void
LinearGridInterpolator3D::throwGridInterpolator3DException(void)
{
  throw GridInterpolator3DException(grida.lower(),gridb.lower(),gridc.lower(),
                                    grida.upper(),gridb.upper(),gridc.upper());
}

LinearGridInterpolator3D::ValueType 
LinearGridInterpolator3D::interpolate( Scalar a, Scalar b, Scalar c) 
{
  /*
  int i = grida.index(a);
  int j = gridb.index(b);
  int k = gridc.index(c);

  if (i==-1 || j==-1 || k==-1) {
    // point outside of grid validity!
    throwGridInterpolator3DException();
  }


  Scalar s = (a - grida.node(i)) / grida.step();
  Scalar t = (b - gridb.node(j)) / gridb.step();
  Scalar u = (c - gridc.node(k)) / gridc.step();

  */

  Scalar s, t, u;
  int i = grida.index(a,s);
  int j = gridb.index(b,t);
  int k = gridc.index(c,u);
  
  // test range??

  grida.normalize(i,s);
  gridb.normalize(j,t);
  gridc.normalize(k,u);

#ifdef DEBUG_LinearGridInterpolator3D
  if (InterpolationDebug::debug) {
    using std::cout;
    using std::endl;
    cout << "LinearGridInterpolator3D called with a,b,c " << a << "," << b << "," << c << endl;
    cout <<" i,j,k = " << i << "," << j << "," << k
         << " s,t,u = " << s << "," << t << "," << u << endl;
    cout << "Node positions for " << i << "," << j << "," << k << " : "
         << grida.node(i) << "," << gridb.node(j) << "," << gridc.node(k) << endl;
    cout << "Node positions for " << i+1 << "," << j+1 << "," << k+1 << " : "
         << grida.node(i+1) << "," << gridb.node(j+1) << "," << gridc.node(k+1) << endl;
    cout << "Grid(" << i << "," << j << "," << k << ") = " << grid(i,  j,  k) << " ";
    cout << "Grid(" << i << "," << j << "," << k+1 << ") = " << grid(i,j,k+1) << endl;
    cout << "Grid(" << i << "," << j+1 << "," << k << ") = " << grid(i,j+1,k) << " ";
    cout << "Grid(" << i << "," << j+1 << "," << k+1 << ") = " << grid(i,j+1,k+1) << endl;
    cout << "Grid(" << i+1 << "," << j << "," << k << ") = " << grid(i+1,j,k) << " ";
    cout << "Grid(" << i+1 << "," << j << "," << k+1 << ") = " << grid(i+1,j,k+1) << endl;
    cout << "Grid(" << i+1 << "," << j+1 << "," << k << ") = " << grid(i+1,j+1,k) << " ";
    cout << "Grid(" << i+1 << "," << j+1 << "," << k+1 << ") = " << grid(i+1,j+1,k+1) << endl;
    cout << "number of nodes: " << grida.nodes() << "," << gridb.nodes() << "," << gridc.nodes() << endl;

//     cout << (1-s)*(1-t)*(1-u)*grid(i,  j,  k) << " " << (1-s)*(1-t)*u*grid(i,  j,  k+1) << endl 
//       << (1-s)*   t *(1-u)*grid(i,  j+1,k) << " " << (1-s)*   t *u*grid(i,  j+1,k+1) << endl 
//       << s    *(1-t)*(1-u)*grid(i+1,j,  k) << " " <<  s    *(1-t)*u*grid(i+1,j,  k+1) << endl 
//       << s    *   t *(1-u)*grid(i+1,j+1,k) << " " << s    *   t *u*grid(i+1,j+1,k+1) << endl;
  }

#endif

  int ind = grid.index(i,j,k);
  int s1 = grid.stride1(); 
  int s2 = grid.stride2(); 
  int s3 = grid.stride3(); 
  //chances are this is more numerically precise this way



  // this code for test to check  properly inline of wrapped math...
#if defined(CMS_TEST_RAWSSE)

  __m128 resultSIMD =                 _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));
  resultSIMD = _mm_add_ps(resultSIMD, _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)));
  resultSIMD = _mm_add_ps(resultSIMD, _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)));
  resultSIMD = _mm_add_ps(resultSIMD, _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)));
  resultSIMD = _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)));
  resultSIMD = _mm_add_ps(resultSIMD, _mm_mul_ps(_mm_set1_ps(        s *         t    ), _mm_sub_ps(grid(ind + s1 + s2     ).v.vec, grid(ind + s1     ).v.vec)));
  resultSIMD = _mm_add_ps(resultSIMD, _mm_mul_ps(_mm_set1_ps(        s                ), _mm_sub_ps(grid(ind + s1          ).v.vec, grid(ind          ).v.vec)));
  resultSIMD = _mm_add_ps(resultSIMD,                                                                                             grid(ind          ).v.vec);
  
  ValueType result; result.v=resultSIMD;


#else
  
  ValueType result = ((1.f-s)*(1.f-t)*u)*(grid(ind      +s3) - grid(ind      ));
  result =  result + ((1.f-s)*     t *u)*(grid(ind   +s2+s3) - grid(ind   +s2));
  result =  result + (s      *(1.f-t)*u)*(grid(ind+s1   +s3) - grid(ind+s1   ));
  result =  result + (s      *     t *u)*(grid(ind+s1+s2+s3) - grid(ind+s1+s2)); 
  result =  result + (        (1.f-s)*t)*(grid(ind   +s2   ) - grid(ind      ));
  result =  result + (      s        *t)*(grid(ind+s1+s2   ) - grid(ind+s1   ));
  result =  result + (                s)*(grid(ind+s1      ) - grid(ind      ));
  result =  result +                                           grid(ind      );


#endif



  //      (1-s)*(1-t)*(1-u)*grid(i,  j,  k) + (1-s)*(1-t)*u*grid(i,  j,  k+1) + 
  //      (1-s)*   t *(1-u)*grid(i,  j+1,k) + (1-s)*   t *u*grid(i,  j+1,k+1) +
  //      s    *(1-t)*(1-u)*grid(i+1,j,  k) + s    *(1-t)*u*grid(i+1,j,  k+1) + 
  //      s    *   t *(1-u)*grid(i+1,j+1,k) + s    *   t *u*grid(i+1,j+1,k+1);


  return result;


}