casadi_blazing_5d_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_ttv5<T1>" "casadi_blazing_tensor_ttv5"
24 
25 // SYMBOL "blazing_5d_boor_eval"
26 template<typename T1>
27 void casadi_blazing_5d_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+=5+1;
30  starts = iw;
31 
32  casadi_int n_b[5];
33 
34  simde__m256d d0[5], d1[5], d2[5];
35  const T1* inv2[5] = {0, 0, 0, 0, 0}; const T1* inv3[5] = {0, 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 < 5; ++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 5D 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  casadi_int s4 = n_b[0]*n_b[1]*n_b[2]*n_b[3];
52  const T1* base = c + starts[0] + starts[1]*s1 + starts[2]*s2 + starts[3]*s3 + starts[4]*s4;
53 
54  if (f) {
55  f[0] = casadi_blazing_tensor_ttv5<T1>(base, s1, s2, s3, s4, d0[0], d0[1], d0[2], d0[3], d0[4]);
56  }
57 
58  // Jacobian and Hessian share knot pointers and J bases
59  if (J || H) {
60  const T1* t[5];
61  simde__m256d dJ[5];
62  for (int i = 0; i < 5; ++i) {
63  t[i] = all_knots + offset[i] + starts[i];
64  dJ[i] = casadi_blazing_dbasis<T1>(d1[i], t[i], inv3[i]);
65  }
66 
67  if (J) {
68  J[0] = casadi_blazing_tensor_ttv5<T1>(base, s1, s2, s3, s4,
69  dJ[0], d0[1], d0[2], d0[3], d0[4]);
70  J[1] = casadi_blazing_tensor_ttv5<T1>(base, s1, s2, s3, s4,
71  d0[0], dJ[1], d0[2], d0[3], d0[4]);
72  J[2] = casadi_blazing_tensor_ttv5<T1>(base, s1, s2, s3, s4,
73  d0[0], d0[1], dJ[2], d0[3], d0[4]);
74  J[3] = casadi_blazing_tensor_ttv5<T1>(base, s1, s2, s3, s4,
75  d0[0], d0[1], d0[2], dJ[3], d0[4]);
76  J[4] = casadi_blazing_tensor_ttv5<T1>(base, s1, s2, s3, s4,
77  d0[0], d0[1], d0[2], d0[3], dJ[4]);
78  }
79 
80  if (H) {
81  simde__m256d dH[5];
82  for (int i = 0; i < 5; ++i)
83  dH[i] = casadi_blazing_d2basis<T1>(d2[i], t[i], inv2[i], inv3[i]);
84 
85  // Diagonal
86  H[0] = casadi_blazing_tensor_ttv5<T1>(base, s1, s2, s3, s4,
87  dH[0], d0[1], d0[2], d0[3], d0[4]);
88  H[6] = casadi_blazing_tensor_ttv5<T1>(base, s1, s2, s3, s4,
89  d0[0], dH[1], d0[2], d0[3], d0[4]);
90  H[12] = casadi_blazing_tensor_ttv5<T1>(base, s1, s2, s3, s4,
91  d0[0], d0[1], dH[2], d0[3], d0[4]);
92  H[18] = casadi_blazing_tensor_ttv5<T1>(base, s1, s2, s3, s4,
93  d0[0], d0[1], d0[2], dH[3], d0[4]);
94  H[24] = casadi_blazing_tensor_ttv5<T1>(base, s1, s2, s3, s4,
95  d0[0], d0[1], d0[2], d0[3], dH[4]);
96 
97  // Off-diagonal
98  H[1] = H[5] = casadi_blazing_tensor_ttv5<T1>(base, s1, s2, s3, s4,
99  dJ[0], dJ[1], d0[2], d0[3], d0[4]);
100  H[2] = H[10] = casadi_blazing_tensor_ttv5<T1>(base, s1, s2, s3, s4,
101  dJ[0], d0[1], dJ[2], d0[3], d0[4]);
102  H[3] = H[15] = casadi_blazing_tensor_ttv5<T1>(base, s1, s2, s3, s4,
103  dJ[0], d0[1], d0[2], dJ[3], d0[4]);
104  H[4] = H[20] = casadi_blazing_tensor_ttv5<T1>(base, s1, s2, s3, s4,
105  dJ[0], d0[1], d0[2], d0[3], dJ[4]);
106  H[7] = H[11] = casadi_blazing_tensor_ttv5<T1>(base, s1, s2, s3, s4,
107  d0[0], dJ[1], dJ[2], d0[3], d0[4]);
108  H[8] = H[16] = casadi_blazing_tensor_ttv5<T1>(base, s1, s2, s3, s4,
109  d0[0], dJ[1], d0[2], dJ[3], d0[4]);
110  H[9] = H[21] = casadi_blazing_tensor_ttv5<T1>(base, s1, s2, s3, s4,
111  d0[0], dJ[1], d0[2], d0[3], dJ[4]);
112  H[13] = H[17] = casadi_blazing_tensor_ttv5<T1>(base, s1, s2, s3, s4,
113  d0[0], d0[1], dJ[2], dJ[3], d0[4]);
114  H[14] = H[22] = casadi_blazing_tensor_ttv5<T1>(base, s1, s2, s3, s4,
115  d0[0], d0[1], dJ[2], d0[3], dJ[4]);
116  H[19] = H[23] = casadi_blazing_tensor_ttv5<T1>(base, s1, s2, s3, s4,
117  d0[0], d0[1], d0[2], dJ[3], dJ[4]);
118  }
119  }
120 }