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