94 if (require_dl_dq & rotation_derivative_dldq::use_dl) {
113 if (require_dl_dq & rotation_derivative_dldq::use_dq) {
123 tmp_Q0Q0_L[0][0][0] = (Q1[0] * Q0[0]) / (L0-L1) * Q1[0] +
124 (Q2[0] * Q0[0]) / (L0-L2) * Q2[0] +
125 (Q3[0] * Q0[0]) / (L0-L3) * Q3[0];
126 tmp_Q0Q0_L[0][0][1] = (Q1[0] * Q0[1]) / (L0-L1) * Q1[0] +
127 (Q2[0] * Q0[1]) / (L0-L2) * Q2[0] +
128 (Q3[0] * Q0[1]) / (L0-L3) * Q3[0];
129 tmp_Q0Q0_L[0][0][2] = (Q1[0] * Q0[2]) / (L0-L1) * Q1[0] +
130 (Q2[0] * Q0[2]) / (L0-L2) * Q2[0] +
131 (Q3[0] * Q0[2]) / (L0-L3) * Q3[0];
132 tmp_Q0Q0_L[0][0][3] = (Q1[0] * Q0[3]) / (L0-L1) * Q1[0] +
133 (Q2[0] * Q0[3]) / (L0-L2) * Q2[0] +
134 (Q3[0] * Q0[3]) / (L0-L3) * Q3[0];
136 tmp_Q0Q0_L[0][1][0] = (Q1[1] * Q0[0]) / (L0-L1) * Q1[0] +
137 (Q2[1] * Q0[0]) / (L0-L2) * Q2[0] +
138 (Q3[1] * Q0[0]) / (L0-L3) * Q3[0];
139 tmp_Q0Q0_L[0][1][1] = (Q1[1] * Q0[1]) / (L0-L1) * Q1[0] +
140 (Q2[1] * Q0[1]) / (L0-L2) * Q2[0] +
141 (Q3[1] * Q0[1]) / (L0-L3) * Q3[0];
142 tmp_Q0Q0_L[0][1][2] = (Q1[1] * Q0[2]) / (L0-L1) * Q1[0] +
143 (Q2[1] * Q0[2]) / (L0-L2) * Q2[0] +
144 (Q3[1] * Q0[2]) / (L0-L3) * Q3[0];
145 tmp_Q0Q0_L[0][1][3] = (Q1[1] * Q0[3]) / (L0-L1) * Q1[0] +
146 (Q2[1] * Q0[3]) / (L0-L2) * Q2[0] +
147 (Q3[1] * Q0[3]) / (L0-L3) * Q3[0];
149 tmp_Q0Q0_L[0][2][0] = (Q1[2] * Q0[0]) / (L0-L1) * Q1[0] +
150 (Q2[2] * Q0[0]) / (L0-L2) * Q2[0] +
151 (Q3[2] * Q0[0]) / (L0-L3) * Q3[0];
152 tmp_Q0Q0_L[0][2][1] = (Q1[2] * Q0[1]) / (L0-L1) * Q1[0] +
153 (Q2[2] * Q0[1]) / (L0-L2) * Q2[0] +
154 (Q3[2] * Q0[1]) / (L0-L3) * Q3[0];
155 tmp_Q0Q0_L[0][2][2] = (Q1[2] * Q0[2]) / (L0-L1) * Q1[0] +
156 (Q2[2] * Q0[2]) / (L0-L2) * Q2[0] +
157 (Q3[2] * Q0[2]) / (L0-L3) * Q3[0];
158 tmp_Q0Q0_L[0][2][3] = (Q1[2] * Q0[3]) / (L0-L1) * Q1[0] +
159 (Q2[2] * Q0[3]) / (L0-L2) * Q2[0] +
160 (Q3[2] * Q0[3]) / (L0-L3) * Q3[0];
162 tmp_Q0Q0_L[0][3][0] = (Q1[3] * Q0[0]) / (L0-L1) * Q1[0] +
163 (Q2[3] * Q0[0]) / (L0-L2) * Q2[0] +
164 (Q3[3] * Q0[0]) / (L0-L3) * Q3[0];
165 tmp_Q0Q0_L[0][3][1] = (Q1[3] * Q0[1]) / (L0-L1) * Q1[0] +
166 (Q2[3] * Q0[1]) / (L0-L2) * Q2[0] +
167 (Q3[3] * Q0[1]) / (L0-L3) * Q3[0];
168 tmp_Q0Q0_L[0][3][2] = (Q1[3] * Q0[2]) / (L0-L1) * Q1[0] +
169 (Q2[3] * Q0[2]) / (L0-L2) * Q2[0] +
170 (Q3[3] * Q0[2]) / (L0-L3) * Q3[0];
171 tmp_Q0Q0_L[0][3][3] = (Q1[3] * Q0[3]) / (L0-L1) * Q1[0] +
172 (Q2[3] * Q0[3]) / (L0-L2) * Q2[0] +
173 (Q3[3] * Q0[3]) / (L0-L3) * Q3[0];
175 tmp_Q0Q0_L[1][0][0] = (Q1[0] * Q0[0]) / (L0-L1) * Q1[1] +
176 (Q2[0] * Q0[0]) / (L0-L2) * Q2[1] +
177 (Q3[0] * Q0[0]) / (L0-L3) * Q3[1];
178 tmp_Q0Q0_L[1][0][1] = (Q1[0] * Q0[1]) / (L0-L1) * Q1[1] +
179 (Q2[0] * Q0[1]) / (L0-L2) * Q2[1] +
180 (Q3[0] * Q0[1]) / (L0-L3) * Q3[1];
181 tmp_Q0Q0_L[1][0][2] = (Q1[0] * Q0[2]) / (L0-L1) * Q1[1] +
182 (Q2[0] * Q0[2]) / (L0-L2) * Q2[1] +
183 (Q3[0] * Q0[2]) / (L0-L3) * Q3[1];
184 tmp_Q0Q0_L[1][0][3] = (Q1[0] * Q0[3]) / (L0-L1) * Q1[1] +
185 (Q2[0] * Q0[3]) / (L0-L2) * Q2[1] +
186 (Q3[0] * Q0[3]) / (L0-L3) * Q3[1];
188 tmp_Q0Q0_L[1][1][0] = (Q1[1] * Q0[0]) / (L0-L1) * Q1[1] +
189 (Q2[1] * Q0[0]) / (L0-L2) * Q2[1] +
190 (Q3[1] * Q0[0]) / (L0-L3) * Q3[1];
191 tmp_Q0Q0_L[1][1][1] = (Q1[1] * Q0[1]) / (L0-L1) * Q1[1] +
192 (Q2[1] * Q0[1]) / (L0-L2) * Q2[1] +
193 (Q3[1] * Q0[1]) / (L0-L3) * Q3[1];
194 tmp_Q0Q0_L[1][1][2] = (Q1[1] * Q0[2]) / (L0-L1) * Q1[1] +
195 (Q2[1] * Q0[2]) / (L0-L2) * Q2[1] +
196 (Q3[1] * Q0[2]) / (L0-L3) * Q3[1];
197 tmp_Q0Q0_L[1][1][3] = (Q1[1] * Q0[3]) / (L0-L1) * Q1[1] +
198 (Q2[1] * Q0[3]) / (L0-L2) * Q2[1] +
199 (Q3[1] * Q0[3]) / (L0-L3) * Q3[1];
201 tmp_Q0Q0_L[1][2][0] = (Q1[2] * Q0[0]) / (L0-L1) * Q1[1] +
202 (Q2[2] * Q0[0]) / (L0-L2) * Q2[1] +
203 (Q3[2] * Q0[0]) / (L0-L3) * Q3[1];
204 tmp_Q0Q0_L[1][2][1] = (Q1[2] * Q0[1]) / (L0-L1) * Q1[1] +
205 (Q2[2] * Q0[1]) / (L0-L2) * Q2[1] +
206 (Q3[2] * Q0[1]) / (L0-L3) * Q3[1];
207 tmp_Q0Q0_L[1][2][2] = (Q1[2] * Q0[2]) / (L0-L1) * Q1[1] +
208 (Q2[2] * Q0[2]) / (L0-L2) * Q2[1] +
209 (Q3[2] * Q0[2]) / (L0-L3) * Q3[1];
210 tmp_Q0Q0_L[1][2][3] = (Q1[2] * Q0[3]) / (L0-L1) * Q1[1] +
211 (Q2[2] * Q0[3]) / (L0-L2) * Q2[1] +
212 (Q3[2] * Q0[3]) / (L0-L3) * Q3[1];
214 tmp_Q0Q0_L[1][3][0] = (Q1[3] * Q0[0]) / (L0-L1) * Q1[1] +
215 (Q2[3] * Q0[0]) / (L0-L2) * Q2[1] +
216 (Q3[3] * Q0[0]) / (L0-L3) * Q3[1];
217 tmp_Q0Q0_L[1][3][1] = (Q1[3] * Q0[1]) / (L0-L1) * Q1[1] +
218 (Q2[3] * Q0[1]) / (L0-L2) * Q2[1] +
219 (Q3[3] * Q0[1]) / (L0-L3) * Q3[1];
220 tmp_Q0Q0_L[1][3][2] = (Q1[3] * Q0[2]) / (L0-L1) * Q1[1] +
221 (Q2[3] * Q0[2]) / (L0-L2) * Q2[1] +
222 (Q3[3] * Q0[2]) / (L0-L3) * Q3[1];
223 tmp_Q0Q0_L[1][3][3] = (Q1[3] * Q0[3]) / (L0-L1) * Q1[1] +
224 (Q2[3] * Q0[3]) / (L0-L2) * Q2[1] +
225 (Q3[3] * Q0[3]) / (L0-L3) * Q3[1];
227 tmp_Q0Q0_L[2][0][0] = (Q1[0] * Q0[0]) / (L0-L1) * Q1[2] +
228 (Q2[0] * Q0[0]) / (L0-L2) * Q2[2] +
229 (Q3[0] * Q0[0]) / (L0-L3) * Q3[2];
230 tmp_Q0Q0_L[2][0][1] = (Q1[0] * Q0[1]) / (L0-L1) * Q1[2] +
231 (Q2[0] * Q0[1]) / (L0-L2) * Q2[2] +
232 (Q3[0] * Q0[1]) / (L0-L3) * Q3[2];
233 tmp_Q0Q0_L[2][0][2] = (Q1[0] * Q0[2]) / (L0-L1) * Q1[2] +
234 (Q2[0] * Q0[2]) / (L0-L2) * Q2[2] +
235 (Q3[0] * Q0[2]) / (L0-L3) * Q3[2];
236 tmp_Q0Q0_L[2][0][3] = (Q1[0] * Q0[3]) / (L0-L1) * Q1[2] +
237 (Q2[0] * Q0[3]) / (L0-L2) * Q2[2] +
238 (Q3[0] * Q0[3]) / (L0-L3) * Q3[2];
240 tmp_Q0Q0_L[2][1][0] = (Q1[1] * Q0[0]) / (L0-L1) * Q1[2] +
241 (Q2[1] * Q0[0]) / (L0-L2) * Q2[2] +
242 (Q3[1] * Q0[0]) / (L0-L3) * Q3[2];
243 tmp_Q0Q0_L[2][1][1] = (Q1[1] * Q0[1]) / (L0-L1) * Q1[2] +
244 (Q2[1] * Q0[1]) / (L0-L2) * Q2[2] +
245 (Q3[1] * Q0[1]) / (L0-L3) * Q3[2];
246 tmp_Q0Q0_L[2][1][2] = (Q1[1] * Q0[2]) / (L0-L1) * Q1[2] +
247 (Q2[1] * Q0[2]) / (L0-L2) * Q2[2] +
248 (Q3[1] * Q0[2]) / (L0-L3) * Q3[2];
249 tmp_Q0Q0_L[2][1][3] = (Q1[1] * Q0[3]) / (L0-L1) * Q1[2] +
250 (Q2[1] * Q0[3]) / (L0-L2) * Q2[2] +
251 (Q3[1] * Q0[3]) / (L0-L3) * Q3[2];
253 tmp_Q0Q0_L[2][2][0] = (Q1[2] * Q0[0]) / (L0-L1) * Q1[2] +
254 (Q2[2] * Q0[0]) / (L0-L2) * Q2[2] +
255 (Q3[2] * Q0[0]) / (L0-L3) * Q3[2];
256 tmp_Q0Q0_L[2][2][1] = (Q1[2] * Q0[1]) / (L0-L1) * Q1[2] +
257 (Q2[2] * Q0[1]) / (L0-L2) * Q2[2] +
258 (Q3[2] * Q0[1]) / (L0-L3) * Q3[2];
259 tmp_Q0Q0_L[2][2][2] = (Q1[2] * Q0[2]) / (L0-L1) * Q1[2] +
260 (Q2[2] * Q0[2]) / (L0-L2) * Q2[2] +
261 (Q3[2] * Q0[2]) / (L0-L3) * Q3[2];
262 tmp_Q0Q0_L[2][2][3] = (Q1[2] * Q0[3]) / (L0-L1) * Q1[2] +
263 (Q2[2] * Q0[3]) / (L0-L2) * Q2[2] +
264 (Q3[2] * Q0[3]) / (L0-L3) * Q3[2];
266 tmp_Q0Q0_L[2][3][0] = (Q1[3] * Q0[0]) / (L0-L1) * Q1[2] +
267 (Q2[3] * Q0[0]) / (L0-L2) * Q2[2] +
268 (Q3[3] * Q0[0]) / (L0-L3) * Q3[2];
269 tmp_Q0Q0_L[2][3][1] = (Q1[3] * Q0[1]) / (L0-L1) * Q1[2] +
270 (Q2[3] * Q0[1]) / (L0-L2) * Q2[2] +
271 (Q3[3] * Q0[1]) / (L0-L3) * Q3[2];
272 tmp_Q0Q0_L[2][3][2] = (Q1[3] * Q0[2]) / (L0-L1) * Q1[2] +
273 (Q2[3] * Q0[2]) / (L0-L2) * Q2[2] +
274 (Q3[3] * Q0[2]) / (L0-L3) * Q3[2];
275 tmp_Q0Q0_L[2][3][3] = (Q1[3] * Q0[3]) / (L0-L1) * Q1[2] +
276 (Q2[3] * Q0[3]) / (L0-L2) * Q2[2] +
277 (Q3[3] * Q0[3]) / (L0-L3) * Q3[2];
279 tmp_Q0Q0_L[3][0][0] = (Q1[0] * Q0[0]) / (L0-L1) * Q1[3] +
280 (Q2[0] * Q0[0]) / (L0-L2) * Q2[3] +
281 (Q3[0] * Q0[0]) / (L0-L3) * Q3[3];
282 tmp_Q0Q0_L[3][0][1] = (Q1[0] * Q0[1]) / (L0-L1) * Q1[3] +
283 (Q2[0] * Q0[1]) / (L0-L2) * Q2[3] +
284 (Q3[0] * Q0[1]) / (L0-L3) * Q3[3];
285 tmp_Q0Q0_L[3][0][2] = (Q1[0] * Q0[2]) / (L0-L1) * Q1[3] +
286 (Q2[0] * Q0[2]) / (L0-L2) * Q2[3] +
287 (Q3[0] * Q0[2]) / (L0-L3) * Q3[3];
288 tmp_Q0Q0_L[3][0][3] = (Q1[0] * Q0[3]) / (L0-L1) * Q1[3] +
289 (Q2[0] * Q0[3]) / (L0-L2) * Q2[3] +
290 (Q3[0] * Q0[3]) / (L0-L3) * Q3[3];
292 tmp_Q0Q0_L[3][1][0] = (Q1[1] * Q0[0]) / (L0-L1) * Q1[3] +
293 (Q2[1] * Q0[0]) / (L0-L2) * Q2[3] +
294 (Q3[1] * Q0[0]) / (L0-L3) * Q3[3];
295 tmp_Q0Q0_L[3][1][1] = (Q1[1] * Q0[1]) / (L0-L1) * Q1[3] +
296 (Q2[1] * Q0[1]) / (L0-L2) * Q2[3] +
297 (Q3[1] * Q0[1]) / (L0-L3) * Q3[3];
298 tmp_Q0Q0_L[3][1][2] = (Q1[1] * Q0[2]) / (L0-L1) * Q1[3] +
299 (Q2[1] * Q0[2]) / (L0-L2) * Q2[3] +
300 (Q3[1] * Q0[2]) / (L0-L3) * Q3[3];
301 tmp_Q0Q0_L[3][1][3] = (Q1[1] * Q0[3]) / (L0-L1) * Q1[3] +
302 (Q2[1] * Q0[3]) / (L0-L2) * Q2[3] +
303 (Q3[1] * Q0[3]) / (L0-L3) * Q3[3];
305 tmp_Q0Q0_L[3][2][0] = (Q1[2] * Q0[0]) / (L0-L1) * Q1[3] +
306 (Q2[2] * Q0[0]) / (L0-L2) * Q2[3] +
307 (Q3[2] * Q0[0]) / (L0-L3) * Q3[3];
308 tmp_Q0Q0_L[3][2][1] = (Q1[2] * Q0[1]) / (L0-L1) * Q1[3] +
309 (Q2[2] * Q0[1]) / (L0-L2) * Q2[3] +
310 (Q3[2] * Q0[1]) / (L0-L3) * Q3[3];
311 tmp_Q0Q0_L[3][2][2] = (Q1[2] * Q0[2]) / (L0-L1) * Q1[3] +
312 (Q2[2] * Q0[2]) / (L0-L2) * Q2[3] +
313 (Q3[2] * Q0[2]) / (L0-L3) * Q3[3];
314 tmp_Q0Q0_L[3][2][3] = (Q1[2] * Q0[3]) / (L0-L1) * Q1[3] +
315 (Q2[2] * Q0[3]) / (L0-L2) * Q2[3] +
316 (Q3[2] * Q0[3]) / (L0-L3) * Q3[3];
318 tmp_Q0Q0_L[3][3][0] = (Q1[3] * Q0[0]) / (L0-L1) * Q1[3] +
319 (Q2[3] * Q0[0]) / (L0-L2) * Q2[3] +
320 (Q3[3] * Q0[0]) / (L0-L3) * Q3[3];
321 tmp_Q0Q0_L[3][3][1] = (Q1[3] * Q0[1]) / (L0-L1) * Q1[3] +
322 (Q2[3] * Q0[1]) / (L0-L2) * Q2[3] +
323 (Q3[3] * Q0[1]) / (L0-L3) * Q3[3];
324 tmp_Q0Q0_L[3][3][2] = (Q1[3] * Q0[2]) / (L0-L1) * Q1[3] +
325 (Q2[3] * Q0[2]) / (L0-L2) * Q2[3] +
326 (Q3[3] * Q0[2]) / (L0-L3) * Q3[3];
327 tmp_Q0Q0_L[3][3][3] = (Q1[3] * Q0[3]) / (L0-L1) * Q1[3] +
328 (Q2[3] * Q0[3]) / (L0-L2) * Q2[3] +
329 (Q3[3] * Q0[3]) / (L0-L3) * Q3[3];
343 std::array<cvm::rvector, 4>* _noalias
const dq0_out,
344 std::array<std::array<cvm::rvector, 4>, 4>* _noalias
const ds_out)
const {
347 *ds_out = std::array<std::array<cvm::rvector, 4>, 4>();
348 for (
int i = 0; i < 4; ++i) {
349 for (
int j = 0; j < 4; ++j) {
350 (*ds_out)[i][j] = ds[i][j];
363 *dl0_out =
tmp_Q0Q0[0][0] * ds[0][0] +
396 (*dq0_out)[0] = tmp_Q0Q0_L[0][0][0] * ds[0][0] +
397 tmp_Q0Q0_L[0][0][1] * ds[0][1] +
398 tmp_Q0Q0_L[0][0][2] * ds[0][2] +
399 tmp_Q0Q0_L[0][0][3] * ds[0][3] +
400 tmp_Q0Q0_L[0][1][0] * ds[1][0] +
401 tmp_Q0Q0_L[0][1][1] * ds[1][1] +
402 tmp_Q0Q0_L[0][1][2] * ds[1][2] +
403 tmp_Q0Q0_L[0][1][3] * ds[1][3] +
404 tmp_Q0Q0_L[0][2][0] * ds[2][0] +
405 tmp_Q0Q0_L[0][2][1] * ds[2][1] +
406 tmp_Q0Q0_L[0][2][2] * ds[2][2] +
407 tmp_Q0Q0_L[0][2][3] * ds[2][3] +
408 tmp_Q0Q0_L[0][3][0] * ds[3][0] +
409 tmp_Q0Q0_L[0][3][1] * ds[3][1] +
410 tmp_Q0Q0_L[0][3][2] * ds[3][2] +
411 tmp_Q0Q0_L[0][3][3] * ds[3][3];
413 (*dq0_out)[1] = tmp_Q0Q0_L[1][0][0] * ds[0][0] +
414 tmp_Q0Q0_L[1][0][1] * ds[0][1] +
415 tmp_Q0Q0_L[1][0][2] * ds[0][2] +
416 tmp_Q0Q0_L[1][0][3] * ds[0][3] +
417 tmp_Q0Q0_L[1][1][0] * ds[1][0] +
418 tmp_Q0Q0_L[1][1][1] * ds[1][1] +
419 tmp_Q0Q0_L[1][1][2] * ds[1][2] +
420 tmp_Q0Q0_L[1][1][3] * ds[1][3] +
421 tmp_Q0Q0_L[1][2][0] * ds[2][0] +
422 tmp_Q0Q0_L[1][2][1] * ds[2][1] +
423 tmp_Q0Q0_L[1][2][2] * ds[2][2] +
424 tmp_Q0Q0_L[1][2][3] * ds[2][3] +
425 tmp_Q0Q0_L[1][3][0] * ds[3][0] +
426 tmp_Q0Q0_L[1][3][1] * ds[3][1] +
427 tmp_Q0Q0_L[1][3][2] * ds[3][2] +
428 tmp_Q0Q0_L[1][3][3] * ds[3][3];
430 (*dq0_out)[2] = tmp_Q0Q0_L[2][0][0] * ds[0][0] +
431 tmp_Q0Q0_L[2][0][1] * ds[0][1] +
432 tmp_Q0Q0_L[2][0][2] * ds[0][2] +
433 tmp_Q0Q0_L[2][0][3] * ds[0][3] +
434 tmp_Q0Q0_L[2][1][0] * ds[1][0] +
435 tmp_Q0Q0_L[2][1][1] * ds[1][1] +
436 tmp_Q0Q0_L[2][1][2] * ds[1][2] +
437 tmp_Q0Q0_L[2][1][3] * ds[1][3] +
438 tmp_Q0Q0_L[2][2][0] * ds[2][0] +
439 tmp_Q0Q0_L[2][2][1] * ds[2][1] +
440 tmp_Q0Q0_L[2][2][2] * ds[2][2] +
441 tmp_Q0Q0_L[2][2][3] * ds[2][3] +
442 tmp_Q0Q0_L[2][3][0] * ds[3][0] +
443 tmp_Q0Q0_L[2][3][1] * ds[3][1] +
444 tmp_Q0Q0_L[2][3][2] * ds[3][2] +
445 tmp_Q0Q0_L[2][3][3] * ds[3][3];
447 (*dq0_out)[3] = tmp_Q0Q0_L[3][0][0] * ds[0][0] +
448 tmp_Q0Q0_L[3][0][1] * ds[0][1] +
449 tmp_Q0Q0_L[3][0][2] * ds[0][2] +
450 tmp_Q0Q0_L[3][0][3] * ds[0][3] +
451 tmp_Q0Q0_L[3][1][0] * ds[1][0] +
452 tmp_Q0Q0_L[3][1][1] * ds[1][1] +
453 tmp_Q0Q0_L[3][1][2] * ds[1][2] +
454 tmp_Q0Q0_L[3][1][3] * ds[1][3] +
455 tmp_Q0Q0_L[3][2][0] * ds[2][0] +
456 tmp_Q0Q0_L[3][2][1] * ds[2][1] +
457 tmp_Q0Q0_L[3][2][2] * ds[2][2] +
458 tmp_Q0Q0_L[3][2][3] * ds[2][3] +
459 tmp_Q0Q0_L[3][3][0] * ds[3][0] +
460 tmp_Q0Q0_L[3][3][1] * ds[3][1] +
461 tmp_Q0Q0_L[3][3][2] * ds[3][2] +
462 tmp_Q0Q0_L[3][3][3] * ds[3][3];