File indexing completed on 2024-12-22 04:09:39
0001 ///////////////////////////////////////////////////////////////////////////// 0002 // einspline: a library for creating and evaluating B-splines // 0003 // Copyright (C) 2007 Kenneth P. Esler, Jr. // 0004 // // 0005 // This program is free software; you can redistribute it and/or modify // 0006 // it under the terms of the GNU General Public License as published by // 0007 // the Free Software Foundation; either version 2 of the License, or // 0008 // (at your option) any later version. // 0009 // // 0010 // This program is distributed in the hope that it will be useful, // 0011 // but WITHOUT ANY WARRANTY; without even the implied warranty of // 0012 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // 0013 // GNU General Public License for more details. // 0014 // // 0015 // You should have received a copy of the GNU General Public License // 0016 // along with this program; if not, write to the Free Software // 0017 // Foundation, Inc., 51 Franklin Street, Fifth Floor, // 0018 // Boston, MA 02110-1301 USA // 0019 ///////////////////////////////////////////////////////////////////////////// 0020 0021 #ifndef BSPLINE_EVAL_STD_C_H 0022 #define BSPLINE_EVAL_STD_C_H 0023 0024 #include <math.h> 0025 #include <stdio.h> 0026 0027 extern const float* restrict Af; 0028 extern const float* restrict dAf; 0029 extern const float* restrict d2Af; 0030 0031 /************************************************************/ 0032 /* 1D single-precision, real evaulation functions */ 0033 /************************************************************/ 0034 0035 /* Value only */ 0036 inline void 0037 eval_UBspline_1d_c (UBspline_1d_c * restrict spline, 0038 double x, complex_float* restrict val) 0039 { 0040 x -= spline->x_grid.start; 0041 float u = x*spline->x_grid.delta_inv; 0042 float ipart, t; 0043 t = modff (u, &ipart); 0044 int i = (int) ipart; 0045 0046 float tp[4]; 0047 tp[0] = t*t*t; tp[1] = t*t; tp[2] = t; tp[3] = 1.0; 0048 complex_float* restrict coefs = spline->coefs; 0049 0050 *val = 0051 (coefs[i+0]*(Af[ 0]*tp[0] + Af[ 1]*tp[1] + Af[ 2]*tp[2] + Af[ 3]*tp[3])+ 0052 coefs[i+1]*(Af[ 4]*tp[0] + Af[ 5]*tp[1] + Af[ 6]*tp[2] + Af[ 7]*tp[3])+ 0053 coefs[i+2]*(Af[ 8]*tp[0] + Af[ 9]*tp[1] + Af[10]*tp[2] + Af[11]*tp[3])+ 0054 coefs[i+3]*(Af[12]*tp[0] + Af[13]*tp[1] + Af[14]*tp[2] + Af[15]*tp[3])); 0055 } 0056 0057 /* Value and first derivative */ 0058 inline void 0059 eval_UBspline_1d_c_vg (UBspline_1d_c * restrict spline, double x, 0060 complex_float* restrict val, complex_float* restrict grad) 0061 { 0062 x -= spline->x_grid.start; 0063 float u = x*spline->x_grid.delta_inv; 0064 float ipart, t; 0065 t = modff (u, &ipart); 0066 int i = (int) ipart; 0067 0068 float tp[4]; 0069 tp[0] = t*t*t; tp[1] = t*t; tp[2] = t; tp[3] = 1.0; 0070 complex_float* restrict coefs = spline->coefs; 0071 0072 float dxInv = spline->x_grid.delta_inv; 0073 0074 *val = 0075 (coefs[i+0]*(Af[ 0]*tp[0] + Af[ 1]*tp[1] + Af[ 2]*tp[2] + Af[ 3]*tp[3])+ 0076 coefs[i+1]*(Af[ 4]*tp[0] + Af[ 5]*tp[1] + Af[ 6]*tp[2] + Af[ 7]*tp[3])+ 0077 coefs[i+2]*(Af[ 8]*tp[0] + Af[ 9]*tp[1] + Af[10]*tp[2] + Af[11]*tp[3])+ 0078 coefs[i+3]*(Af[12]*tp[0] + Af[13]*tp[1] + Af[14]*tp[2] + Af[15]*tp[3])); 0079 *grad = dxInv * 0080 (coefs[i+0]*(dAf[ 1]*tp[1] + dAf[ 2]*tp[2] + dAf[ 3]*tp[3])+ 0081 coefs[i+1]*(dAf[ 5]*tp[1] + dAf[ 6]*tp[2] + dAf[ 7]*tp[3])+ 0082 coefs[i+2]*(dAf[ 9]*tp[1] + dAf[10]*tp[2] + dAf[11]*tp[3])+ 0083 coefs[i+3]*(dAf[13]*tp[1] + dAf[14]*tp[2] + dAf[15]*tp[3])); 0084 } 0085 /* Value, first derivative, and second derivative */ 0086 inline void 0087 eval_UBspline_1d_c_vgl (UBspline_1d_c * restrict spline, double x, 0088 complex_float* restrict val, complex_float* restrict grad, 0089 complex_float* restrict lapl) 0090 { 0091 x -= spline->x_grid.start; 0092 float u = x*spline->x_grid.delta_inv; 0093 float ipart, t; 0094 t = modff (u, &ipart); 0095 int i = (int) ipart; 0096 0097 float tp[4]; 0098 tp[0] = t*t*t; tp[1] = t*t; tp[2] = t; tp[3] = 1.0; 0099 complex_float* restrict coefs = spline->coefs; 0100 0101 float dxInv = spline->x_grid.delta_inv; 0102 0103 *val = 0104 (coefs[i+0]*(Af[ 0]*tp[0] + Af[ 1]*tp[1] + Af[ 2]*tp[2] + Af[ 3]*tp[3])+ 0105 coefs[i+1]*(Af[ 4]*tp[0] + Af[ 5]*tp[1] + Af[ 6]*tp[2] + Af[ 7]*tp[3])+ 0106 coefs[i+2]*(Af[ 8]*tp[0] + Af[ 9]*tp[1] + Af[10]*tp[2] + Af[11]*tp[3])+ 0107 coefs[i+3]*(Af[12]*tp[0] + Af[13]*tp[1] + Af[14]*tp[2] + Af[15]*tp[3])); 0108 *grad = dxInv * 0109 (coefs[i+0]*(dAf[ 1]*tp[1] + dAf[ 2]*tp[2] + dAf[ 3]*tp[3])+ 0110 coefs[i+1]*(dAf[ 5]*tp[1] + dAf[ 6]*tp[2] + dAf[ 7]*tp[3])+ 0111 coefs[i+2]*(dAf[ 9]*tp[1] + dAf[10]*tp[2] + dAf[11]*tp[3])+ 0112 coefs[i+3]*(dAf[13]*tp[1] + dAf[14]*tp[2] + dAf[15]*tp[3])); 0113 *lapl = dxInv * dxInv * 0114 (coefs[i+0]*(d2Af[ 2]*tp[2] + d2Af[ 3]*tp[3])+ 0115 coefs[i+1]*(d2Af[ 6]*tp[2] + d2Af[ 7]*tp[3])+ 0116 coefs[i+2]*(d2Af[10]*tp[2] + d2Af[11]*tp[3])+ 0117 coefs[i+3]*(d2Af[14]*tp[2] + d2Af[15]*tp[3])); 0118 } 0119 inline void 0120 eval_UBspline_1d_c_vgh (UBspline_1d_c * restrict spline, double x, 0121 complex_float* restrict val, 0122 complex_float* restrict grad, 0123 complex_float* restrict hess) 0124 { 0125 eval_UBspline_1d_c_vgl (spline, x, val, grad, hess); 0126 } 0127 0128 /************************************************************/ 0129 /* 2D single-precision, real evaulation functions */ 0130 /************************************************************/ 0131 0132 /* Value only */ 0133 inline void 0134 eval_UBspline_2d_c (UBspline_2d_c * restrict spline, 0135 double x, double y, complex_float* restrict val) 0136 { 0137 x -= spline->x_grid.start; 0138 y -= spline->y_grid.start; 0139 float ux = x*spline->x_grid.delta_inv; 0140 float uy = y*spline->y_grid.delta_inv; 0141 float ipartx, iparty, tx, ty; 0142 tx = modff (ux, &ipartx); 0143 ty = modff (uy, &iparty); 0144 int ix = (int) ipartx; 0145 int iy = (int) iparty; 0146 0147 float tpx[4], tpy[4], a[4], b[4]; 0148 tpx[0] = tx*tx*tx; tpx[1] = tx*tx; tpx[2] = tx; tpx[3] = 1.0; 0149 tpy[0] = ty*ty*ty; tpy[1] = ty*ty; tpy[2] = ty; tpy[3] = 1.0; 0150 complex_float* restrict coefs = spline->coefs; 0151 0152 a[0] = (Af[ 0]*tpx[0] + Af[ 1]*tpx[1] + Af[ 2]*tpx[2] + Af[ 3]*tpx[3]); 0153 a[1] = (Af[ 4]*tpx[0] + Af[ 5]*tpx[1] + Af[ 6]*tpx[2] + Af[ 7]*tpx[3]); 0154 a[2] = (Af[ 8]*tpx[0] + Af[ 9]*tpx[1] + Af[10]*tpx[2] + Af[11]*tpx[3]); 0155 a[3] = (Af[12]*tpx[0] + Af[13]*tpx[1] + Af[14]*tpx[2] + Af[15]*tpx[3]); 0156 0157 b[0] = (Af[ 0]*tpy[0] + Af[ 1]*tpy[1] + Af[ 2]*tpy[2] + Af[ 3]*tpy[3]); 0158 b[1] = (Af[ 4]*tpy[0] + Af[ 5]*tpy[1] + Af[ 6]*tpy[2] + Af[ 7]*tpy[3]); 0159 b[2] = (Af[ 8]*tpy[0] + Af[ 9]*tpy[1] + Af[10]*tpy[2] + Af[11]*tpy[3]); 0160 b[3] = (Af[12]*tpy[0] + Af[13]*tpy[1] + Af[14]*tpy[2] + Af[15]*tpy[3]); 0161 0162 int xs = spline->x_stride; 0163 #define C(i,j) coefs[(ix+(i))*xs+iy+(j)] 0164 *val = (a[0]*(C(0,0)*b[0]+C(0,1)*b[1]+C(0,2)*b[2]+C(0,3)*b[3])+ 0165 a[1]*(C(1,0)*b[0]+C(1,1)*b[1]+C(1,2)*b[2]+C(1,3)*b[3])+ 0166 a[2]*(C(2,0)*b[0]+C(2,1)*b[1]+C(2,2)*b[2]+C(2,3)*b[3])+ 0167 a[3]*(C(3,0)*b[0]+C(3,1)*b[1]+C(3,2)*b[2]+C(3,3)*b[3])); 0168 #undef C 0169 0170 } 0171 0172 0173 /* Value and gradient */ 0174 inline void 0175 eval_UBspline_2d_c_vg (UBspline_2d_c * restrict spline, 0176 double x, double y, 0177 complex_float* restrict val, complex_float* restrict grad) 0178 { 0179 x -= spline->x_grid.start; 0180 y -= spline->y_grid.start; 0181 float ux = x*spline->x_grid.delta_inv; 0182 float uy = y*spline->y_grid.delta_inv; 0183 float ipartx, iparty, tx, ty; 0184 tx = modff (ux, &ipartx); 0185 ty = modff (uy, &iparty); 0186 int ix = (int) ipartx; 0187 int iy = (int) iparty; 0188 0189 float tpx[4], tpy[4], a[4], b[4], da[4], db[4]; 0190 tpx[0] = tx*tx*tx; tpx[1] = tx*tx; tpx[2] = tx; tpx[3] = 1.0; 0191 tpy[0] = ty*ty*ty; tpy[1] = ty*ty; tpy[2] = ty; tpy[3] = 1.0; 0192 complex_float* restrict coefs = spline->coefs; 0193 0194 a[0] = (Af[ 0]*tpx[0] + Af[ 1]*tpx[1] + Af[ 2]*tpx[2] + Af[ 3]*tpx[3]); 0195 a[1] = (Af[ 4]*tpx[0] + Af[ 5]*tpx[1] + Af[ 6]*tpx[2] + Af[ 7]*tpx[3]); 0196 a[2] = (Af[ 8]*tpx[0] + Af[ 9]*tpx[1] + Af[10]*tpx[2] + Af[11]*tpx[3]); 0197 a[3] = (Af[12]*tpx[0] + Af[13]*tpx[1] + Af[14]*tpx[2] + Af[15]*tpx[3]); 0198 da[0] = (dAf[ 1]*tpx[1] + dAf[ 2]*tpx[2] + dAf[ 3]*tpx[3]); 0199 da[1] = (dAf[ 5]*tpx[1] + dAf[ 6]*tpx[2] + dAf[ 7]*tpx[3]); 0200 da[2] = (dAf[ 9]*tpx[1] + dAf[10]*tpx[2] + dAf[11]*tpx[3]); 0201 da[3] = (dAf[13]*tpx[1] + dAf[14]*tpx[2] + dAf[15]*tpx[3]); 0202 0203 b[0] = (Af[ 0]*tpy[0] + Af[ 1]*tpy[1] + Af[ 2]*tpy[2] + Af[ 3]*tpy[3]); 0204 b[1] = (Af[ 4]*tpy[0] + Af[ 5]*tpy[1] + Af[ 6]*tpy[2] + Af[ 7]*tpy[3]); 0205 b[2] = (Af[ 8]*tpy[0] + Af[ 9]*tpy[1] + Af[10]*tpy[2] + Af[11]*tpy[3]); 0206 b[3] = (Af[12]*tpy[0] + Af[13]*tpy[1] + Af[14]*tpy[2] + Af[15]*tpy[3]); 0207 db[0] = (dAf[ 1]*tpy[1] + dAf[ 2]*tpy[2] + dAf[ 3]*tpy[3]); 0208 db[1] = (dAf[ 5]*tpy[1] + dAf[ 6]*tpy[2] + dAf[ 7]*tpy[3]); 0209 db[2] = (dAf[ 9]*tpy[1] + dAf[10]*tpy[2] + dAf[11]*tpy[3]); 0210 db[3] = (dAf[13]*tpy[1] + dAf[14]*tpy[2] + dAf[15]*tpy[3]); 0211 0212 int xs = spline->x_stride; 0213 float dxInv = spline->x_grid.delta_inv; 0214 float dyInv = spline->y_grid.delta_inv; 0215 #define C(i,j) coefs[(ix+(i))*xs+iy+(j)] 0216 *val = 0217 (a[0]*(C(0,0)*b[0]+C(0,1)*b[1]+C(0,2)*b[2]+C(0,3)*b[3])+ 0218 a[1]*(C(1,0)*b[0]+C(1,1)*b[1]+C(1,2)*b[2]+C(1,3)*b[3])+ 0219 a[2]*(C(2,0)*b[0]+C(2,1)*b[1]+C(2,2)*b[2]+C(2,3)*b[3])+ 0220 a[3]*(C(3,0)*b[0]+C(3,1)*b[1]+C(3,2)*b[2]+C(3,3)*b[3])); 0221 grad[0] = dxInv * 0222 (da[0]*(C(0,0)*b[0]+C(0,1)*b[1]+C(0,2)*b[2]+C(0,3)*b[3])+ 0223 da[1]*(C(1,0)*b[0]+C(1,1)*b[1]+C(1,2)*b[2]+C(1,3)*b[3])+ 0224 da[2]*(C(2,0)*b[0]+C(2,1)*b[1]+C(2,2)*b[2]+C(2,3)*b[3])+ 0225 da[3]*(C(3,0)*b[0]+C(3,1)*b[1]+C(3,2)*b[2]+C(3,3)*b[3])); 0226 grad[1] = dyInv * 0227 (a[0]*(C(0,0)*db[0]+C(0,1)*db[1]+C(0,2)*db[2]+C(0,3)*db[3])+ 0228 a[1]*(C(1,0)*db[0]+C(1,1)*db[1]+C(1,2)*db[2]+C(1,3)*db[3])+ 0229 a[2]*(C(2,0)*db[0]+C(2,1)*db[1]+C(2,2)*db[2]+C(2,3)*db[3])+ 0230 a[3]*(C(3,0)*db[0]+C(3,1)*db[1]+C(3,2)*db[2]+C(3,3)*db[3])); 0231 #undef C 0232 0233 } 0234 0235 /* Value, gradient, and laplacian */ 0236 inline void 0237 eval_UBspline_2d_c_vgl (UBspline_2d_c * restrict spline, 0238 double x, double y, complex_float* restrict val, 0239 complex_float* restrict grad, complex_float* restrict lapl) 0240 { 0241 x -= spline->x_grid.start; 0242 y -= spline->y_grid.start; 0243 float ux = x*spline->x_grid.delta_inv; 0244 float uy = y*spline->y_grid.delta_inv; 0245 float ipartx, iparty, tx, ty; 0246 tx = modff (ux, &ipartx); 0247 ty = modff (uy, &iparty); 0248 int ix = (int) ipartx; 0249 int iy = (int) iparty; 0250 0251 float tpx[4], tpy[4], a[4], b[4], da[4], db[4], d2a[4], d2b[4]; 0252 tpx[0] = tx*tx*tx; tpx[1] = tx*tx; tpx[2] = tx; tpx[3] = 1.0; 0253 tpy[0] = ty*ty*ty; tpy[1] = ty*ty; tpy[2] = ty; tpy[3] = 1.0; 0254 complex_float* restrict coefs = spline->coefs; 0255 0256 a[0] = ( Af[ 0]*tpx[0] + Af[ 1]*tpx[1] + Af[ 2]*tpx[2] + Af[ 3]*tpx[3]); 0257 a[1] = ( Af[ 4]*tpx[0] + Af[ 5]*tpx[1] + Af[ 6]*tpx[2] + Af[ 7]*tpx[3]); 0258 a[2] = ( Af[ 8]*tpx[0] + Af[ 9]*tpx[1] + Af[10]*tpx[2] + Af[11]*tpx[3]); 0259 a[3] = ( Af[12]*tpx[0] + Af[13]*tpx[1] + Af[14]*tpx[2] + Af[15]*tpx[3]); 0260 da[0] = ( dAf[ 1]*tpx[1] + dAf[ 2]*tpx[2] + dAf[ 3]*tpx[3]); 0261 da[1] = ( dAf[ 5]*tpx[1] + dAf[ 6]*tpx[2] + dAf[ 7]*tpx[3]); 0262 da[2] = ( dAf[ 9]*tpx[1] + dAf[10]*tpx[2] + dAf[11]*tpx[3]); 0263 da[3] = ( dAf[13]*tpx[1] + dAf[14]*tpx[2] + dAf[15]*tpx[3]); 0264 d2a[0] = (d2Af[ 2]*tpx[2] + d2Af[ 3]*tpx[3]); 0265 d2a[1] = (d2Af[ 6]*tpx[2] + d2Af[ 7]*tpx[3]); 0266 d2a[2] = (d2Af[10]*tpx[2] + d2Af[11]*tpx[3]); 0267 d2a[3] = (d2Af[14]*tpx[2] + d2Af[15]*tpx[3]); 0268 0269 b[0] = ( Af[ 0]*tpy[0] + Af[ 1]*tpy[1] + Af[ 2]*tpy[2] + Af[ 3]*tpy[3]); 0270 b[1] = ( Af[ 4]*tpy[0] + Af[ 5]*tpy[1] + Af[ 6]*tpy[2] + Af[ 7]*tpy[3]); 0271 b[2] = ( Af[ 8]*tpy[0] + Af[ 9]*tpy[1] + Af[10]*tpy[2] + Af[11]*tpy[3]); 0272 b[3] = ( Af[12]*tpy[0] + Af[13]*tpy[1] + Af[14]*tpy[2] + Af[15]*tpy[3]); 0273 db[0] = (dAf[ 1]*tpy[1] + dAf[ 2]*tpy[2] + dAf[ 3]*tpy[3]); 0274 db[1] = (dAf[ 5]*tpy[1] + dAf[ 6]*tpy[2] + dAf[ 7]*tpy[3]); 0275 db[2] = (dAf[ 9]*tpy[1] + dAf[10]*tpy[2] + dAf[11]*tpy[3]); 0276 db[3] = (dAf[13]*tpy[1] + dAf[14]*tpy[2] + dAf[15]*tpy[3]); 0277 d2b[0] = (d2Af[ 2]*tpy[2] + d2Af[ 3]*tpy[3]); 0278 d2b[1] = (d2Af[ 6]*tpy[2] + d2Af[ 7]*tpy[3]); 0279 d2b[2] = (d2Af[10]*tpy[2] + d2Af[11]*tpy[3]); 0280 d2b[3] = (d2Af[14]*tpy[2] + d2Af[15]*tpy[3]); 0281 0282 int xs = spline->x_stride; 0283 0284 float dxInv = spline->x_grid.delta_inv; 0285 float dyInv = spline->y_grid.delta_inv; 0286 0287 #define C(i,j) coefs[(ix+(i))*xs+iy+(j)] 0288 *val = 0289 (a[0]*(C(0,0)*b[0]+C(0,1)*b[1]+C(0,2)*b[2]+C(0,3)*b[3])+ 0290 a[1]*(C(1,0)*b[0]+C(1,1)*b[1]+C(1,2)*b[2]+C(1,3)*b[3])+ 0291 a[2]*(C(2,0)*b[0]+C(2,1)*b[1]+C(2,2)*b[2]+C(2,3)*b[3])+ 0292 a[3]*(C(3,0)*b[0]+C(3,1)*b[1]+C(3,2)*b[2]+C(3,3)*b[3])); 0293 grad[0] = dxInv * 0294 (da[0]*(C(0,0)*b[0]+C(0,1)*b[1]+C(0,2)*b[2]+C(0,3)*b[3])+ 0295 da[1]*(C(1,0)*b[0]+C(1,1)*b[1]+C(1,2)*b[2]+C(1,3)*b[3])+ 0296 da[2]*(C(2,0)*b[0]+C(2,1)*b[1]+C(2,2)*b[2]+C(2,3)*b[3])+ 0297 da[3]*(C(3,0)*b[0]+C(3,1)*b[1]+C(3,2)*b[2]+C(3,3)*b[3])); 0298 grad[1] = dyInv* 0299 (a[0]*(C(0,0)*db[0]+C(0,1)*db[1]+C(0,2)*db[2]+C(0,3)*db[3])+ 0300 a[1]*(C(1,0)*db[0]+C(1,1)*db[1]+C(1,2)*db[2]+C(1,3)*db[3])+ 0301 a[2]*(C(2,0)*db[0]+C(2,1)*db[1]+C(2,2)*db[2]+C(2,3)*db[3])+ 0302 a[3]*(C(3,0)*db[0]+C(3,1)*db[1]+C(3,2)*db[2]+C(3,3)*db[3])); 0303 *lapl = 0304 dyInv * dyInv * 0305 (a[0]*(C(0,0)*d2b[0]+C(0,1)*d2b[1]+C(0,2)*d2b[2]+C(0,3)*d2b[3])+ 0306 a[1]*(C(1,0)*d2b[0]+C(1,1)*d2b[1]+C(1,2)*d2b[2]+C(1,3)*d2b[3])+ 0307 a[2]*(C(2,0)*d2b[0]+C(2,1)*d2b[1]+C(2,2)*d2b[2]+C(2,3)*d2b[3])+ 0308 a[3]*(C(3,0)*d2b[0]+C(3,1)*d2b[1]+C(3,2)*d2b[2]+C(3,3)*d2b[3])) + 0309 dxInv * dxInv * 0310 (d2a[0]*(C(0,0)*b[0]+C(0,1)*b[1]+C(0,2)*b[2]+C(0,3)*b[3])+ 0311 d2a[1]*(C(1,0)*b[0]+C(1,1)*b[1]+C(1,2)*b[2]+C(1,3)*b[3])+ 0312 d2a[2]*(C(2,0)*b[0]+C(2,1)*b[1]+C(2,2)*b[2]+C(2,3)*b[3])+ 0313 d2a[3]*(C(3,0)*b[0]+C(3,1)*b[1]+C(3,2)*b[2]+C(3,3)*b[3])); 0314 0315 #undef C 0316 0317 } 0318 0319 /* Value, gradient, and Hessian */ 0320 inline void 0321 eval_UBspline_2d_c_vgh (UBspline_2d_c * restrict spline, 0322 double x, double y, complex_float* restrict val, 0323 complex_float* restrict grad, complex_float* restrict hess) 0324 { 0325 x -= spline->x_grid.start; 0326 y -= spline->y_grid.start; 0327 float ux = x*spline->x_grid.delta_inv; 0328 float uy = y*spline->y_grid.delta_inv; 0329 float ipartx, iparty, tx, ty; 0330 tx = modff (ux, &ipartx); 0331 ty = modff (uy, &iparty); 0332 int ix = (int) ipartx; 0333 int iy = (int) iparty; 0334 0335 float tpx[4], tpy[4], a[4], b[4], da[4], db[4], d2a[4], d2b[4]; 0336 tpx[0] = tx*tx*tx; tpx[1] = tx*tx; tpx[2] = tx; tpx[3] = 1.0; 0337 tpy[0] = ty*ty*ty; tpy[1] = ty*ty; tpy[2] = ty; tpy[3] = 1.0; 0338 complex_float* restrict coefs = spline->coefs; 0339 0340 a[0] = ( Af[ 0]*tpx[0] + Af[ 1]*tpx[1] + Af[ 2]*tpx[2] + Af[ 3]*tpx[3]); 0341 a[1] = ( Af[ 4]*tpx[0] + Af[ 5]*tpx[1] + Af[ 6]*tpx[2] + Af[ 7]*tpx[3]); 0342 a[2] = ( Af[ 8]*tpx[0] + Af[ 9]*tpx[1] + Af[10]*tpx[2] + Af[11]*tpx[3]); 0343 a[3] = ( Af[12]*tpx[0] + Af[13]*tpx[1] + Af[14]*tpx[2] + Af[15]*tpx[3]); 0344 da[0] = ( dAf[ 1]*tpx[1] + dAf[ 2]*tpx[2] + dAf[ 3]*tpx[3]); 0345 da[1] = ( dAf[ 5]*tpx[1] + dAf[ 6]*tpx[2] + dAf[ 7]*tpx[3]); 0346 da[2] = ( dAf[ 9]*tpx[1] + dAf[10]*tpx[2] + dAf[11]*tpx[3]); 0347 da[3] = ( dAf[13]*tpx[1] + dAf[14]*tpx[2] + dAf[15]*tpx[3]); 0348 d2a[0] = (d2Af[ 2]*tpx[2] + d2Af[ 3]*tpx[3]); 0349 d2a[1] = (d2Af[ 6]*tpx[2] + d2Af[ 7]*tpx[3]); 0350 d2a[2] = (d2Af[10]*tpx[2] + d2Af[11]*tpx[3]); 0351 d2a[3] = (d2Af[14]*tpx[2] + d2Af[15]*tpx[3]); 0352 0353 b[0] = ( Af[ 0]*tpy[0] + Af[ 1]*tpy[1] + Af[ 2]*tpy[2] + Af[ 3]*tpy[3]); 0354 b[1] = ( Af[ 4]*tpy[0] + Af[ 5]*tpy[1] + Af[ 6]*tpy[2] + Af[ 7]*tpy[3]); 0355 b[2] = ( Af[ 8]*tpy[0] + Af[ 9]*tpy[1] + Af[10]*tpy[2] + Af[11]*tpy[3]); 0356 b[3] = ( Af[12]*tpy[0] + Af[13]*tpy[1] + Af[14]*tpy[2] + Af[15]*tpy[3]); 0357 db[0] = ( dAf[ 1]*tpy[1] + dAf[ 2]*tpy[2] + dAf[ 3]*tpy[3]); 0358 db[1] = ( dAf[ 5]*tpy[1] + dAf[ 6]*tpy[2] + dAf[ 7]*tpy[3]); 0359 db[2] = ( dAf[ 9]*tpy[1] + dAf[10]*tpy[2] + dAf[11]*tpy[3]); 0360 db[3] = ( dAf[13]*tpy[1] + dAf[14]*tpy[2] + dAf[15]*tpy[3]); 0361 d2b[0] = (d2Af[ 2]*tpy[2] + d2Af[ 3]*tpy[3]); 0362 d2b[1] = (d2Af[ 6]*tpy[2] + d2Af[ 7]*tpy[3]); 0363 d2b[2] = (d2Af[10]*tpy[2] + d2Af[11]*tpy[3]); 0364 d2b[3] = (d2Af[14]*tpy[2] + d2Af[15]*tpy[3]); 0365 0366 int xs = spline->x_stride; 0367 float dxInv = spline->x_grid.delta_inv; 0368 float dyInv = spline->y_grid.delta_inv; 0369 #define C(i,j) coefs[(ix+(i))*xs+iy+(j)] 0370 *val = 0371 ( a[0]*(C(0,0)* b[0]+C(0,1)* b[1]+C(0,2)* b[2]+C(0,3)* b[3])+ 0372 a[1]*(C(1,0)* b[0]+C(1,1)* b[1]+C(1,2)* b[2]+C(1,3)* b[3])+ 0373 a[2]*(C(2,0)* b[0]+C(2,1)* b[1]+C(2,2)* b[2]+C(2,3)* b[3])+ 0374 a[3]*(C(3,0)* b[0]+C(3,1)* b[1]+C(3,2)* b[2]+C(3,3)* b[3])); 0375 grad[0] = dxInv * 0376 ( da[0]*(C(0,0)* b[0]+C(0,1)* b[1]+C(0,2)* b[2]+C(0,3)* b[3])+ 0377 da[1]*(C(1,0)* b[0]+C(1,1)* b[1]+C(1,2)* b[2]+C(1,3)* b[3])+ 0378 da[2]*(C(2,0)* b[0]+C(2,1)* b[1]+C(2,2)* b[2]+C(2,3)* b[3])+ 0379 da[3]*(C(3,0)* b[0]+C(3,1)* b[1]+C(3,2)* b[2]+C(3,3)* b[3])); 0380 grad[1] = dyInv * 0381 ( a[0]*(C(0,0)* db[0]+C(0,1)* db[1]+C(0,2)* db[2]+C(0,3)* db[3])+ 0382 a[1]*(C(1,0)* db[0]+C(1,1)* db[1]+C(1,2)* db[2]+C(1,3)* db[3])+ 0383 a[2]*(C(2,0)* db[0]+C(2,1)* db[1]+C(2,2)* db[2]+C(2,3)* db[3])+ 0384 a[3]*(C(3,0)* db[0]+C(3,1)* db[1]+C(3,2)* db[2]+C(3,3)* db[3])); 0385 hess[0] = dxInv * dxInv * 0386 (d2a[0]*(C(0,0)* b[0]+C(0,1)* b[1]+C(0,2)* b[2]+C(0,3)* b[3])+ 0387 d2a[1]*(C(1,0)* b[0]+C(1,1)* b[1]+C(1,2)* b[2]+C(1,3)* b[3])+ 0388 d2a[2]*(C(2,0)* b[0]+C(2,1)* b[1]+C(2,2)* b[2]+C(2,3)* b[3])+ 0389 d2a[3]*(C(3,0)* b[0]+C(3,1)* b[1]+C(3,2)* b[2]+C(3,3)* b[3])); 0390 hess[1] = dxInv * dyInv * 0391 ( da[0]*(C(0,0)* db[0]+C(0,1)* db[1]+C(0,2)* db[2]+C(0,3)* db[3])+ 0392 da[1]*(C(1,0)* db[0]+C(1,1)* db[1]+C(1,2)* db[2]+C(1,3)* db[3])+ 0393 da[2]*(C(2,0)* db[0]+C(2,1)* db[1]+C(2,2)* db[2]+C(2,3)* db[3])+ 0394 da[3]*(C(3,0)* db[0]+C(3,1)* db[1]+C(3,2)* db[2]+C(3,3)* db[3])); 0395 hess[3] = dyInv * dyInv * 0396 ( a[0]*(C(0,0)*d2b[0]+C(0,1)*d2b[1]+C(0,2)*d2b[2]+C(0,3)*d2b[3])+ 0397 a[1]*(C(1,0)*d2b[0]+C(1,1)*d2b[1]+C(1,2)*d2b[2]+C(1,3)*d2b[3])+ 0398 a[2]*(C(2,0)*d2b[0]+C(2,1)*d2b[1]+C(2,2)*d2b[2]+C(2,3)*d2b[3])+ 0399 a[3]*(C(3,0)*d2b[0]+C(3,1)*d2b[1]+C(3,2)*d2b[2]+C(3,3)*d2b[3])); 0400 hess[2] = hess[1]; 0401 0402 #undef C 0403 0404 } 0405 0406 0407 /************************************************************/ 0408 /* 3D single-precision, real evaulation functions */ 0409 /************************************************************/ 0410 0411 /* Value only */ 0412 inline void 0413 eval_UBspline_3d_c (UBspline_3d_c * restrict spline, 0414 double x, double y, double z, 0415 complex_float* restrict val) 0416 { 0417 x -= spline->x_grid.start; 0418 y -= spline->y_grid.start; 0419 z -= spline->z_grid.start; 0420 float ux = x*spline->x_grid.delta_inv; 0421 float uy = y*spline->y_grid.delta_inv; 0422 float uz = z*spline->z_grid.delta_inv; 0423 float ipartx, iparty, ipartz, tx, ty, tz; 0424 tx = modff (ux, &ipartx); int ix = (int) ipartx; 0425 ty = modff (uy, &iparty); int iy = (int) iparty; 0426 tz = modff (uz, &ipartz); int iz = (int) ipartz; 0427 0428 0429 0430 0431 float tpx[4], tpy[4], tpz[4], a[4], b[4], c[4]; 0432 tpx[0] = tx*tx*tx; tpx[1] = tx*tx; tpx[2] = tx; tpx[3] = 1.0; 0433 tpy[0] = ty*ty*ty; tpy[1] = ty*ty; tpy[2] = ty; tpy[3] = 1.0; 0434 tpz[0] = tz*tz*tz; tpz[1] = tz*tz; tpz[2] = tz; tpz[3] = 1.0; 0435 complex_float* restrict coefs = spline->coefs; 0436 0437 a[0] = (Af[ 0]*tpx[0] + Af[ 1]*tpx[1] + Af[ 2]*tpx[2] + Af[ 3]*tpx[3]); 0438 a[1] = (Af[ 4]*tpx[0] + Af[ 5]*tpx[1] + Af[ 6]*tpx[2] + Af[ 7]*tpx[3]); 0439 a[2] = (Af[ 8]*tpx[0] + Af[ 9]*tpx[1] + Af[10]*tpx[2] + Af[11]*tpx[3]); 0440 a[3] = (Af[12]*tpx[0] + Af[13]*tpx[1] + Af[14]*tpx[2] + Af[15]*tpx[3]); 0441 0442 b[0] = (Af[ 0]*tpy[0] + Af[ 1]*tpy[1] + Af[ 2]*tpy[2] + Af[ 3]*tpy[3]); 0443 b[1] = (Af[ 4]*tpy[0] + Af[ 5]*tpy[1] + Af[ 6]*tpy[2] + Af[ 7]*tpy[3]); 0444 b[2] = (Af[ 8]*tpy[0] + Af[ 9]*tpy[1] + Af[10]*tpy[2] + Af[11]*tpy[3]); 0445 b[3] = (Af[12]*tpy[0] + Af[13]*tpy[1] + Af[14]*tpy[2] + Af[15]*tpy[3]); 0446 0447 c[0] = (Af[ 0]*tpz[0] + Af[ 1]*tpz[1] + Af[ 2]*tpz[2] + Af[ 3]*tpz[3]); 0448 c[1] = (Af[ 4]*tpz[0] + Af[ 5]*tpz[1] + Af[ 6]*tpz[2] + Af[ 7]*tpz[3]); 0449 c[2] = (Af[ 8]*tpz[0] + Af[ 9]*tpz[1] + Af[10]*tpz[2] + Af[11]*tpz[3]); 0450 c[3] = (Af[12]*tpz[0] + Af[13]*tpz[1] + Af[14]*tpz[2] + Af[15]*tpz[3]); 0451 0452 int xs = spline->x_stride; 0453 int ys = spline->y_stride; 0454 #define P(i,j,k) coefs[(ix+(i))*xs+(iy+(j))*ys+(iz+(k))] 0455 *val = (a[0]*(b[0]*(P(0,0,0)*c[0]+P(0,0,1)*c[1]+P(0,0,2)*c[2]+P(0,0,3)*c[3])+ 0456 b[1]*(P(0,1,0)*c[0]+P(0,1,1)*c[1]+P(0,1,2)*c[2]+P(0,1,3)*c[3])+ 0457 b[2]*(P(0,2,0)*c[0]+P(0,2,1)*c[1]+P(0,2,2)*c[2]+P(0,2,3)*c[3])+ 0458 b[3]*(P(0,3,0)*c[0]+P(0,3,1)*c[1]+P(0,3,2)*c[2]+P(0,3,3)*c[3]))+ 0459 a[1]*(b[0]*(P(1,0,0)*c[0]+P(1,0,1)*c[1]+P(1,0,2)*c[2]+P(1,0,3)*c[3])+ 0460 b[1]*(P(1,1,0)*c[0]+P(1,1,1)*c[1]+P(1,1,2)*c[2]+P(1,1,3)*c[3])+ 0461 b[2]*(P(1,2,0)*c[0]+P(1,2,1)*c[1]+P(1,2,2)*c[2]+P(1,2,3)*c[3])+ 0462 b[3]*(P(1,3,0)*c[0]+P(1,3,1)*c[1]+P(1,3,2)*c[2]+P(1,3,3)*c[3]))+ 0463 a[2]*(b[0]*(P(2,0,0)*c[0]+P(2,0,1)*c[1]+P(2,0,2)*c[2]+P(2,0,3)*c[3])+ 0464 b[1]*(P(2,1,0)*c[0]+P(2,1,1)*c[1]+P(2,1,2)*c[2]+P(2,1,3)*c[3])+ 0465 b[2]*(P(2,2,0)*c[0]+P(2,2,1)*c[1]+P(2,2,2)*c[2]+P(2,2,3)*c[3])+ 0466 b[3]*(P(2,3,0)*c[0]+P(2,3,1)*c[1]+P(2,3,2)*c[2]+P(2,3,3)*c[3]))+ 0467 a[3]*(b[0]*(P(3,0,0)*c[0]+P(3,0,1)*c[1]+P(3,0,2)*c[2]+P(3,0,3)*c[3])+ 0468 b[1]*(P(3,1,0)*c[0]+P(3,1,1)*c[1]+P(3,1,2)*c[2]+P(3,1,3)*c[3])+ 0469 b[2]*(P(3,2,0)*c[0]+P(3,2,1)*c[1]+P(3,2,2)*c[2]+P(3,2,3)*c[3])+ 0470 b[3]*(P(3,3,0)*c[0]+P(3,3,1)*c[1]+P(3,3,2)*c[2]+P(3,3,3)*c[3]))); 0471 #undef P 0472 0473 } 0474 0475 /* Value and gradient */ 0476 inline void 0477 eval_UBspline_3d_c_vg (UBspline_3d_c * restrict spline, 0478 double x, double y, double z, 0479 complex_float* restrict val, complex_float* restrict grad) 0480 { 0481 x -= spline->x_grid.start; 0482 y -= spline->y_grid.start; 0483 z -= spline->z_grid.start; 0484 float ux = x*spline->x_grid.delta_inv; 0485 float uy = y*spline->y_grid.delta_inv; 0486 float uz = z*spline->z_grid.delta_inv; 0487 float ipartx, iparty, ipartz, tx, ty, tz; 0488 tx = modff (ux, &ipartx); int ix = (int) ipartx; 0489 ty = modff (uy, &iparty); int iy = (int) iparty; 0490 tz = modff (uz, &ipartz); int iz = (int) ipartz; 0491 0492 float tpx[4], tpy[4], tpz[4], a[4], b[4], c[4], da[4], db[4], dc[4]; 0493 complex_float cP[16], bcP[4], dbcP[4]; 0494 tpx[0] = tx*tx*tx; tpx[1] = tx*tx; tpx[2] = tx; tpx[3] = 1.0; 0495 tpy[0] = ty*ty*ty; tpy[1] = ty*ty; tpy[2] = ty; tpy[3] = 1.0; 0496 tpz[0] = tz*tz*tz; tpz[1] = tz*tz; tpz[2] = tz; tpz[3] = 1.0; 0497 complex_float* restrict coefs = spline->coefs; 0498 0499 a[0] = ( Af[ 0]*tpx[0] + Af[ 1]*tpx[1] + Af[ 2]*tpx[2] + Af[ 3]*tpx[3]); 0500 a[1] = ( Af[ 4]*tpx[0] + Af[ 5]*tpx[1] + Af[ 6]*tpx[2] + Af[ 7]*tpx[3]); 0501 a[2] = ( Af[ 8]*tpx[0] + Af[ 9]*tpx[1] + Af[10]*tpx[2] + Af[11]*tpx[3]); 0502 a[3] = ( Af[12]*tpx[0] + Af[13]*tpx[1] + Af[14]*tpx[2] + Af[15]*tpx[3]); 0503 da[0] = ( dAf[ 1]*tpx[1] + dAf[ 2]*tpx[2] + dAf[ 3]*tpx[3]); 0504 da[1] = ( dAf[ 5]*tpx[1] + dAf[ 6]*tpx[2] + dAf[ 7]*tpx[3]); 0505 da[2] = ( dAf[ 9]*tpx[1] + dAf[10]*tpx[2] + dAf[11]*tpx[3]); 0506 da[3] = ( dAf[13]*tpx[1] + dAf[14]*tpx[2] + dAf[15]*tpx[3]); 0507 0508 b[0] = ( Af[ 0]*tpy[0] + Af[ 1]*tpy[1] + Af[ 2]*tpy[2] + Af[ 3]*tpy[3]); 0509 b[1] = ( Af[ 4]*tpy[0] + Af[ 5]*tpy[1] + Af[ 6]*tpy[2] + Af[ 7]*tpy[3]); 0510 b[2] = ( Af[ 8]*tpy[0] + Af[ 9]*tpy[1] + Af[10]*tpy[2] + Af[11]*tpy[3]); 0511 b[3] = ( Af[12]*tpy[0] + Af[13]*tpy[1] + Af[14]*tpy[2] + Af[15]*tpy[3]); 0512 db[0] = (dAf[ 1]*tpy[1] + dAf[ 2]*tpy[2] + dAf[ 3]*tpy[3]); 0513 db[1] = (dAf[ 5]*tpy[1] + dAf[ 6]*tpy[2] + dAf[ 7]*tpy[3]); 0514 db[2] = (dAf[ 9]*tpy[1] + dAf[10]*tpy[2] + dAf[11]*tpy[3]); 0515 db[3] = (dAf[13]*tpy[1] + dAf[14]*tpy[2] + dAf[15]*tpy[3]); 0516 0517 c[0] = ( Af[ 0]*tpz[0] + Af[ 1]*tpz[1] + Af[ 2]*tpz[2] + Af[ 3]*tpz[3]); 0518 c[1] = ( Af[ 4]*tpz[0] + Af[ 5]*tpz[1] + Af[ 6]*tpz[2] + Af[ 7]*tpz[3]); 0519 c[2] = ( Af[ 8]*tpz[0] + Af[ 9]*tpz[1] + Af[10]*tpz[2] + Af[11]*tpz[3]); 0520 c[3] = ( Af[12]*tpz[0] + Af[13]*tpz[1] + Af[14]*tpz[2] + Af[15]*tpz[3]); 0521 dc[0] = (dAf[ 1]*tpz[1] + dAf[ 2]*tpz[2] + dAf[ 3]*tpz[3]); 0522 dc[1] = (dAf[ 5]*tpz[1] + dAf[ 6]*tpz[2] + dAf[ 7]*tpz[3]); 0523 dc[2] = (dAf[ 9]*tpz[1] + dAf[10]*tpz[2] + dAf[11]*tpz[3]); 0524 dc[3] = (dAf[13]*tpz[1] + dAf[14]*tpz[2] + dAf[15]*tpz[3]); 0525 0526 int xs = spline->x_stride; 0527 int ys = spline->y_stride; 0528 0529 float dxInv = spline->x_grid.delta_inv; 0530 float dyInv = spline->y_grid.delta_inv; 0531 float dzInv = spline->z_grid.delta_inv; 0532 0533 #define P(i,j,k) coefs[(ix+(i))*xs+(iy+(j))*ys+(iz+(k))] 0534 cP[ 0] = (P(0,0,0)*c[0]+P(0,0,1)*c[1]+P(0,0,2)*c[2]+P(0,0,3)*c[3]); 0535 cP[ 1] = (P(0,1,0)*c[0]+P(0,1,1)*c[1]+P(0,1,2)*c[2]+P(0,1,3)*c[3]); 0536 cP[ 2] = (P(0,2,0)*c[0]+P(0,2,1)*c[1]+P(0,2,2)*c[2]+P(0,2,3)*c[3]); 0537 cP[ 3] = (P(0,3,0)*c[0]+P(0,3,1)*c[1]+P(0,3,2)*c[2]+P(0,3,3)*c[3]); 0538 cP[ 4] = (P(1,0,0)*c[0]+P(1,0,1)*c[1]+P(1,0,2)*c[2]+P(1,0,3)*c[3]); 0539 cP[ 5] = (P(1,1,0)*c[0]+P(1,1,1)*c[1]+P(1,1,2)*c[2]+P(1,1,3)*c[3]); 0540 cP[ 6] = (P(1,2,0)*c[0]+P(1,2,1)*c[1]+P(1,2,2)*c[2]+P(1,2,3)*c[3]); 0541 cP[ 7] = (P(1,3,0)*c[0]+P(1,3,1)*c[1]+P(1,3,2)*c[2]+P(1,3,3)*c[3]); 0542 cP[ 8] = (P(2,0,0)*c[0]+P(2,0,1)*c[1]+P(2,0,2)*c[2]+P(2,0,3)*c[3]); 0543 cP[ 9] = (P(2,1,0)*c[0]+P(2,1,1)*c[1]+P(2,1,2)*c[2]+P(2,1,3)*c[3]); 0544 cP[10] = (P(2,2,0)*c[0]+P(2,2,1)*c[1]+P(2,2,2)*c[2]+P(2,2,3)*c[3]); 0545 cP[11] = (P(2,3,0)*c[0]+P(2,3,1)*c[1]+P(2,3,2)*c[2]+P(2,3,3)*c[3]); 0546 cP[12] = (P(3,0,0)*c[0]+P(3,0,1)*c[1]+P(3,0,2)*c[2]+P(3,0,3)*c[3]); 0547 cP[13] = (P(3,1,0)*c[0]+P(3,1,1)*c[1]+P(3,1,2)*c[2]+P(3,1,3)*c[3]); 0548 cP[14] = (P(3,2,0)*c[0]+P(3,2,1)*c[1]+P(3,2,2)*c[2]+P(3,2,3)*c[3]); 0549 cP[15] = (P(3,3,0)*c[0]+P(3,3,1)*c[1]+P(3,3,2)*c[2]+P(3,3,3)*c[3]); 0550 0551 bcP[0] = ( b[0]*cP[ 0] + b[1]*cP[ 1] + b[2]*cP[ 2] + b[3]*cP[ 3]); 0552 bcP[1] = ( b[0]*cP[ 4] + b[1]*cP[ 5] + b[2]*cP[ 6] + b[3]*cP[ 7]); 0553 bcP[2] = ( b[0]*cP[ 8] + b[1]*cP[ 9] + b[2]*cP[10] + b[3]*cP[11]); 0554 bcP[3] = ( b[0]*cP[12] + b[1]*cP[13] + b[2]*cP[14] + b[3]*cP[15]); 0555 0556 dbcP[0] = ( db[0]*cP[ 0] + db[1]*cP[ 1] + db[2]*cP[ 2] + db[3]*cP[ 3]); 0557 dbcP[1] = ( db[0]*cP[ 4] + db[1]*cP[ 5] + db[2]*cP[ 6] + db[3]*cP[ 7]); 0558 dbcP[2] = ( db[0]*cP[ 8] + db[1]*cP[ 9] + db[2]*cP[10] + db[3]*cP[11]); 0559 dbcP[3] = ( db[0]*cP[12] + db[1]*cP[13] + db[2]*cP[14] + db[3]*cP[15]); 0560 0561 *val = ( a[0]*bcP[0] + a[1]*bcP[1] + a[2]*bcP[2] + a[3]*bcP[3]); 0562 grad[0] = dxInv * 0563 (da[0]*bcP[0] + da[1]*bcP[1] + da[2]*bcP[2] + da[3]*bcP[3]); 0564 grad[1] = dyInv * 0565 (a[0]*dbcP[0] + a[1]*dbcP[1] + a[2]*dbcP[2] + a[3]*dbcP[3]); 0566 grad[2] = dzInv * 0567 (a[0]*(b[0]*(P(0,0,0)*dc[0]+P(0,0,1)*dc[1]+P(0,0,2)*dc[2]+P(0,0,3)*dc[3])+ 0568 b[1]*(P(0,1,0)*dc[0]+P(0,1,1)*dc[1]+P(0,1,2)*dc[2]+P(0,1,3)*dc[3])+ 0569 b[2]*(P(0,2,0)*dc[0]+P(0,2,1)*dc[1]+P(0,2,2)*dc[2]+P(0,2,3)*dc[3])+ 0570 b[3]*(P(0,3,0)*dc[0]+P(0,3,1)*dc[1]+P(0,3,2)*dc[2]+P(0,3,3)*dc[3]))+ 0571 a[1]*(b[0]*(P(1,0,0)*dc[0]+P(1,0,1)*dc[1]+P(1,0,2)*dc[2]+P(1,0,3)*dc[3])+ 0572 b[1]*(P(1,1,0)*dc[0]+P(1,1,1)*dc[1]+P(1,1,2)*dc[2]+P(1,1,3)*dc[3])+ 0573 b[2]*(P(1,2,0)*dc[0]+P(1,2,1)*dc[1]+P(1,2,2)*dc[2]+P(1,2,3)*dc[3])+ 0574 b[3]*(P(1,3,0)*dc[0]+P(1,3,1)*dc[1]+P(1,3,2)*dc[2]+P(1,3,3)*dc[3]))+ 0575 a[2]*(b[0]*(P(2,0,0)*dc[0]+P(2,0,1)*dc[1]+P(2,0,2)*dc[2]+P(2,0,3)*dc[3])+ 0576 b[1]*(P(2,1,0)*dc[0]+P(2,1,1)*dc[1]+P(2,1,2)*dc[2]+P(2,1,3)*dc[3])+ 0577 b[2]*(P(2,2,0)*dc[0]+P(2,2,1)*dc[1]+P(2,2,2)*dc[2]+P(2,2,3)*dc[3])+ 0578 b[3]*(P(2,3,0)*dc[0]+P(2,3,1)*dc[1]+P(2,3,2)*dc[2]+P(2,3,3)*dc[3]))+ 0579 a[3]*(b[0]*(P(3,0,0)*dc[0]+P(3,0,1)*dc[1]+P(3,0,2)*dc[2]+P(3,0,3)*dc[3])+ 0580 b[1]*(P(3,1,0)*dc[0]+P(3,1,1)*dc[1]+P(3,1,2)*dc[2]+P(3,1,3)*dc[3])+ 0581 b[2]*(P(3,2,0)*dc[0]+P(3,2,1)*dc[1]+P(3,2,2)*dc[2]+P(3,2,3)*dc[3])+ 0582 b[3]*(P(3,3,0)*dc[0]+P(3,3,1)*dc[1]+P(3,3,2)*dc[2]+P(3,3,3)*dc[3]))); 0583 #undef P 0584 0585 } 0586 0587 0588 0589 /* Value, gradient, and laplacian */ 0590 inline void 0591 eval_UBspline_3d_c_vgl (UBspline_3d_c * restrict spline, 0592 double x, double y, double z, 0593 complex_float* restrict val, complex_float* restrict grad, 0594 complex_float* restrict lapl) 0595 { 0596 x -= spline->x_grid.start; 0597 y -= spline->y_grid.start; 0598 z -= spline->z_grid.start; 0599 float ux = x*spline->x_grid.delta_inv; 0600 float uy = y*spline->y_grid.delta_inv; 0601 float uz = z*spline->z_grid.delta_inv; 0602 float ipartx, iparty, ipartz, tx, ty, tz; 0603 tx = modff (ux, &ipartx); int ix = (int) ipartx; 0604 ty = modff (uy, &iparty); int iy = (int) iparty; 0605 tz = modff (uz, &ipartz); int iz = (int) ipartz; 0606 0607 float tpx[4], tpy[4], tpz[4], a[4], b[4], c[4], da[4], db[4], dc[4], 0608 d2a[4], d2b[4], d2c[4]; 0609 complex_float cP[16], dcP[16], bcP[4], dbcP[4], d2bcP[4], bdcP[4]; 0610 tpx[0] = tx*tx*tx; tpx[1] = tx*tx; tpx[2] = tx; tpx[3] = 1.0; 0611 tpy[0] = ty*ty*ty; tpy[1] = ty*ty; tpy[2] = ty; tpy[3] = 1.0; 0612 tpz[0] = tz*tz*tz; tpz[1] = tz*tz; tpz[2] = tz; tpz[3] = 1.0; 0613 complex_float* restrict coefs = spline->coefs; 0614 0615 a[0] = ( Af[ 0]*tpx[0] + Af[ 1]*tpx[1] + Af[ 2]*tpx[2] + Af[ 3]*tpx[3]); 0616 a[1] = ( Af[ 4]*tpx[0] + Af[ 5]*tpx[1] + Af[ 6]*tpx[2] + Af[ 7]*tpx[3]); 0617 a[2] = ( Af[ 8]*tpx[0] + Af[ 9]*tpx[1] + Af[10]*tpx[2] + Af[11]*tpx[3]); 0618 a[3] = ( Af[12]*tpx[0] + Af[13]*tpx[1] + Af[14]*tpx[2] + Af[15]*tpx[3]); 0619 da[0] = ( dAf[ 1]*tpx[1] + dAf[ 2]*tpx[2] + dAf[ 3]*tpx[3]); 0620 da[1] = ( dAf[ 5]*tpx[1] + dAf[ 6]*tpx[2] + dAf[ 7]*tpx[3]); 0621 da[2] = ( dAf[ 9]*tpx[1] + dAf[10]*tpx[2] + dAf[11]*tpx[3]); 0622 da[3] = ( dAf[13]*tpx[1] + dAf[14]*tpx[2] + dAf[15]*tpx[3]); 0623 d2a[0] = (d2Af[ 2]*tpx[2] + d2Af[ 3]*tpx[3]); 0624 d2a[1] = (d2Af[ 6]*tpx[2] + d2Af[ 7]*tpx[3]); 0625 d2a[2] = (d2Af[10]*tpx[2] + d2Af[11]*tpx[3]); 0626 d2a[3] = (d2Af[14]*tpx[2] + d2Af[15]*tpx[3]); 0627 0628 b[0] = ( Af[ 0]*tpy[0] + Af[ 1]*tpy[1] + Af[ 2]*tpy[2] + Af[ 3]*tpy[3]); 0629 b[1] = ( Af[ 4]*tpy[0] + Af[ 5]*tpy[1] + Af[ 6]*tpy[2] + Af[ 7]*tpy[3]); 0630 b[2] = ( Af[ 8]*tpy[0] + Af[ 9]*tpy[1] + Af[10]*tpy[2] + Af[11]*tpy[3]); 0631 b[3] = ( Af[12]*tpy[0] + Af[13]*tpy[1] + Af[14]*tpy[2] + Af[15]*tpy[3]); 0632 db[0] = (dAf[ 1]*tpy[1] + dAf[ 2]*tpy[2] + dAf[ 3]*tpy[3]); 0633 db[1] = (dAf[ 5]*tpy[1] + dAf[ 6]*tpy[2] + dAf[ 7]*tpy[3]); 0634 db[2] = (dAf[ 9]*tpy[1] + dAf[10]*tpy[2] + dAf[11]*tpy[3]); 0635 db[3] = (dAf[13]*tpy[1] + dAf[14]*tpy[2] + dAf[15]*tpy[3]); 0636 d2b[0] = (d2Af[ 2]*tpy[2] + d2Af[ 3]*tpy[3]); 0637 d2b[1] = (d2Af[ 6]*tpy[2] + d2Af[ 7]*tpy[3]); 0638 d2b[2] = (d2Af[10]*tpy[2] + d2Af[11]*tpy[3]); 0639 d2b[3] = (d2Af[14]*tpy[2] + d2Af[15]*tpy[3]); 0640 0641 c[0] = ( Af[ 0]*tpz[0] + Af[ 1]*tpz[1] + Af[ 2]*tpz[2] + Af[ 3]*tpz[3]); 0642 c[1] = ( Af[ 4]*tpz[0] + Af[ 5]*tpz[1] + Af[ 6]*tpz[2] + Af[ 7]*tpz[3]); 0643 c[2] = ( Af[ 8]*tpz[0] + Af[ 9]*tpz[1] + Af[10]*tpz[2] + Af[11]*tpz[3]); 0644 c[3] = ( Af[12]*tpz[0] + Af[13]*tpz[1] + Af[14]*tpz[2] + Af[15]*tpz[3]); 0645 dc[0] = (dAf[ 1]*tpz[1] + dAf[ 2]*tpz[2] + dAf[ 3]*tpz[3]); 0646 dc[1] = (dAf[ 5]*tpz[1] + dAf[ 6]*tpz[2] + dAf[ 7]*tpz[3]); 0647 dc[2] = (dAf[ 9]*tpz[1] + dAf[10]*tpz[2] + dAf[11]*tpz[3]); 0648 dc[3] = (dAf[13]*tpz[1] + dAf[14]*tpz[2] + dAf[15]*tpz[3]); 0649 d2c[0] = (d2Af[ 2]*tpz[2] + d2Af[ 3]*tpz[3]); 0650 d2c[1] = (d2Af[ 6]*tpz[2] + d2Af[ 7]*tpz[3]); 0651 d2c[2] = (d2Af[10]*tpz[2] + d2Af[11]*tpz[3]); 0652 d2c[3] = (d2Af[14]*tpz[2] + d2Af[15]*tpz[3]); 0653 0654 int xs = spline->x_stride; 0655 int ys = spline->y_stride; 0656 0657 float dxInv = spline->x_grid.delta_inv; 0658 float dyInv = spline->y_grid.delta_inv; 0659 float dzInv = spline->z_grid.delta_inv; 0660 0661 #define P(i,j,k) coefs[(ix+(i))*xs+(iy+(j))*ys+(iz+(k))] 0662 cP[ 0] = (P(0,0,0)*c[0]+P(0,0,1)*c[1]+P(0,0,2)*c[2]+P(0,0,3)*c[3]); 0663 cP[ 1] = (P(0,1,0)*c[0]+P(0,1,1)*c[1]+P(0,1,2)*c[2]+P(0,1,3)*c[3]); 0664 cP[ 2] = (P(0,2,0)*c[0]+P(0,2,1)*c[1]+P(0,2,2)*c[2]+P(0,2,3)*c[3]); 0665 cP[ 3] = (P(0,3,0)*c[0]+P(0,3,1)*c[1]+P(0,3,2)*c[2]+P(0,3,3)*c[3]); 0666 cP[ 4] = (P(1,0,0)*c[0]+P(1,0,1)*c[1]+P(1,0,2)*c[2]+P(1,0,3)*c[3]); 0667 cP[ 5] = (P(1,1,0)*c[0]+P(1,1,1)*c[1]+P(1,1,2)*c[2]+P(1,1,3)*c[3]); 0668 cP[ 6] = (P(1,2,0)*c[0]+P(1,2,1)*c[1]+P(1,2,2)*c[2]+P(1,2,3)*c[3]); 0669 cP[ 7] = (P(1,3,0)*c[0]+P(1,3,1)*c[1]+P(1,3,2)*c[2]+P(1,3,3)*c[3]); 0670 cP[ 8] = (P(2,0,0)*c[0]+P(2,0,1)*c[1]+P(2,0,2)*c[2]+P(2,0,3)*c[3]); 0671 cP[ 9] = (P(2,1,0)*c[0]+P(2,1,1)*c[1]+P(2,1,2)*c[2]+P(2,1,3)*c[3]); 0672 cP[10] = (P(2,2,0)*c[0]+P(2,2,1)*c[1]+P(2,2,2)*c[2]+P(2,2,3)*c[3]); 0673 cP[11] = (P(2,3,0)*c[0]+P(2,3,1)*c[1]+P(2,3,2)*c[2]+P(2,3,3)*c[3]); 0674 cP[12] = (P(3,0,0)*c[0]+P(3,0,1)*c[1]+P(3,0,2)*c[2]+P(3,0,3)*c[3]); 0675 cP[13] = (P(3,1,0)*c[0]+P(3,1,1)*c[1]+P(3,1,2)*c[2]+P(3,1,3)*c[3]); 0676 cP[14] = (P(3,2,0)*c[0]+P(3,2,1)*c[1]+P(3,2,2)*c[2]+P(3,2,3)*c[3]); 0677 cP[15] = (P(3,3,0)*c[0]+P(3,3,1)*c[1]+P(3,3,2)*c[2]+P(3,3,3)*c[3]); 0678 0679 dcP[ 0] = (P(0,0,0)*dc[0]+P(0,0,1)*dc[1]+P(0,0,2)*dc[2]+P(0,0,3)*dc[3]); 0680 dcP[ 1] = (P(0,1,0)*dc[0]+P(0,1,1)*dc[1]+P(0,1,2)*dc[2]+P(0,1,3)*dc[3]); 0681 dcP[ 2] = (P(0,2,0)*dc[0]+P(0,2,1)*dc[1]+P(0,2,2)*dc[2]+P(0,2,3)*dc[3]); 0682 dcP[ 3] = (P(0,3,0)*dc[0]+P(0,3,1)*dc[1]+P(0,3,2)*dc[2]+P(0,3,3)*dc[3]); 0683 dcP[ 4] = (P(1,0,0)*dc[0]+P(1,0,1)*dc[1]+P(1,0,2)*dc[2]+P(1,0,3)*dc[3]); 0684 dcP[ 5] = (P(1,1,0)*dc[0]+P(1,1,1)*dc[1]+P(1,1,2)*dc[2]+P(1,1,3)*dc[3]); 0685 dcP[ 6] = (P(1,2,0)*dc[0]+P(1,2,1)*dc[1]+P(1,2,2)*dc[2]+P(1,2,3)*dc[3]); 0686 dcP[ 7] = (P(1,3,0)*dc[0]+P(1,3,1)*dc[1]+P(1,3,2)*dc[2]+P(1,3,3)*dc[3]); 0687 dcP[ 8] = (P(2,0,0)*dc[0]+P(2,0,1)*dc[1]+P(2,0,2)*dc[2]+P(2,0,3)*dc[3]); 0688 dcP[ 9] = (P(2,1,0)*dc[0]+P(2,1,1)*dc[1]+P(2,1,2)*dc[2]+P(2,1,3)*dc[3]); 0689 dcP[10] = (P(2,2,0)*dc[0]+P(2,2,1)*dc[1]+P(2,2,2)*dc[2]+P(2,2,3)*dc[3]); 0690 dcP[11] = (P(2,3,0)*dc[0]+P(2,3,1)*dc[1]+P(2,3,2)*dc[2]+P(2,3,3)*dc[3]); 0691 dcP[12] = (P(3,0,0)*dc[0]+P(3,0,1)*dc[1]+P(3,0,2)*dc[2]+P(3,0,3)*dc[3]); 0692 dcP[13] = (P(3,1,0)*dc[0]+P(3,1,1)*dc[1]+P(3,1,2)*dc[2]+P(3,1,3)*dc[3]); 0693 dcP[14] = (P(3,2,0)*dc[0]+P(3,2,1)*dc[1]+P(3,2,2)*dc[2]+P(3,2,3)*dc[3]); 0694 dcP[15] = (P(3,3,0)*dc[0]+P(3,3,1)*dc[1]+P(3,3,2)*dc[2]+P(3,3,3)*dc[3]); 0695 0696 bcP[0] = ( b[0]*cP[ 0] + b[1]*cP[ 1] + b[2]*cP[ 2] + b[3]*cP[ 3]); 0697 bcP[1] = ( b[0]*cP[ 4] + b[1]*cP[ 5] + b[2]*cP[ 6] + b[3]*cP[ 7]); 0698 bcP[2] = ( b[0]*cP[ 8] + b[1]*cP[ 9] + b[2]*cP[10] + b[3]*cP[11]); 0699 bcP[3] = ( b[0]*cP[12] + b[1]*cP[13] + b[2]*cP[14] + b[3]*cP[15]); 0700 0701 dbcP[0] = ( db[0]*cP[ 0] + db[1]*cP[ 1] + db[2]*cP[ 2] + db[3]*cP[ 3]); 0702 dbcP[1] = ( db[0]*cP[ 4] + db[1]*cP[ 5] + db[2]*cP[ 6] + db[3]*cP[ 7]); 0703 dbcP[2] = ( db[0]*cP[ 8] + db[1]*cP[ 9] + db[2]*cP[10] + db[3]*cP[11]); 0704 dbcP[3] = ( db[0]*cP[12] + db[1]*cP[13] + db[2]*cP[14] + db[3]*cP[15]); 0705 0706 bdcP[0] = ( b[0]*dcP[ 0] + b[1]*dcP[ 1] + b[2]*dcP[ 2] + b[3]*dcP[ 3]); 0707 bdcP[1] = ( b[0]*dcP[ 4] + b[1]*dcP[ 5] + b[2]*dcP[ 6] + b[3]*dcP[ 7]); 0708 bdcP[2] = ( b[0]*dcP[ 8] + b[1]*dcP[ 9] + b[2]*dcP[10] + b[3]*dcP[11]); 0709 bdcP[3] = ( b[0]*dcP[12] + b[1]*dcP[13] + b[2]*dcP[14] + b[3]*dcP[15]); 0710 0711 d2bcP[0] = ( d2b[0]*cP[ 0] + d2b[1]*cP[ 1] + d2b[2]*cP[ 2] + d2b[3]*cP[ 3]); 0712 d2bcP[1] = ( d2b[0]*cP[ 4] + d2b[1]*cP[ 5] + d2b[2]*cP[ 6] + d2b[3]*cP[ 7]); 0713 d2bcP[2] = ( d2b[0]*cP[ 8] + d2b[1]*cP[ 9] + d2b[2]*cP[10] + d2b[3]*cP[11]); 0714 d2bcP[3] = ( d2b[0]*cP[12] + d2b[1]*cP[13] + d2b[2]*cP[14] + d2b[3]*cP[15]); 0715 0716 0717 *val = 0718 ( a[0]*bcP[0] + a[1]*bcP[1] + a[2]*bcP[2] + a[3]*bcP[3]); 0719 0720 grad[0] = dxInv * 0721 (da[0]*bcP[0] + da[1]*bcP[1] + da[2]*bcP[2] + da[3]*bcP[3]); 0722 grad[1] = dyInv * 0723 (a[0]*dbcP[0] + a[1]*dbcP[1] + a[2]*dbcP[2] + a[3]*dbcP[3]); 0724 grad[2] = dzInv * 0725 (a[0]*bdcP[0] + a[1]*bdcP[1] + a[2]*bdcP[2] + a[3]*bdcP[3]); 0726 0727 *lapl = 0728 dxInv * dxInv * 0729 (d2a[0]*bcP[0] + d2a[1]*bcP[1] + d2a[2]*bcP[2] + d2a[3]*bcP[3]) 0730 0731 + dyInv * dyInv * 0732 (a[0]*d2bcP[0] + a[1]*d2bcP[1] + a[2]*d2bcP[2] + a[3]*d2bcP[3]) + 0733 0734 + dzInv * dzInv * 0735 (a[0]*(b[0]*(P(0,0,0)*d2c[0]+P(0,0,1)*d2c[1]+P(0,0,2)*d2c[2]+P(0,0,3)*d2c[3])+ 0736 b[1]*(P(0,1,0)*d2c[0]+P(0,1,1)*d2c[1]+P(0,1,2)*d2c[2]+P(0,1,3)*d2c[3])+ 0737 b[2]*(P(0,2,0)*d2c[0]+P(0,2,1)*d2c[1]+P(0,2,2)*d2c[2]+P(0,2,3)*d2c[3])+ 0738 b[3]*(P(0,3,0)*d2c[0]+P(0,3,1)*d2c[1]+P(0,3,2)*d2c[2]+P(0,3,3)*d2c[3]))+ 0739 a[1]*(b[0]*(P(1,0,0)*d2c[0]+P(1,0,1)*d2c[1]+P(1,0,2)*d2c[2]+P(1,0,3)*d2c[3])+ 0740 b[1]*(P(1,1,0)*d2c[0]+P(1,1,1)*d2c[1]+P(1,1,2)*d2c[2]+P(1,1,3)*d2c[3])+ 0741 b[2]*(P(1,2,0)*d2c[0]+P(1,2,1)*d2c[1]+P(1,2,2)*d2c[2]+P(1,2,3)*d2c[3])+ 0742 b[3]*(P(1,3,0)*d2c[0]+P(1,3,1)*d2c[1]+P(1,3,2)*d2c[2]+P(1,3,3)*d2c[3]))+ 0743 a[2]*(b[0]*(P(2,0,0)*d2c[0]+P(2,0,1)*d2c[1]+P(2,0,2)*d2c[2]+P(2,0,3)*d2c[3])+ 0744 b[1]*(P(2,1,0)*d2c[0]+P(2,1,1)*d2c[1]+P(2,1,2)*d2c[2]+P(2,1,3)*d2c[3])+ 0745 b[2]*(P(2,2,0)*d2c[0]+P(2,2,1)*d2c[1]+P(2,2,2)*d2c[2]+P(2,2,3)*d2c[3])+ 0746 b[3]*(P(2,3,0)*d2c[0]+P(2,3,1)*d2c[1]+P(2,3,2)*d2c[2]+P(2,3,3)*d2c[3]))+ 0747 a[3]*(b[0]*(P(3,0,0)*d2c[0]+P(3,0,1)*d2c[1]+P(3,0,2)*d2c[2]+P(3,0,3)*d2c[3])+ 0748 b[1]*(P(3,1,0)*d2c[0]+P(3,1,1)*d2c[1]+P(3,1,2)*d2c[2]+P(3,1,3)*d2c[3])+ 0749 b[2]*(P(3,2,0)*d2c[0]+P(3,2,1)*d2c[1]+P(3,2,2)*d2c[2]+P(3,2,3)*d2c[3])+ 0750 b[3]*(P(3,3,0)*d2c[0]+P(3,3,1)*d2c[1]+P(3,3,2)*d2c[2]+P(3,3,3)*d2c[3]))); 0751 #undef P 0752 0753 } 0754 0755 0756 0757 0758 0759 /* Value, gradient, and Hessian */ 0760 inline void 0761 eval_UBspline_3d_c_vgh (UBspline_3d_c * restrict spline, 0762 double x, double y, double z, 0763 complex_float* restrict val, complex_float* restrict grad, 0764 complex_float* restrict hess) 0765 { 0766 x -= spline->x_grid.start; 0767 y -= spline->y_grid.start; 0768 z -= spline->z_grid.start; 0769 float ux = x*spline->x_grid.delta_inv; 0770 float uy = y*spline->y_grid.delta_inv; 0771 float uz = z*spline->z_grid.delta_inv; 0772 ux = fmin (ux, (double)(spline->x_grid.num)-1.0e-5); 0773 uy = fmin (uy, (double)(spline->y_grid.num)-1.0e-5); 0774 uz = fmin (uz, (double)(spline->z_grid.num)-1.0e-5); 0775 float ipartx, iparty, ipartz, tx, ty, tz; 0776 tx = modff (ux, &ipartx); int ix = (int) ipartx; 0777 ty = modff (uy, &iparty); int iy = (int) iparty; 0778 tz = modff (uz, &ipartz); int iz = (int) ipartz; 0779 0780 float tpx[4], tpy[4], tpz[4], a[4], b[4], c[4], da[4], db[4], dc[4], 0781 d2a[4], d2b[4], d2c[4]; 0782 complex_float cP[16], dcP[16], d2cP[16], bcP[4], dbcP[4], 0783 d2bcP[4], dbdcP[4], bd2cP[4], bdcP[4]; 0784 tpx[0] = tx*tx*tx; tpx[1] = tx*tx; tpx[2] = tx; tpx[3] = 1.0; 0785 tpy[0] = ty*ty*ty; tpy[1] = ty*ty; tpy[2] = ty; tpy[3] = 1.0; 0786 tpz[0] = tz*tz*tz; tpz[1] = tz*tz; tpz[2] = tz; tpz[3] = 1.0; 0787 complex_float* restrict coefs = spline->coefs; 0788 0789 a[0] = ( Af[ 0]*tpx[0] + Af[ 1]*tpx[1] + Af[ 2]*tpx[2] + Af[ 3]*tpx[3]); 0790 a[1] = ( Af[ 4]*tpx[0] + Af[ 5]*tpx[1] + Af[ 6]*tpx[2] + Af[ 7]*tpx[3]); 0791 a[2] = ( Af[ 8]*tpx[0] + Af[ 9]*tpx[1] + Af[10]*tpx[2] + Af[11]*tpx[3]); 0792 a[3] = ( Af[12]*tpx[0] + Af[13]*tpx[1] + Af[14]*tpx[2] + Af[15]*tpx[3]); 0793 da[0] = ( dAf[ 1]*tpx[1] + dAf[ 2]*tpx[2] + dAf[ 3]*tpx[3]); 0794 da[1] = ( dAf[ 5]*tpx[1] + dAf[ 6]*tpx[2] + dAf[ 7]*tpx[3]); 0795 da[2] = ( dAf[ 9]*tpx[1] + dAf[10]*tpx[2] + dAf[11]*tpx[3]); 0796 da[3] = ( dAf[13]*tpx[1] + dAf[14]*tpx[2] + dAf[15]*tpx[3]); 0797 d2a[0] = (d2Af[ 2]*tpx[2] + d2Af[ 3]*tpx[3]); 0798 d2a[1] = (d2Af[ 6]*tpx[2] + d2Af[ 7]*tpx[3]); 0799 d2a[2] = (d2Af[10]*tpx[2] + d2Af[11]*tpx[3]); 0800 d2a[3] = (d2Af[14]*tpx[2] + d2Af[15]*tpx[3]); 0801 0802 b[0] = ( Af[ 0]*tpy[0] + Af[ 1]*tpy[1] + Af[ 2]*tpy[2] + Af[ 3]*tpy[3]); 0803 b[1] = ( Af[ 4]*tpy[0] + Af[ 5]*tpy[1] + Af[ 6]*tpy[2] + Af[ 7]*tpy[3]); 0804 b[2] = ( Af[ 8]*tpy[0] + Af[ 9]*tpy[1] + Af[10]*tpy[2] + Af[11]*tpy[3]); 0805 b[3] = ( Af[12]*tpy[0] + Af[13]*tpy[1] + Af[14]*tpy[2] + Af[15]*tpy[3]); 0806 db[0] = (dAf[ 1]*tpy[1] + dAf[ 2]*tpy[2] + dAf[ 3]*tpy[3]); 0807 db[1] = (dAf[ 5]*tpy[1] + dAf[ 6]*tpy[2] + dAf[ 7]*tpy[3]); 0808 db[2] = (dAf[ 9]*tpy[1] + dAf[10]*tpy[2] + dAf[11]*tpy[3]); 0809 db[3] = (dAf[13]*tpy[1] + dAf[14]*tpy[2] + dAf[15]*tpy[3]); 0810 d2b[0] = (d2Af[ 2]*tpy[2] + d2Af[ 3]*tpy[3]); 0811 d2b[1] = (d2Af[ 6]*tpy[2] + d2Af[ 7]*tpy[3]); 0812 d2b[2] = (d2Af[10]*tpy[2] + d2Af[11]*tpy[3]); 0813 d2b[3] = (d2Af[14]*tpy[2] + d2Af[15]*tpy[3]); 0814 0815 c[0] = ( Af[ 0]*tpz[0] + Af[ 1]*tpz[1] + Af[ 2]*tpz[2] + Af[ 3]*tpz[3]); 0816 c[1] = ( Af[ 4]*tpz[0] + Af[ 5]*tpz[1] + Af[ 6]*tpz[2] + Af[ 7]*tpz[3]); 0817 c[2] = ( Af[ 8]*tpz[0] + Af[ 9]*tpz[1] + Af[10]*tpz[2] + Af[11]*tpz[3]); 0818 c[3] = ( Af[12]*tpz[0] + Af[13]*tpz[1] + Af[14]*tpz[2] + Af[15]*tpz[3]); 0819 dc[0] = (dAf[ 1]*tpz[1] + dAf[ 2]*tpz[2] + dAf[ 3]*tpz[3]); 0820 dc[1] = (dAf[ 5]*tpz[1] + dAf[ 6]*tpz[2] + dAf[ 7]*tpz[3]); 0821 dc[2] = (dAf[ 9]*tpz[1] + dAf[10]*tpz[2] + dAf[11]*tpz[3]); 0822 dc[3] = (dAf[13]*tpz[1] + dAf[14]*tpz[2] + dAf[15]*tpz[3]); 0823 d2c[0] = (d2Af[ 2]*tpz[2] + d2Af[ 3]*tpz[3]); 0824 d2c[1] = (d2Af[ 6]*tpz[2] + d2Af[ 7]*tpz[3]); 0825 d2c[2] = (d2Af[10]*tpz[2] + d2Af[11]*tpz[3]); 0826 d2c[3] = (d2Af[14]*tpz[2] + d2Af[15]*tpz[3]); 0827 0828 int xs = spline->x_stride; 0829 int ys = spline->y_stride; 0830 int offmax = (ix+3)*xs + (iy+3)*ys + iz+3; 0831 0832 float dxInv = spline->x_grid.delta_inv; 0833 float dyInv = spline->y_grid.delta_inv; 0834 float dzInv = spline->z_grid.delta_inv; 0835 0836 #define P(i,j,k) coefs[(ix+(i))*xs+(iy+(j))*ys+(iz+(k))] 0837 cP[ 0] = (P(0,0,0)*c[0]+P(0,0,1)*c[1]+P(0,0,2)*c[2]+P(0,0,3)*c[3]); 0838 cP[ 1] = (P(0,1,0)*c[0]+P(0,1,1)*c[1]+P(0,1,2)*c[2]+P(0,1,3)*c[3]); 0839 cP[ 2] = (P(0,2,0)*c[0]+P(0,2,1)*c[1]+P(0,2,2)*c[2]+P(0,2,3)*c[3]); 0840 cP[ 3] = (P(0,3,0)*c[0]+P(0,3,1)*c[1]+P(0,3,2)*c[2]+P(0,3,3)*c[3]); 0841 cP[ 4] = (P(1,0,0)*c[0]+P(1,0,1)*c[1]+P(1,0,2)*c[2]+P(1,0,3)*c[3]); 0842 cP[ 5] = (P(1,1,0)*c[0]+P(1,1,1)*c[1]+P(1,1,2)*c[2]+P(1,1,3)*c[3]); 0843 cP[ 6] = (P(1,2,0)*c[0]+P(1,2,1)*c[1]+P(1,2,2)*c[2]+P(1,2,3)*c[3]); 0844 cP[ 7] = (P(1,3,0)*c[0]+P(1,3,1)*c[1]+P(1,3,2)*c[2]+P(1,3,3)*c[3]); 0845 cP[ 8] = (P(2,0,0)*c[0]+P(2,0,1)*c[1]+P(2,0,2)*c[2]+P(2,0,3)*c[3]); 0846 cP[ 9] = (P(2,1,0)*c[0]+P(2,1,1)*c[1]+P(2,1,2)*c[2]+P(2,1,3)*c[3]); 0847 cP[10] = (P(2,2,0)*c[0]+P(2,2,1)*c[1]+P(2,2,2)*c[2]+P(2,2,3)*c[3]); 0848 cP[11] = (P(2,3,0)*c[0]+P(2,3,1)*c[1]+P(2,3,2)*c[2]+P(2,3,3)*c[3]); 0849 cP[12] = (P(3,0,0)*c[0]+P(3,0,1)*c[1]+P(3,0,2)*c[2]+P(3,0,3)*c[3]); 0850 cP[13] = (P(3,1,0)*c[0]+P(3,1,1)*c[1]+P(3,1,2)*c[2]+P(3,1,3)*c[3]); 0851 cP[14] = (P(3,2,0)*c[0]+P(3,2,1)*c[1]+P(3,2,2)*c[2]+P(3,2,3)*c[3]); 0852 cP[15] = (P(3,3,0)*c[0]+P(3,3,1)*c[1]+P(3,3,2)*c[2]+P(3,3,3)*c[3]); 0853 0854 dcP[ 0] = (P(0,0,0)*dc[0]+P(0,0,1)*dc[1]+P(0,0,2)*dc[2]+P(0,0,3)*dc[3]); 0855 dcP[ 1] = (P(0,1,0)*dc[0]+P(0,1,1)*dc[1]+P(0,1,2)*dc[2]+P(0,1,3)*dc[3]); 0856 dcP[ 2] = (P(0,2,0)*dc[0]+P(0,2,1)*dc[1]+P(0,2,2)*dc[2]+P(0,2,3)*dc[3]); 0857 dcP[ 3] = (P(0,3,0)*dc[0]+P(0,3,1)*dc[1]+P(0,3,2)*dc[2]+P(0,3,3)*dc[3]); 0858 dcP[ 4] = (P(1,0,0)*dc[0]+P(1,0,1)*dc[1]+P(1,0,2)*dc[2]+P(1,0,3)*dc[3]); 0859 dcP[ 5] = (P(1,1,0)*dc[0]+P(1,1,1)*dc[1]+P(1,1,2)*dc[2]+P(1,1,3)*dc[3]); 0860 dcP[ 6] = (P(1,2,0)*dc[0]+P(1,2,1)*dc[1]+P(1,2,2)*dc[2]+P(1,2,3)*dc[3]); 0861 dcP[ 7] = (P(1,3,0)*dc[0]+P(1,3,1)*dc[1]+P(1,3,2)*dc[2]+P(1,3,3)*dc[3]); 0862 dcP[ 8] = (P(2,0,0)*dc[0]+P(2,0,1)*dc[1]+P(2,0,2)*dc[2]+P(2,0,3)*dc[3]); 0863 dcP[ 9] = (P(2,1,0)*dc[0]+P(2,1,1)*dc[1]+P(2,1,2)*dc[2]+P(2,1,3)*dc[3]); 0864 dcP[10] = (P(2,2,0)*dc[0]+P(2,2,1)*dc[1]+P(2,2,2)*dc[2]+P(2,2,3)*dc[3]); 0865 dcP[11] = (P(2,3,0)*dc[0]+P(2,3,1)*dc[1]+P(2,3,2)*dc[2]+P(2,3,3)*dc[3]); 0866 dcP[12] = (P(3,0,0)*dc[0]+P(3,0,1)*dc[1]+P(3,0,2)*dc[2]+P(3,0,3)*dc[3]); 0867 dcP[13] = (P(3,1,0)*dc[0]+P(3,1,1)*dc[1]+P(3,1,2)*dc[2]+P(3,1,3)*dc[3]); 0868 dcP[14] = (P(3,2,0)*dc[0]+P(3,2,1)*dc[1]+P(3,2,2)*dc[2]+P(3,2,3)*dc[3]); 0869 dcP[15] = (P(3,3,0)*dc[0]+P(3,3,1)*dc[1]+P(3,3,2)*dc[2]+P(3,3,3)*dc[3]); 0870 0871 d2cP[ 0] = (P(0,0,0)*d2c[0]+P(0,0,1)*d2c[1]+P(0,0,2)*d2c[2]+P(0,0,3)*d2c[3]); 0872 d2cP[ 1] = (P(0,1,0)*d2c[0]+P(0,1,1)*d2c[1]+P(0,1,2)*d2c[2]+P(0,1,3)*d2c[3]); 0873 d2cP[ 2] = (P(0,2,0)*d2c[0]+P(0,2,1)*d2c[1]+P(0,2,2)*d2c[2]+P(0,2,3)*d2c[3]); 0874 d2cP[ 3] = (P(0,3,0)*d2c[0]+P(0,3,1)*d2c[1]+P(0,3,2)*d2c[2]+P(0,3,3)*d2c[3]); 0875 d2cP[ 4] = (P(1,0,0)*d2c[0]+P(1,0,1)*d2c[1]+P(1,0,2)*d2c[2]+P(1,0,3)*d2c[3]); 0876 d2cP[ 5] = (P(1,1,0)*d2c[0]+P(1,1,1)*d2c[1]+P(1,1,2)*d2c[2]+P(1,1,3)*d2c[3]); 0877 d2cP[ 6] = (P(1,2,0)*d2c[0]+P(1,2,1)*d2c[1]+P(1,2,2)*d2c[2]+P(1,2,3)*d2c[3]); 0878 d2cP[ 7] = (P(1,3,0)*d2c[0]+P(1,3,1)*d2c[1]+P(1,3,2)*d2c[2]+P(1,3,3)*d2c[3]); 0879 d2cP[ 8] = (P(2,0,0)*d2c[0]+P(2,0,1)*d2c[1]+P(2,0,2)*d2c[2]+P(2,0,3)*d2c[3]); 0880 d2cP[ 9] = (P(2,1,0)*d2c[0]+P(2,1,1)*d2c[1]+P(2,1,2)*d2c[2]+P(2,1,3)*d2c[3]); 0881 d2cP[10] = (P(2,2,0)*d2c[0]+P(2,2,1)*d2c[1]+P(2,2,2)*d2c[2]+P(2,2,3)*d2c[3]); 0882 d2cP[11] = (P(2,3,0)*d2c[0]+P(2,3,1)*d2c[1]+P(2,3,2)*d2c[2]+P(2,3,3)*d2c[3]); 0883 d2cP[12] = (P(3,0,0)*d2c[0]+P(3,0,1)*d2c[1]+P(3,0,2)*d2c[2]+P(3,0,3)*d2c[3]); 0884 d2cP[13] = (P(3,1,0)*d2c[0]+P(3,1,1)*d2c[1]+P(3,1,2)*d2c[2]+P(3,1,3)*d2c[3]); 0885 d2cP[14] = (P(3,2,0)*d2c[0]+P(3,2,1)*d2c[1]+P(3,2,2)*d2c[2]+P(3,2,3)*d2c[3]); 0886 d2cP[15] = (P(3,3,0)*d2c[0]+P(3,3,1)*d2c[1]+P(3,3,2)*d2c[2]+P(3,3,3)*d2c[3]); 0887 0888 bcP[0] = ( b[0]*cP[ 0] + b[1]*cP[ 1] + b[2]*cP[ 2] + b[3]*cP[ 3]); 0889 bcP[1] = ( b[0]*cP[ 4] + b[1]*cP[ 5] + b[2]*cP[ 6] + b[3]*cP[ 7]); 0890 bcP[2] = ( b[0]*cP[ 8] + b[1]*cP[ 9] + b[2]*cP[10] + b[3]*cP[11]); 0891 bcP[3] = ( b[0]*cP[12] + b[1]*cP[13] + b[2]*cP[14] + b[3]*cP[15]); 0892 0893 dbcP[0] = ( db[0]*cP[ 0] + db[1]*cP[ 1] + db[2]*cP[ 2] + db[3]*cP[ 3]); 0894 dbcP[1] = ( db[0]*cP[ 4] + db[1]*cP[ 5] + db[2]*cP[ 6] + db[3]*cP[ 7]); 0895 dbcP[2] = ( db[0]*cP[ 8] + db[1]*cP[ 9] + db[2]*cP[10] + db[3]*cP[11]); 0896 dbcP[3] = ( db[0]*cP[12] + db[1]*cP[13] + db[2]*cP[14] + db[3]*cP[15]); 0897 0898 bdcP[0] = ( b[0]*dcP[ 0] + b[1]*dcP[ 1] + b[2]*dcP[ 2] + b[3]*dcP[ 3]); 0899 bdcP[1] = ( b[0]*dcP[ 4] + b[1]*dcP[ 5] + b[2]*dcP[ 6] + b[3]*dcP[ 7]); 0900 bdcP[2] = ( b[0]*dcP[ 8] + b[1]*dcP[ 9] + b[2]*dcP[10] + b[3]*dcP[11]); 0901 bdcP[3] = ( b[0]*dcP[12] + b[1]*dcP[13] + b[2]*dcP[14] + b[3]*dcP[15]); 0902 0903 bd2cP[0] = ( b[0]*d2cP[ 0] + b[1]*d2cP[ 1] + b[2]*d2cP[ 2] + b[3]*d2cP[ 3]); 0904 bd2cP[1] = ( b[0]*d2cP[ 4] + b[1]*d2cP[ 5] + b[2]*d2cP[ 6] + b[3]*d2cP[ 7]); 0905 bd2cP[2] = ( b[0]*d2cP[ 8] + b[1]*d2cP[ 9] + b[2]*d2cP[10] + b[3]*d2cP[11]); 0906 bd2cP[3] = ( b[0]*d2cP[12] + b[1]*d2cP[13] + b[2]*d2cP[14] + b[3]*d2cP[15]); 0907 0908 d2bcP[0] = ( d2b[0]*cP[ 0] + d2b[1]*cP[ 1] + d2b[2]*cP[ 2] + d2b[3]*cP[ 3]); 0909 d2bcP[1] = ( d2b[0]*cP[ 4] + d2b[1]*cP[ 5] + d2b[2]*cP[ 6] + d2b[3]*cP[ 7]); 0910 d2bcP[2] = ( d2b[0]*cP[ 8] + d2b[1]*cP[ 9] + d2b[2]*cP[10] + d2b[3]*cP[11]); 0911 d2bcP[3] = ( d2b[0]*cP[12] + d2b[1]*cP[13] + d2b[2]*cP[14] + d2b[3]*cP[15]); 0912 0913 dbdcP[0] = ( db[0]*dcP[ 0] + db[1]*dcP[ 1] + db[2]*dcP[ 2] + db[3]*dcP[ 3]); 0914 dbdcP[1] = ( db[0]*dcP[ 4] + db[1]*dcP[ 5] + db[2]*dcP[ 6] + db[3]*dcP[ 7]); 0915 dbdcP[2] = ( db[0]*dcP[ 8] + db[1]*dcP[ 9] + db[2]*dcP[10] + db[3]*dcP[11]); 0916 dbdcP[3] = ( db[0]*dcP[12] + db[1]*dcP[13] + db[2]*dcP[14] + db[3]*dcP[15]); 0917 0918 *val = a[0]*bcP[0] + a[1]*bcP[1] + a[2]*bcP[2] + a[3]*bcP[3]; 0919 grad[0] = dxInv * 0920 (da[0] *bcP[0] + da[1]*bcP[1] + da[2]*bcP[2] + da[3]*bcP[3]); 0921 grad[1] = dyInv * 0922 (a[0]*dbcP[0] + a[1]*dbcP[1] + a[2]*dbcP[2] + a[3]*dbcP[3]); 0923 grad[2] = dzInv * 0924 (a[0]*bdcP[0] + a[1]*bdcP[1] + a[2]*bdcP[2] + a[3]*bdcP[3]); 0925 // d2x 0926 hess[0] = dxInv * dxInv * 0927 (d2a[0]*bcP[0] + d2a[1]*bcP[1] + d2a[2]*bcP[2] + d2a[3]*bcP[3]); 0928 // dx dy 0929 hess[1] = dxInv * dyInv * 0930 (da[0]*dbcP[0] + da[1]*dbcP[1] + da[2]*dbcP[2] + da[3]*dbcP[3]); 0931 hess[3] = hess[1]; 0932 // dx dz; 0933 hess[2] = dxInv * dzInv * 0934 (da[0]*bdcP[0] + da[1]*bdcP[1] + da[2]*bdcP[2] + da[3]*bdcP[3]); 0935 hess[6] = hess[2]; 0936 // d2y 0937 hess[4] = dyInv * dyInv * 0938 (a[0]*d2bcP[0] + a[1]*d2bcP[1] + a[2]*d2bcP[2] + a[3]*d2bcP[3]); 0939 // dy dz 0940 hess[5] = dyInv * dzInv * 0941 (a[0]*dbdcP[0] + a[1]*dbdcP[1] + a[2]*dbdcP[2] + a[3]*dbdcP[3]); 0942 hess[7] = hess[5]; 0943 // d2z 0944 hess[8] = dzInv * dzInv * 0945 (a[0]*bd2cP[0] + a[1]*bd2cP[1] + a[2]*bd2cP[2] + a[3]*bd2cP[3]); 0946 #undef P 0947 0948 } 0949 0950 #endif