765{
775 float ipartx, iparty, ipartz, tx, ty, tz;
776 tx = modff (ux, &ipartx); int ix = (int) ipartx;
777 ty = modff (uy, &iparty); int iy = (int) iparty;
778 tz = modff (uz, &ipartz); int iz = (int) ipartz;
779
780 float tpx[4], tpy[4], tpz[4], a[4],
b[4], c[4], da[4], db[4], dc[4],
781 d2a[4], d2b[4], d2c[4];
783 d2bcP[4], dbdcP[4], bd2cP[4], bdcP[4];
784 tpx[0] = tx*tx*tx; tpx[1] = tx*tx; tpx[2] = tx; tpx[3] = 1.0;
785 tpy[0] = ty*ty*ty; tpy[1] = ty*ty; tpy[2] = ty; tpy[3] = 1.0;
786 tpz[0] = tz*tz*tz; tpz[1] = tz*tz; tpz[2] = tz; tpz[3] = 1.0;
788
789 a[0] = (
Af[ 0]*tpx[0] +
Af[ 1]*tpx[1] +
Af[ 2]*tpx[2] +
Af[ 3]*tpx[3]);
790 a[1] = (
Af[ 4]*tpx[0] +
Af[ 5]*tpx[1] +
Af[ 6]*tpx[2] +
Af[ 7]*tpx[3]);
791 a[2] = (
Af[ 8]*tpx[0] +
Af[ 9]*tpx[1] +
Af[10]*tpx[2] +
Af[11]*tpx[3]);
792 a[3] = (
Af[12]*tpx[0] +
Af[13]*tpx[1] +
Af[14]*tpx[2] +
Af[15]*tpx[3]);
793 da[0] = (
dAf[ 1]*tpx[1] +
dAf[ 2]*tpx[2] +
dAf[ 3]*tpx[3]);
794 da[1] = (
dAf[ 5]*tpx[1] +
dAf[ 6]*tpx[2] +
dAf[ 7]*tpx[3]);
795 da[2] = (
dAf[ 9]*tpx[1] +
dAf[10]*tpx[2] +
dAf[11]*tpx[3]);
796 da[3] = (
dAf[13]*tpx[1] +
dAf[14]*tpx[2] +
dAf[15]*tpx[3]);
797 d2a[0] = (
d2Af[ 2]*tpx[2] +
d2Af[ 3]*tpx[3]);
798 d2a[1] = (
d2Af[ 6]*tpx[2] +
d2Af[ 7]*tpx[3]);
799 d2a[2] = (
d2Af[10]*tpx[2] +
d2Af[11]*tpx[3]);
800 d2a[3] = (
d2Af[14]*tpx[2] +
d2Af[15]*tpx[3]);
801
802 b[0] = (
Af[ 0]*tpy[0] +
Af[ 1]*tpy[1] +
Af[ 2]*tpy[2] +
Af[ 3]*tpy[3]);
803 b[1] = (
Af[ 4]*tpy[0] +
Af[ 5]*tpy[1] +
Af[ 6]*tpy[2] +
Af[ 7]*tpy[3]);
804 b[2] = (
Af[ 8]*tpy[0] +
Af[ 9]*tpy[1] +
Af[10]*tpy[2] +
Af[11]*tpy[3]);
805 b[3] = (
Af[12]*tpy[0] +
Af[13]*tpy[1] +
Af[14]*tpy[2] +
Af[15]*tpy[3]);
806 db[0] = (
dAf[ 1]*tpy[1] +
dAf[ 2]*tpy[2] +
dAf[ 3]*tpy[3]);
807 db[1] = (
dAf[ 5]*tpy[1] +
dAf[ 6]*tpy[2] +
dAf[ 7]*tpy[3]);
808 db[2] = (
dAf[ 9]*tpy[1] +
dAf[10]*tpy[2] +
dAf[11]*tpy[3]);
809 db[3] = (
dAf[13]*tpy[1] +
dAf[14]*tpy[2] +
dAf[15]*tpy[3]);
810 d2b[0] = (
d2Af[ 2]*tpy[2] +
d2Af[ 3]*tpy[3]);
811 d2b[1] = (
d2Af[ 6]*tpy[2] +
d2Af[ 7]*tpy[3]);
812 d2b[2] = (
d2Af[10]*tpy[2] +
d2Af[11]*tpy[3]);
813 d2b[3] = (
d2Af[14]*tpy[2] +
d2Af[15]*tpy[3]);
814
815 c[0] = (
Af[ 0]*tpz[0] +
Af[ 1]*tpz[1] +
Af[ 2]*tpz[2] +
Af[ 3]*tpz[3]);
816 c[1] = (
Af[ 4]*tpz[0] +
Af[ 5]*tpz[1] +
Af[ 6]*tpz[2] +
Af[ 7]*tpz[3]);
817 c[2] = (
Af[ 8]*tpz[0] +
Af[ 9]*tpz[1] +
Af[10]*tpz[2] +
Af[11]*tpz[3]);
818 c[3] = (
Af[12]*tpz[0] +
Af[13]*tpz[1] +
Af[14]*tpz[2] +
Af[15]*tpz[3]);
819 dc[0] = (
dAf[ 1]*tpz[1] +
dAf[ 2]*tpz[2] +
dAf[ 3]*tpz[3]);
820 dc[1] = (
dAf[ 5]*tpz[1] +
dAf[ 6]*tpz[2] +
dAf[ 7]*tpz[3]);
821 dc[2] = (
dAf[ 9]*tpz[1] +
dAf[10]*tpz[2] +
dAf[11]*tpz[3]);
822 dc[3] = (
dAf[13]*tpz[1] +
dAf[14]*tpz[2] +
dAf[15]*tpz[3]);
823 d2c[0] = (
d2Af[ 2]*tpz[2] +
d2Af[ 3]*tpz[3]);
824 d2c[1] = (
d2Af[ 6]*tpz[2] +
d2Af[ 7]*tpz[3]);
825 d2c[2] = (
d2Af[10]*tpz[2] +
d2Af[11]*tpz[3]);
826 d2c[3] = (
d2Af[14]*tpz[2] +
d2Af[15]*tpz[3]);
827
830 int offmax = (ix+3)*xs + (iy+3)*ys + iz+3;
831
835
836#define P(i,j,k) coefs[(ix+(i))*xs+(iy+(j))*ys+(iz+(k))]
837 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]);
838 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]);
839 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]);
840 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]);
841 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]);
842 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]);
843 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]);
844 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]);
845 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]);
846 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]);
847 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]);
848 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]);
849 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]);
850 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]);
851 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]);
852 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]);
853
854 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]);
855 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]);
856 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]);
857 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]);
858 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]);
859 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]);
860 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]);
861 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]);
862 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]);
863 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]);
864 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]);
865 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]);
866 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]);
867 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]);
868 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]);
869 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]);
870
871 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]);
872 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]);
873 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]);
874 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]);
875 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]);
876 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]);
877 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]);
878 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]);
879 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]);
880 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]);
881 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]);
882 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]);
883 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]);
884 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]);
885 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]);
886 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]);
887
888 bcP[0] = (
b[0]*cP[ 0] +
b[1]*cP[ 1] +
b[2]*cP[ 2] +
b[3]*cP[ 3]);
889 bcP[1] = (
b[0]*cP[ 4] +
b[1]*cP[ 5] +
b[2]*cP[ 6] +
b[3]*cP[ 7]);
890 bcP[2] = (
b[0]*cP[ 8] +
b[1]*cP[ 9] +
b[2]*cP[10] +
b[3]*cP[11]);
891 bcP[3] = (
b[0]*cP[12] +
b[1]*cP[13] +
b[2]*cP[14] +
b[3]*cP[15]);
892
893 dbcP[0] = ( db[0]*cP[ 0] + db[1]*cP[ 1] + db[2]*cP[ 2] + db[3]*cP[ 3]);
894 dbcP[1] = ( db[0]*cP[ 4] + db[1]*cP[ 5] + db[2]*cP[ 6] + db[3]*cP[ 7]);
895 dbcP[2] = ( db[0]*cP[ 8] + db[1]*cP[ 9] + db[2]*cP[10] + db[3]*cP[11]);
896 dbcP[3] = ( db[0]*cP[12] + db[1]*cP[13] + db[2]*cP[14] + db[3]*cP[15]);
897
898 bdcP[0] = (
b[0]*dcP[ 0] +
b[1]*dcP[ 1] +
b[2]*dcP[ 2] +
b[3]*dcP[ 3]);
899 bdcP[1] = (
b[0]*dcP[ 4] +
b[1]*dcP[ 5] +
b[2]*dcP[ 6] +
b[3]*dcP[ 7]);
900 bdcP[2] = (
b[0]*dcP[ 8] +
b[1]*dcP[ 9] +
b[2]*dcP[10] +
b[3]*dcP[11]);
901 bdcP[3] = (
b[0]*dcP[12] +
b[1]*dcP[13] +
b[2]*dcP[14] +
b[3]*dcP[15]);
902
903 bd2cP[0] = (
b[0]*d2cP[ 0] +
b[1]*d2cP[ 1] +
b[2]*d2cP[ 2] +
b[3]*d2cP[ 3]);
904 bd2cP[1] = (
b[0]*d2cP[ 4] +
b[1]*d2cP[ 5] +
b[2]*d2cP[ 6] +
b[3]*d2cP[ 7]);
905 bd2cP[2] = (
b[0]*d2cP[ 8] +
b[1]*d2cP[ 9] +
b[2]*d2cP[10] +
b[3]*d2cP[11]);
906 bd2cP[3] = (
b[0]*d2cP[12] +
b[1]*d2cP[13] +
b[2]*d2cP[14] +
b[3]*d2cP[15]);
907
908 d2bcP[0] = ( d2b[0]*cP[ 0] + d2b[1]*cP[ 1] + d2b[2]*cP[ 2] + d2b[3]*cP[ 3]);
909 d2bcP[1] = ( d2b[0]*cP[ 4] + d2b[1]*cP[ 5] + d2b[2]*cP[ 6] + d2b[3]*cP[ 7]);
910 d2bcP[2] = ( d2b[0]*cP[ 8] + d2b[1]*cP[ 9] + d2b[2]*cP[10] + d2b[3]*cP[11]);
911 d2bcP[3] = ( d2b[0]*cP[12] + d2b[1]*cP[13] + d2b[2]*cP[14] + d2b[3]*cP[15]);
912
913 dbdcP[0] = ( db[0]*dcP[ 0] + db[1]*dcP[ 1] + db[2]*dcP[ 2] + db[3]*dcP[ 3]);
914 dbdcP[1] = ( db[0]*dcP[ 4] + db[1]*dcP[ 5] + db[2]*dcP[ 6] + db[3]*dcP[ 7]);
915 dbdcP[2] = ( db[0]*dcP[ 8] + db[1]*dcP[ 9] + db[2]*dcP[10] + db[3]*dcP[11]);
916 dbdcP[3] = ( db[0]*dcP[12] + db[1]*dcP[13] + db[2]*dcP[14] + db[3]*dcP[15]);
917
918 *val = a[0]*bcP[0] + a[1]*bcP[1] + a[2]*bcP[2] + a[3]*bcP[3];
919 grad[0] = dxInv *
920 (da[0] *bcP[0] + da[1]*bcP[1] + da[2]*bcP[2] + da[3]*bcP[3]);
921 grad[1] = dyInv *
922 (a[0]*dbcP[0] + a[1]*dbcP[1] + a[2]*dbcP[2] + a[3]*dbcP[3]);
923 grad[2] = dzInv *
924 (a[0]*bdcP[0] + a[1]*bdcP[1] + a[2]*bdcP[2] + a[3]*bdcP[3]);
925
926 hess[0] = dxInv * dxInv *
927 (d2a[0]*bcP[0] + d2a[1]*bcP[1] + d2a[2]*bcP[2] + d2a[3]*bcP[3]);
928
929 hess[1] = dxInv * dyInv *
930 (da[0]*dbcP[0] + da[1]*dbcP[1] + da[2]*dbcP[2] + da[3]*dbcP[3]);
931 hess[3] = hess[1];
932
933 hess[2] = dxInv * dzInv *
934 (da[0]*bdcP[0] + da[1]*bdcP[1] + da[2]*bdcP[2] + da[3]*bdcP[3]);
935 hess[6] = hess[2];
936
937 hess[4] = dyInv * dyInv *
938 (a[0]*d2bcP[0] + a[1]*d2bcP[1] + a[2]*d2bcP[2] + a[3]*d2bcP[3]);
939
940 hess[5] = dyInv * dzInv *
941 (a[0]*dbdcP[0] + a[1]*dbdcP[1] + a[2]*dbdcP[2] + a[3]*dbdcP[3]);
942 hess[7] = hess[5];
943
944 hess[8] = dzInv * dzInv *
945 (a[0]*bd2cP[0] + a[1]*bd2cP[1] + a[2]*bd2cP[2] + a[3]*bd2cP[3]);
946#undef P
947
948}