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