casadi_blazing_3d_boor_eval.hpp
1 //
2 // MIT No Attribution
3 //
4 // Copyright (C) 2010-2023 Joel Andersson, Joris Gillis, Moritz Diehl, KU Leuven.
5 //
6 // Permission is hereby granted, free of charge, to any person obtaining a copy of this
7 // software and associated documentation files (the "Software"), to deal in the Software
8 // without restriction, including without limitation the rights to use, copy, modify,
9 // merge, publish, distribute, sublicense, and/or sell copies of the Software, and to
10 // permit persons to whom the Software is furnished to do so.
11 //
12 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
13 // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
14 // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
15 // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
16 // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
17 // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
18 //
19 
20 // C-REPLACE "casadi_blazing_boor_init<T1>" "casadi_blazing_boor_init"
21 // C-REPLACE "casadi_blazing_dbasis<T1>" "casadi_blazing_dbasis"
22 // C-REPLACE "casadi_blazing_d2basis<T1>" "casadi_blazing_d2basis"
23 // C-REPLACE "casadi_blazing_tensor_ttv3<T1>" "casadi_blazing_tensor_ttv3"
24 
25 // SYMBOL "blazing_3d_boor_eval"
26 template<typename T1>
27 void casadi_blazing_3d_boor_eval(T1* f, T1* J, T1* H, const T1* all_knots, const T1* all_knots_cache, const casadi_int* offset, const T1* c, const T1* dc, const T1* ddc, const T1* all_x, const casadi_int* lookup_mode, casadi_int* iw, T1* w) { // NOLINT(whitespace/line_length)
28  casadi_int *starts;
29  iw+=3+1;
30  starts = iw;
31 
32  casadi_int stride1 = offset[1]-offset[0]-4;
33  casadi_int stride2 = (offset[2]-offset[1]-4)*stride1;
34 
35  simde__m256d zero = simde_mm256_set1_pd(0.0);
36 
37  // Per-dimension de Boor evaluation. Per-dim cache slice is
38  // [intercept, slope, inv1[n_k], inv2[n_k], inv3[n_k]] -- size 2 + 3*n_k.
39  simde__m256d d0[3], d1[3], d2[3];
40  const T1* inv2[3] = {0, 0, 0}; const T1* inv3[3] = {0, 0, 0};
41  const T1* dim_cache = all_knots_cache;
42  for (int i = 0; i < 3; ++i) {
43  starts[i] = casadi_blazing_boor_init<T1>(all_x[i], all_knots, dim_cache,
44  offset[i], offset[i+1], lookup_mode[i], &d0[i], &d1[i], &d2[i], &inv2[i], &inv3[i]);
45  if (dim_cache) dim_cache += 2 + 3 * (offset[i+1] - offset[i]);
46  }
47 
48  // Load coefficient sub-tensor (same C used for value and all NPC derivatives)
49  simde__m256d C[16];
50  for (int j=0;j<4;++j) {
51  for (int k=0;k<4;++k) {
52  C[j+4*k] = simde_mm256_loadu_pd(c+(starts[1]+j)*stride1+(starts[2]+k)*stride2+starts[0]);
53  }
54  }
55 
56  if (f) {
57  f[0] = casadi_blazing_tensor_ttv3<T1>(C, &d0[0], &d0[1], &d0[2]);
58  }
59 
60  // Jacobian and Hessian share knot pointers and J bases
61  if (J || H) {
62  const T1* t[3];
63  for (int i = 0; i < 3; ++i) {
64  t[i] = all_knots + offset[i] + starts[i];
65  }
66 
67  // Effective first-derivative basis: raw for precomputed, summation-by-parts for NPC
68  simde__m256d Ji[3];
69  for (int i = 0; i < 3; ++i) {
70  if (dc) {
71  Ji[i] = d1[i];
72  } else {
73  casadi_blazing_dbasis<T1>(&Ji[i], &d1[i], t[i], inv3[i]);
74  }
75  }
76 
77  // First derivatives
78  if (J) {
79  // J[0]: d/dx0
80  if (dc) {
81  stride1 = offset[1]-offset[0]-4-1;
82  stride2 = (offset[2]-offset[1]-4)*stride1;
83  for (int j=0;j<4;++j) {
84  for (int k=0;k<4;++k) {
85  C[j+4*k] = simde_mm256_loadu_pd(
86  dc+(starts[1]+j)*stride1+(starts[2]+k)*stride2+starts[0]-1);
87  }
88  }
89  dc += stride2*(offset[3]-offset[2]-4);
90  }
91  J[0] = casadi_blazing_tensor_ttv3<T1>(C, &Ji[0], &d0[1], &d0[2]);
92 
93  // J[1]: d/dx1
94  if (dc) {
95  stride1 = offset[1]-offset[0]-4;
96  stride2 = (offset[2]-offset[1]-4-1)*stride1;
97  for (int j=0;j<4;++j) {
98  for (int k=0;k<4;++k) {
99  if (j==0) {
100  C[j+4*k] = zero;
101  } else {
102  C[j+4*k] = simde_mm256_loadu_pd(
103  dc+(starts[1]+j-1)*stride1+(starts[2]+k)*stride2+starts[0]);
104  }
105  }
106  }
107  dc += stride2*(offset[3]-offset[2]-4);
108  }
109  J[1] = casadi_blazing_tensor_ttv3<T1>(C, &d0[0], &Ji[1], &d0[2]);
110 
111  // J[2]: d/dx2
112  if (dc) {
113  stride1 = offset[1]-offset[0]-4;
114  stride2 = (offset[2]-offset[1]-4)*stride1;
115  for (int j=0;j<4;++j) {
116  for (int k=0;k<4;++k) {
117  if (k==0) {
118  C[j+4*k] = zero;
119  } else {
120  C[j+4*k] = simde_mm256_loadu_pd(
121  dc+(starts[1]+j)*stride1+(starts[2]+k-1)*stride2+starts[0]);
122  }
123  }
124  }
125  }
126  J[2] = casadi_blazing_tensor_ttv3<T1>(C, &d0[0], &d0[1], &Ji[2]);
127  }
128 
129  // Second derivatives
130  if (H) {
131  // Effective second-derivative basis
132  simde__m256d Hi[3];
133  for (int i = 0; i < 3; ++i) {
134  if (ddc) {
135  Hi[i] = d2[i];
136  } else {
137  casadi_blazing_d2basis<T1>(&Hi[i], &d2[i], t[i], inv2[i], inv3[i]);
138  }
139  }
140 
141  // Diagonal: H[i*(n+1)]
142  // H[0] = d2/dx0^2
143  if (ddc) {
144  stride1 = offset[1]-offset[0]-4-2;
145  stride2 = (offset[2]-offset[1]-4)*stride1;
146  for (int j=0;j<4;++j) {
147  for (int k=0;k<4;++k) {
148  C[j+4*k] = simde_mm256_loadu_pd(
149  ddc+(starts[1]+j)*stride1+(starts[2]+k)*stride2+starts[0]-2);
150  }
151  }
152  ddc += stride2*(offset[3]-offset[2]-4);
153  }
154  H[0] = casadi_blazing_tensor_ttv3<T1>(C, &Hi[0], &d0[1], &d0[2]);
155 
156  // H[4] = d2/dx1^2
157  if (ddc) {
158  stride1 = offset[1]-offset[0]-4;
159  stride2 = (offset[2]-offset[1]-4-2)*stride1;
160  for (int j=0;j<4;++j) {
161  for (int k=0;k<4;++k) {
162  if (j<=1) {
163  C[j+4*k] = zero;
164  } else {
165  C[j+4*k] = simde_mm256_loadu_pd(
166  ddc+(starts[1]+j-2)*stride1+(starts[2]+k)*stride2+starts[0]);
167  }
168  }
169  }
170  ddc += stride2*(offset[3]-offset[2]-4);
171  }
172  H[4] = casadi_blazing_tensor_ttv3<T1>(C, &d0[0], &Hi[1], &d0[2]);
173 
174  // H[8] = d2/dx2^2
175  if (ddc) {
176  stride1 = offset[1]-offset[0]-4;
177  stride2 = (offset[2]-offset[1]-4)*stride1;
178  for (int j=0;j<4;++j) {
179  for (int k=0;k<4;++k) {
180  if (k<=1) {
181  C[j+4*k] = zero;
182  } else {
183  C[j+4*k] = simde_mm256_loadu_pd(
184  ddc+(starts[1]+j)*stride1+(starts[2]+k-2)*stride2+starts[0]);
185  }
186  }
187  }
188  ddc += stride2*(offset[3]-offset[2]-4-2);
189  }
190  H[8] = casadi_blazing_tensor_ttv3<T1>(C, &d0[0], &d0[1], &Hi[2]);
191 
192  // Off-diagonal
193  // H[1] = H[3] = d2/dx0 dx1
194  if (ddc) {
195  stride1 = offset[1]-offset[0]-5;
196  stride2 = (offset[2]-offset[1]-5)*stride1;
197  for (int j=0;j<4;++j) {
198  for (int k=0;k<4;++k) {
199  if (j==0) {
200  C[j+4*k] = zero;
201  } else {
202  C[j+4*k] = simde_mm256_loadu_pd(
203  ddc+(starts[1]+j-1)*stride1+(starts[2]+k)*stride2+starts[0]-1);
204  }
205  }
206  }
207  ddc += stride2*(offset[3]-offset[2]-4);
208  }
209  H[1] = H[3] = casadi_blazing_tensor_ttv3<T1>(C, &Ji[0], &Ji[1], &d0[2]);
210 
211  // H[5] = H[7] = d2/dx1 dx2
212  if (ddc) {
213  stride1 = offset[1]-offset[0]-4;
214  stride2 = (offset[2]-offset[1]-5)*stride1;
215  for (int j=0;j<4;++j) {
216  for (int k=0;k<4;++k) {
217  if (k==0) {
218  C[j+4*k] = zero;
219  } else {
220  C[j+4*k] = simde_mm256_loadu_pd(
221  ddc+(starts[1]+j-1)*stride1+(starts[2]+k-1)*stride2+starts[0]);
222  }
223  }
224  }
225  ddc += stride2*(offset[3]-offset[2]-5);
226  }
227  H[5] = H[7] = casadi_blazing_tensor_ttv3<T1>(C, &d0[0], &Ji[1], &Ji[2]);
228 
229  // H[2] = H[6] = d2/dx0 dx2
230  if (ddc) {
231  stride1 = offset[1]-offset[0]-5;
232  stride2 = (offset[2]-offset[1]-4)*stride1;
233  for (int j=0;j<4;++j) {
234  for (int k=0;k<4;++k) {
235  if (k==0) {
236  C[j+4*k] = zero;
237  } else {
238  C[j+4*k] = simde_mm256_loadu_pd(
239  ddc+(starts[1]+j)*stride1+(starts[2]+k-1)*stride2+starts[0]-1);
240  }
241  }
242  }
243  }
244  H[2] = H[6] = casadi_blazing_tensor_ttv3<T1>(C, &Ji[0], &d0[1], &Ji[2]);
245  }
246  }
247 }