casadi_blazing_4d_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_ttv4<T1>" "casadi_blazing_tensor_ttv4"
24 
25 // SYMBOL "blazing_4d_boor_eval"
26 template<typename T1>
27 void casadi_blazing_4d_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+=4+1;
30  starts = iw;
31 
32  casadi_int n_b[4];
33 
34  simde__m256d d0[4], d1[4], d2[4];
35  const T1* inv2[4] = {0, 0, 0, 0}; const T1* inv3[4] = {0, 0, 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  const T1* dim_cache = all_knots_cache;
40  for (int i = 0; i < 4; ++i) {
41  starts[i] = casadi_blazing_boor_init<T1>(all_x[i], all_knots, dim_cache,
42  offset[i], offset[i+1], lookup_mode[i], &d0[i], &d1[i], &d2[i], &inv2[i], &inv3[i]);
43  n_b[i] = offset[i+1] - offset[i] - 3 - 1;
44  if (dim_cache) dim_cache += 2 + 3 * (offset[i+1] - offset[i]);
45  }
46 
47  // Compute strides and base pointer for 4D coefficient tensor
48  casadi_int s1 = n_b[0];
49  casadi_int s2 = n_b[0]*n_b[1];
50  casadi_int s3 = n_b[0]*n_b[1]*n_b[2];
51  const T1* base = c + starts[0] + starts[1]*s1 + starts[2]*s2 + starts[3]*s3;
52 
53  if (f) {
54  f[0] = casadi_blazing_tensor_ttv4<T1>(base, s1, s2, s3, d0[0], d0[1], d0[2], d0[3]);
55  }
56 
57  // Jacobian and Hessian share knot pointers and J bases
58  if (J || H) {
59  const T1* t[4];
60  simde__m256d dJ[4];
61  for (int i = 0; i < 4; ++i) {
62  t[i] = all_knots + offset[i] + starts[i];
63  dJ[i] = casadi_blazing_dbasis<T1>(d1[i], t[i], inv3[i]);
64  }
65 
66  if (J) {
67  J[0] = casadi_blazing_tensor_ttv4<T1>(base, s1, s2, s3, dJ[0], d0[1], d0[2], d0[3]);
68  J[1] = casadi_blazing_tensor_ttv4<T1>(base, s1, s2, s3, d0[0], dJ[1], d0[2], d0[3]);
69  J[2] = casadi_blazing_tensor_ttv4<T1>(base, s1, s2, s3, d0[0], d0[1], dJ[2], d0[3]);
70  J[3] = casadi_blazing_tensor_ttv4<T1>(base, s1, s2, s3, d0[0], d0[1], d0[2], dJ[3]);
71  }
72 
73  if (H) {
74  simde__m256d dH[4];
75  for (int i = 0; i < 4; ++i)
76  dH[i] = casadi_blazing_d2basis<T1>(d2[i], t[i], inv2[i], inv3[i]);
77 
78  // Diagonal
79  H[0] = casadi_blazing_tensor_ttv4<T1>(base, s1, s2, s3, dH[0], d0[1], d0[2], d0[3]);
80  H[5] = casadi_blazing_tensor_ttv4<T1>(base, s1, s2, s3, d0[0], dH[1], d0[2], d0[3]);
81  H[10] = casadi_blazing_tensor_ttv4<T1>(base, s1, s2, s3, d0[0], d0[1], dH[2], d0[3]);
82  H[15] = casadi_blazing_tensor_ttv4<T1>(base, s1, s2, s3, d0[0], d0[1], d0[2], dH[3]);
83 
84  // Off-diagonal
85  H[1] = H[4] = casadi_blazing_tensor_ttv4<T1>(base, s1, s2, s3, dJ[0], dJ[1], d0[2], d0[3]);
86  H[2] = H[8] = casadi_blazing_tensor_ttv4<T1>(base, s1, s2, s3, dJ[0], d0[1], dJ[2], d0[3]);
87  H[3] = H[12] = casadi_blazing_tensor_ttv4<T1>(base, s1, s2, s3, dJ[0], d0[1], d0[2], dJ[3]);
88  H[6] = H[9] = casadi_blazing_tensor_ttv4<T1>(base, s1, s2, s3, d0[0], dJ[1], dJ[2], d0[3]);
89  H[7] = H[13] = casadi_blazing_tensor_ttv4<T1>(base, s1, s2, s3, d0[0], dJ[1], d0[2], dJ[3]);
90  H[11] = H[14] = casadi_blazing_tensor_ttv4<T1>(base, s1, s2, s3, d0[0], d0[1], dJ[2], dJ[3]);
91  }
92  }
93 }