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