872{
879 double ipartx, iparty, ipartz, tx, ty, tz;
880 tx = modf (ux, &ipartx); int ix = (int) ipartx;
881 ty = modf (uy, &iparty); int iy = (int) iparty;
882 tz = modf (uz, &ipartz); int iz = (int) ipartz;
883
884 double tpx[4], tpy[4], tpz[4], a[4],
b[4], c[4],
885 da[4], db[4], dc[4], d2a[4], d2b[4], d2c[4],
886 d3a[4], d3b[4], d3c[4];
887 tpx[0] = tx*tx*tx; tpx[1] = tx*tx; tpx[2] = tx; tpx[3] = 1.0;
888 tpy[0] = ty*ty*ty; tpy[1] = ty*ty; tpy[2] = ty; tpy[3] = 1.0;
889 tpz[0] = tz*tz*tz; tpz[1] = tz*tz; tpz[2] = tz; tpz[3] = 1.0;
891
892 a[0] = (
Ad[ 0]*tpx[0] +
Ad[ 1]*tpx[1] +
Ad[ 2]*tpx[2] +
Ad[ 3]*tpx[3]);
893 a[1] = (
Ad[ 4]*tpx[0] +
Ad[ 5]*tpx[1] +
Ad[ 6]*tpx[2] +
Ad[ 7]*tpx[3]);
894 a[2] = (
Ad[ 8]*tpx[0] +
Ad[ 9]*tpx[1] +
Ad[10]*tpx[2] +
Ad[11]*tpx[3]);
895 a[3] = (
Ad[12]*tpx[0] +
Ad[13]*tpx[1] +
Ad[14]*tpx[2] +
Ad[15]*tpx[3]);
896 da[0] = (
dAd[ 0]*tpx[0] +
dAd[ 1]*tpx[1] +
dAd[ 2]*tpx[2] +
dAd[ 3]*tpx[3]);
897 da[1] = (
dAd[ 4]*tpx[0] +
dAd[ 5]*tpx[1] +
dAd[ 6]*tpx[2] +
dAd[ 7]*tpx[3]);
898 da[2] = (
dAd[ 8]*tpx[0] +
dAd[ 9]*tpx[1] +
dAd[10]*tpx[2] +
dAd[11]*tpx[3]);
899 da[3] = (
dAd[12]*tpx[0] +
dAd[13]*tpx[1] +
dAd[14]*tpx[2] +
dAd[15]*tpx[3]);
900 d2a[0] = (
d2Ad[ 0]*tpx[0] +
d2Ad[ 1]*tpx[1] +
d2Ad[ 2]*tpx[2] +
d2Ad[ 3]*tpx[3]);
901 d2a[1] = (
d2Ad[ 4]*tpx[0] +
d2Ad[ 5]*tpx[1] +
d2Ad[ 6]*tpx[2] +
d2Ad[ 7]*tpx[3]);
902 d2a[2] = (
d2Ad[ 8]*tpx[0] +
d2Ad[ 9]*tpx[1] +
d2Ad[10]*tpx[2] +
d2Ad[11]*tpx[3]);
903 d2a[3] = (
d2Ad[12]*tpx[0] +
d2Ad[13]*tpx[1] +
d2Ad[14]*tpx[2] +
d2Ad[15]*tpx[3]);
904 d3a[0] = (
d3Ad[ 3]*tpx[3]);
905 d3a[1] = (
d3Ad[ 7]*tpx[3]);
906 d3a[2] = (
d3Ad[11]*tpx[3]);
907 d3a[3] = (
d3Ad[15]*tpx[3]);
908
909 b[0] = (
Ad[ 0]*tpy[0] +
Ad[ 1]*tpy[1] +
Ad[ 2]*tpy[2] +
Ad[ 3]*tpy[3]);
910 b[1] = (
Ad[ 4]*tpy[0] +
Ad[ 5]*tpy[1] +
Ad[ 6]*tpy[2] +
Ad[ 7]*tpy[3]);
911 b[2] = (
Ad[ 8]*tpy[0] +
Ad[ 9]*tpy[1] +
Ad[10]*tpy[2] +
Ad[11]*tpy[3]);
912 b[3] = (
Ad[12]*tpy[0] +
Ad[13]*tpy[1] +
Ad[14]*tpy[2] +
Ad[15]*tpy[3]);
913 db[0] = (
dAd[ 0]*tpy[0] +
dAd[ 1]*tpy[1] +
dAd[ 2]*tpy[2] +
dAd[ 3]*tpy[3]);
914 db[1] = (
dAd[ 4]*tpy[0] +
dAd[ 5]*tpy[1] +
dAd[ 6]*tpy[2] +
dAd[ 7]*tpy[3]);
915 db[2] = (
dAd[ 8]*tpy[0] +
dAd[ 9]*tpy[1] +
dAd[10]*tpy[2] +
dAd[11]*tpy[3]);
916 db[3] = (
dAd[12]*tpy[0] +
dAd[13]*tpy[1] +
dAd[14]*tpy[2] +
dAd[15]*tpy[3]);
917 d2b[0] = (
d2Ad[ 0]*tpy[0] +
d2Ad[ 1]*tpy[1] +
d2Ad[ 2]*tpy[2] +
d2Ad[ 3]*tpy[3]);
918 d2b[1] = (
d2Ad[ 4]*tpy[0] +
d2Ad[ 5]*tpy[1] +
d2Ad[ 6]*tpy[2] +
d2Ad[ 7]*tpy[3]);
919 d2b[2] = (
d2Ad[ 8]*tpy[0] +
d2Ad[ 9]*tpy[1] +
d2Ad[10]*tpy[2] +
d2Ad[11]*tpy[3]);
920 d2b[3] = (
d2Ad[12]*tpy[0] +
d2Ad[13]*tpy[1] +
d2Ad[14]*tpy[2] +
d2Ad[15]*tpy[3]);
921 d3b[0] = (
d3Ad[ 3]*tpy[3]);
922 d3b[1] = (
d3Ad[ 7]*tpy[3]);
923 d3b[2] = (
d3Ad[11]*tpy[3]);
924 d3b[3] = (
d3Ad[15]*tpy[3]);
925
926 c[0] = (
Ad[ 0]*tpz[0] +
Ad[ 1]*tpz[1] +
Ad[ 2]*tpz[2] +
Ad[ 3]*tpz[3]);
927 c[1] = (
Ad[ 4]*tpz[0] +
Ad[ 5]*tpz[1] +
Ad[ 6]*tpz[2] +
Ad[ 7]*tpz[3]);
928 c[2] = (
Ad[ 8]*tpz[0] +
Ad[ 9]*tpz[1] +
Ad[10]*tpz[2] +
Ad[11]*tpz[3]);
929 c[3] = (
Ad[12]*tpz[0] +
Ad[13]*tpz[1] +
Ad[14]*tpz[2] +
Ad[15]*tpz[3]);
930 dc[0] = (
dAd[ 0]*tpz[0] +
dAd[ 1]*tpz[1] +
dAd[ 2]*tpz[2] +
dAd[ 3]*tpz[3]);
931 dc[1] = (
dAd[ 4]*tpz[0] +
dAd[ 5]*tpz[1] +
dAd[ 6]*tpz[2] +
dAd[ 7]*tpz[3]);
932 dc[2] = (
dAd[ 8]*tpz[0] +
dAd[ 9]*tpz[1] +
dAd[10]*tpz[2] +
dAd[11]*tpz[3]);
933 dc[3] = (
dAd[12]*tpz[0] +
dAd[13]*tpz[1] +
dAd[14]*tpz[2] +
dAd[15]*tpz[3]);
934 d2c[0] = (
d2Ad[ 0]*tpz[0] +
d2Ad[ 1]*tpz[1] +
d2Ad[ 2]*tpz[2] +
d2Ad[ 3]*tpz[3]);
935 d2c[1] = (
d2Ad[ 4]*tpz[0] +
d2Ad[ 5]*tpz[1] +
d2Ad[ 6]*tpz[2] +
d2Ad[ 7]*tpz[3]);
936 d2c[2] = (
d2Ad[ 8]*tpz[0] +
d2Ad[ 9]*tpz[1] +
d2Ad[10]*tpz[2] +
d2Ad[11]*tpz[3]);
937 d2c[3] = (
d2Ad[12]*tpz[0] +
d2Ad[13]*tpz[1] +
d2Ad[14]*tpz[2] +
d2Ad[15]*tpz[3]);
938 d3c[0] = (
d3Ad[ 3]*tpz[3]);
939 d3c[1] = (
d3Ad[ 7]*tpz[3]);
940 d3c[2] = (
d3Ad[11]*tpz[3]);
941 d3c[3] = (
d3Ad[15]*tpz[3]);
942
946
948 vals[n] = 0.0;
949 grads[3*n+0] = grads[3*n+1] = grads[3*n+2] = 0.0;
950 for (int i=0; i<9; i++)
951 hess[9*n+i] = 0.0;
952 for (int i=0; i<27; i++)
953 gradhess[27*n+i] = 0.0;
954 }
955
956 for (int i=0; i<4; i++)
957 for (int j=0; j<4; j++)
958 for (int k=0; k<4; k++) {
959 double abc = a[i]*
b[j]*c[k];
960 double dabc[3], d2abc[6], d3abc[10];
961 dabc[0] = da[i]*
b[j]* c[k];
962 dabc[1] = a[i]*db[j]* c[k];
963 dabc[2] = a[i]*
b[j]*dc[k];
964 d2abc[0] = d2a[i]*
b[j]* c[k];
965 d2abc[1] = da[i]* db[j]* c[k];
966 d2abc[2] = da[i]*
b[j]* dc[k];
967 d2abc[3] = a[i]*d2b[j]* c[k];
968 d2abc[4] = a[i]* db[j]* dc[k];
969 d2abc[5] = a[i]*
b[j]*d2c[k];
970
971 d3abc[0] = d3a[i]*
b[j]* c[k];
972 d3abc[1] = d2a[i]* db[j]* c[k];
973 d3abc[2] = d2a[i]*
b[j]* dc[k];
974 d3abc[3] = da[i]*d2b[j]* c[k];
975 d3abc[4] = da[i]* db[j]* dc[k];
976 d3abc[5] = da[i]*
b[j]*d2c[k];
977 d3abc[6] = a[i]*d3b[j]* c[k];
978 d3abc[7] = a[i]*d2b[j]* dc[k];
979 d3abc[8] = a[i]* db[j]*d2c[k];
980 d3abc[9] = a[i]*
b[j]*d3c[k];
981
982 double*
restrict coefs = spline->
coefs + ((ix+i)*xs + (iy+j)*ys + (iz+k)*zs);
984 vals[n] += abc *coefs[n];
985 grads[3*n+0] += dabc[0]*coefs[n];
986 grads[3*n+1] += dabc[1]*coefs[n];
987 grads[3*n+2] += dabc[2]*coefs[n];
988 hess [9*n+0] += d2abc[0]*coefs[n];
989 hess [9*n+1] += d2abc[1]*coefs[n];
990 hess [9*n+2] += d2abc[2]*coefs[n];
991 hess [9*n+4] += d2abc[3]*coefs[n];
992 hess [9*n+5] += d2abc[4]*coefs[n];
993 hess [9*n+8] += d2abc[5]*coefs[n];
994
995 gradhess [27*n+0 ] += d3abc[0]*coefs[n];
996 gradhess [27*n+1 ] += d3abc[1]*coefs[n];
997 gradhess [27*n+2 ] += d3abc[2]*coefs[n];
998 gradhess [27*n+4 ] += d3abc[3]*coefs[n];
999 gradhess [27*n+5 ] += d3abc[4]*coefs[n];
1000 gradhess [27*n+8 ] += d3abc[5]*coefs[n];
1001 gradhess [27*n+13] += d3abc[6]*coefs[n];
1002 gradhess [27*n+14] += d3abc[7]*coefs[n];
1003 gradhess [27*n+17] += d3abc[8]*coefs[n];
1004 gradhess [27*n+26] += d3abc[9]*coefs[n];
1005 }
1006 }
1007
1012 grads[3*n+0] *= dxInv;
1013 grads[3*n+1] *= dyInv;
1014 grads[3*n+2] *= dzInv;
1015 hess [9*n+0] *= dxInv*dxInv;
1016 hess [9*n+4] *= dyInv*dyInv;
1017 hess [9*n+8] *= dzInv*dzInv;
1018 hess [9*n+1] *= dxInv*dyInv;
1019 hess [9*n+2] *= dxInv*dzInv;
1020 hess [9*n+5] *= dyInv*dzInv;
1021
1022 hess [9*n+3] = hess[9*n+1];
1023 hess [9*n+6] = hess[9*n+2];
1024 hess [9*n+7] = hess[9*n+5];
1025
1026 gradhess [27*n+0 ] *= dxInv*dxInv*dxInv;
1027 gradhess [27*n+1 ] *= dxInv*dxInv*dyInv;
1028 gradhess [27*n+2 ] *= dxInv*dxInv*dzInv;
1029 gradhess [27*n+4 ] *= dxInv*dyInv*dyInv;
1030 gradhess [27*n+5 ] *= dxInv*dyInv*dzInv;
1031 gradhess [27*n+8 ] *= dxInv*dzInv*dzInv;
1032 gradhess [27*n+13] *= dyInv*dyInv*dyInv;
1033 gradhess [27*n+14] *= dyInv*dyInv*dzInv;
1034 gradhess [27*n+17] *= dyInv*dzInv*dzInv;
1035 gradhess [27*n+26] *= dzInv*dzInv*dzInv;
1036
1037
1038 gradhess [27*n+9 ] = gradhess [27*n+3 ] = gradhess [27*n+1 ];
1039 gradhess [27*n+18 ] = gradhess [27*n+6 ] = gradhess [27*n+2 ];
1040 gradhess [27*n+22 ] = gradhess [27*n+16 ] = gradhess [27*n+14];
1041 gradhess [27*n+12 ] = gradhess [27*n+10 ] = gradhess [27*n+4 ];
1042 gradhess [27*n+24 ] = gradhess [27*n+20 ] = gradhess [27*n+8 ];
1043 gradhess [27*n+25 ] = gradhess [27*n+23 ] = gradhess [27*n+17];
1044 gradhess [27*n+21 ] = gradhess [27*n+19 ] = gradhess [27*n+15] = gradhess [27*n+11 ] = gradhess [27*n+7 ] = gradhess [27*n+5];
1045
1046 }
1047}
const double *restrict d3Ad