File indexing completed on 2024-04-06 12:22:31
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
0014
0015
0016
0017
0018
0019
0020
0021
0022
0023
0024
0025
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
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
0061
0062
0063
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
0073
0074
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
0112
0113
0114
0115
0116 return result;
0117 }