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  Ji[i] = dc ? d1[i] : casadi_blazing_dbasis<T1>(d1[i], t[i], inv3[i]);
71  }
72 
73  // First derivatives
74  if (J) {
75  // J[0]: d/dx0
76  if (dc) {
77  stride1 = offset[1]-offset[0]-4-1;
78  stride2 = (offset[2]-offset[1]-4)*stride1;
79  for (int j=0;j<4;++j) {
80  for (int k=0;k<4;++k) {
81  C[j+4*k] = simde_mm256_loadu_pd(
82  dc+(starts[1]+j)*stride1+(starts[2]+k)*stride2+starts[0]-1);
83  }
84  }
85  dc += stride2*(offset[3]-offset[2]-4);
86  }
87  J[0] = casadi_blazing_tensor_ttv3<T1>(C, Ji[0], d0[1], d0[2]);
88 
89  // J[1]: d/dx1
90  if (dc) {
91  stride1 = offset[1]-offset[0]-4;
92  stride2 = (offset[2]-offset[1]-4-1)*stride1;
93  for (int j=0;j<4;++j) {
94  for (int k=0;k<4;++k) {
95  if (j==0) {
96  C[j+4*k] = zero;
97  } else {
98  C[j+4*k] = simde_mm256_loadu_pd(
99  dc+(starts[1]+j-1)*stride1+(starts[2]+k)*stride2+starts[0]);
100  }
101  }
102  }
103  dc += stride2*(offset[3]-offset[2]-4);
104  }
105  J[1] = casadi_blazing_tensor_ttv3<T1>(C, d0[0], Ji[1], d0[2]);
106 
107  // J[2]: d/dx2
108  if (dc) {
109  stride1 = offset[1]-offset[0]-4;
110  stride2 = (offset[2]-offset[1]-4)*stride1;
111  for (int j=0;j<4;++j) {
112  for (int k=0;k<4;++k) {
113  if (k==0) {
114  C[j+4*k] = zero;
115  } else {
116  C[j+4*k] = simde_mm256_loadu_pd(
117  dc+(starts[1]+j)*stride1+(starts[2]+k-1)*stride2+starts[0]);
118  }
119  }
120  }
121  }
122  J[2] = casadi_blazing_tensor_ttv3<T1>(C, d0[0], d0[1], Ji[2]);
123  }
124 
125  // Second derivatives
126  if (H) {
127  // Effective second-derivative basis
128  simde__m256d Hi[3];
129  for (int i = 0; i < 3; ++i) {
130  Hi[i] = ddc ? d2[i] : casadi_blazing_d2basis<T1>(d2[i], t[i], inv2[i], inv3[i]);
131  }
132 
133  // Diagonal: H[i*(n+1)]
134  // H[0] = d2/dx0^2
135  if (ddc) {
136  stride1 = offset[1]-offset[0]-4-2;
137  stride2 = (offset[2]-offset[1]-4)*stride1;
138  for (int j=0;j<4;++j) {
139  for (int k=0;k<4;++k) {
140  C[j+4*k] = simde_mm256_loadu_pd(
141  ddc+(starts[1]+j)*stride1+(starts[2]+k)*stride2+starts[0]-2);
142  }
143  }
144  ddc += stride2*(offset[3]-offset[2]-4);
145  }
146  H[0] = casadi_blazing_tensor_ttv3<T1>(C, Hi[0], d0[1], d0[2]);
147 
148  // H[4] = d2/dx1^2
149  if (ddc) {
150  stride1 = offset[1]-offset[0]-4;
151  stride2 = (offset[2]-offset[1]-4-2)*stride1;
152  for (int j=0;j<4;++j) {
153  for (int k=0;k<4;++k) {
154  if (j<=1) {
155  C[j+4*k] = zero;
156  } else {
157  C[j+4*k] = simde_mm256_loadu_pd(
158  ddc+(starts[1]+j-2)*stride1+(starts[2]+k)*stride2+starts[0]);
159  }
160  }
161  }
162  ddc += stride2*(offset[3]-offset[2]-4);
163  }
164  H[4] = casadi_blazing_tensor_ttv3<T1>(C, d0[0], Hi[1], d0[2]);
165 
166  // H[8] = d2/dx2^2
167  if (ddc) {
168  stride1 = offset[1]-offset[0]-4;
169  stride2 = (offset[2]-offset[1]-4)*stride1;
170  for (int j=0;j<4;++j) {
171  for (int k=0;k<4;++k) {
172  if (k<=1) {
173  C[j+4*k] = zero;
174  } else {
175  C[j+4*k] = simde_mm256_loadu_pd(
176  ddc+(starts[1]+j)*stride1+(starts[2]+k-2)*stride2+starts[0]);
177  }
178  }
179  }
180  ddc += stride2*(offset[3]-offset[2]-4-2);
181  }
182  H[8] = casadi_blazing_tensor_ttv3<T1>(C, d0[0], d0[1], Hi[2]);
183 
184  // Off-diagonal
185  // H[1] = H[3] = d2/dx0 dx1
186  if (ddc) {
187  stride1 = offset[1]-offset[0]-5;
188  stride2 = (offset[2]-offset[1]-5)*stride1;
189  for (int j=0;j<4;++j) {
190  for (int k=0;k<4;++k) {
191  if (j==0) {
192  C[j+4*k] = zero;
193  } else {
194  C[j+4*k] = simde_mm256_loadu_pd(
195  ddc+(starts[1]+j-1)*stride1+(starts[2]+k)*stride2+starts[0]-1);
196  }
197  }
198  }
199  ddc += stride2*(offset[3]-offset[2]-4);
200  }
201  H[1] = H[3] = casadi_blazing_tensor_ttv3<T1>(C, Ji[0], Ji[1], d0[2]);
202 
203  // H[5] = H[7] = d2/dx1 dx2
204  if (ddc) {
205  stride1 = offset[1]-offset[0]-4;
206  stride2 = (offset[2]-offset[1]-5)*stride1;
207  for (int j=0;j<4;++j) {
208  for (int k=0;k<4;++k) {
209  if (k==0) {
210  C[j+4*k] = zero;
211  } else {
212  C[j+4*k] = simde_mm256_loadu_pd(
213  ddc+(starts[1]+j-1)*stride1+(starts[2]+k-1)*stride2+starts[0]);
214  }
215  }
216  }
217  ddc += stride2*(offset[3]-offset[2]-5);
218  }
219  H[5] = H[7] = casadi_blazing_tensor_ttv3<T1>(C, d0[0], Ji[1], Ji[2]);
220 
221  // H[2] = H[6] = d2/dx0 dx2
222  if (ddc) {
223  stride1 = offset[1]-offset[0]-5;
224  stride2 = (offset[2]-offset[1]-4)*stride1;
225  for (int j=0;j<4;++j) {
226  for (int k=0;k<4;++k) {
227  if (k==0) {
228  C[j+4*k] = zero;
229  } else {
230  C[j+4*k] = simde_mm256_loadu_pd(
231  ddc+(starts[1]+j)*stride1+(starts[2]+k-1)*stride2+starts[0]-1);
232  }
233  }
234  }
235  }
236  H[2] = H[6] = casadi_blazing_tensor_ttv3<T1>(C, Ji[0], d0[1], Ji[2]);
237  }
238  }
239 }