1use crate::core::base::*;
2use crate::core::quaternion::*;
3use crate::core::transform::*;
4
5#[derive(Debug, PartialEq, Copy, Clone)]
6pub struct DerivativeTerm([Float; 4]);
7
8impl DerivativeTerm {
9 pub fn zero() -> Self {
10 DerivativeTerm([0.0, 0.0, 0.0, 0.0])
11 }
12 pub fn new(c: Float, x: Float, y: Float, z: Float) -> Self {
13 DerivativeTerm([c, x, y, z])
14 }
15 pub fn eval(&self, p: &Point3f) -> Float {
16 return self.0[0] + self.0[1] * p.x + self.0[2] * p.y + self.0[3] * p.z;
17 }
18}
19
20pub fn get_derivatives(
21 t: &[Vector3f; 2],
22 r: &[Quaternion; 2],
23 s: &[Matrix4x4; 2],
24) -> [[DerivativeTerm; 3]; 5] {
25 let cos_theta = Quaternion::dot(&r[0], &r[1]);
26 let theta = Float::acos(Float::clamp(cos_theta, -1.0, 1.0));
27 let qperp = Quaternion::normalize(&(r[1] - r[0] * cos_theta));
28
29 let t0x = t[0].x;
30 let t0y = t[0].y;
31 let t0z = t[0].z;
32 let t1x = t[1].x;
33 let t1y = t[1].y;
34 let t1z = t[1].z;
35 let q0x = r[0].x;
36 let q0y = r[0].y;
37 let q0z = r[0].z;
38 let q0w = r[0].w;
39
40 let qdx = qperp.x; let qdy = qperp.y; let qdz = qperp.z; let qdw = qperp.w; let s000 = s[0].m[4 * 0 + 0];
46 let s001 = s[0].m[4 * 0 + 1];
47 let s002 = s[0].m[4 * 0 + 2];
48 let s010 = s[0].m[4 * 1 + 0];
49 let s011 = s[0].m[4 * 1 + 1];
50 let s012 = s[0].m[4 * 1 + 2];
51 let s020 = s[0].m[4 * 2 + 0];
52 let s021 = s[0].m[4 * 2 + 1];
53 let s022 = s[0].m[4 * 2 + 2];
54
55 let s100 = s[1].m[4 * 0 + 0];
56 let s101 = s[1].m[4 * 0 + 1];
57 let s102 = s[1].m[4 * 0 + 2];
58 let s110 = s[1].m[4 * 1 + 0];
59 let s111 = s[1].m[4 * 1 + 1];
60 let s112 = s[1].m[4 * 1 + 2];
61 let s120 = s[1].m[4 * 2 + 0];
62 let s121 = s[1].m[4 * 2 + 1];
63 let s122 = s[1].m[4 * 2 + 2];
64
65 let c1_0 = DerivativeTerm::new(
66 -t0x + t1x,
67 (-1.0 + q0y * q0y + q0z * q0z + qdy * qdy + qdz * qdz) * s000 + q0w * q0z * s010
68 - qdx * qdy * s010
69 + qdw * qdz * s010
70 - q0w * q0y * s020
71 - qdw * qdy * s020
72 - qdx * qdz * s020
73 + s100
74 - q0y * q0y * s100
75 - q0z * q0z * s100
76 - qdy * qdy * s100
77 - qdz * qdz * s100
78 - q0w * q0z * s110
79 + qdx * qdy * s110
80 - qdw * qdz * s110
81 + q0w * q0y * s120
82 + qdw * qdy * s120
83 + qdx * qdz * s120
84 + q0x * (-(q0y * s010) - q0z * s020 + q0y * s110 + q0z * s120),
85 (-1.0 + q0y * q0y + q0z * q0z + qdy * qdy + qdz * qdz) * s001 + q0w * q0z * s011
86 - qdx * qdy * s011
87 + qdw * qdz * s011
88 - q0w * q0y * s021
89 - qdw * qdy * s021
90 - qdx * qdz * s021
91 + s101
92 - q0y * q0y * s101
93 - q0z * q0z * s101
94 - qdy * qdy * s101
95 - qdz * qdz * s101
96 - q0w * q0z * s111
97 + qdx * qdy * s111
98 - qdw * qdz * s111
99 + q0w * q0y * s121
100 + qdw * qdy * s121
101 + qdx * qdz * s121
102 + q0x * (-(q0y * s011) - q0z * s021 + q0y * s111 + q0z * s121),
103 (-1.0 + q0y * q0y + q0z * q0z + qdy * qdy + qdz * qdz) * s002 + q0w * q0z * s012
104 - qdx * qdy * s012
105 + qdw * qdz * s012
106 - q0w * q0y * s022
107 - qdw * qdy * s022
108 - qdx * qdz * s022
109 + s102
110 - q0y * q0y * s102
111 - q0z * q0z * s102
112 - qdy * qdy * s102
113 - qdz * qdz * s102
114 - q0w * q0z * s112
115 + qdx * qdy * s112
116 - qdw * qdz * s112
117 + q0w * q0y * s122
118 + qdw * qdy * s122
119 + qdx * qdz * s122
120 + q0x * (-(q0y * s012) - q0z * s022 + q0y * s112 + q0z * s122),
121 );
122
123 let c2_0 = DerivativeTerm::new(
124 0.0,
125 -(qdy * qdy * s000) - qdz * qdz * s000 + qdx * qdy * s010 - qdw * qdz * s010
126 + qdw * qdy * s020
127 + qdx * qdz * s020
128 + q0y * q0y * (s000 - s100)
129 + q0z * q0z * (s000 - s100)
130 + qdy * qdy * s100
131 + qdz * qdz * s100
132 - qdx * qdy * s110
133 + qdw * qdz * s110
134 - qdw * qdy * s120
135 - qdx * qdz * s120
136 + 2.0 * q0x * qdy * s010 * theta
137 - 2.0 * q0w * qdz * s010 * theta
138 + 2.0 * q0w * qdy * s020 * theta
139 + 2.0 * q0x * qdz * s020 * theta
140 + q0y
141 * (q0x * (-s010 + s110)
142 + q0w * (-s020 + s120)
143 + 2.0 * (-2.0 * qdy * s000 + qdx * s010 + qdw * s020) * theta)
144 + q0z
145 * (q0w * (s010 - s110) + q0x * (-s020 + s120)
146 - 2.0 * (2.0 * qdz * s000 + qdw * s010 - qdx * s020) * theta),
147 -(qdy * qdy * s001) - qdz * qdz * s001 + qdx * qdy * s011 - qdw * qdz * s011
148 + qdw * qdy * s021
149 + qdx * qdz * s021
150 + q0y * q0y * (s001 - s101)
151 + q0z * q0z * (s001 - s101)
152 + qdy * qdy * s101
153 + qdz * qdz * s101
154 - qdx * qdy * s111
155 + qdw * qdz * s111
156 - qdw * qdy * s121
157 - qdx * qdz * s121
158 + 2.0 * q0x * qdy * s011 * theta
159 - 2.0 * q0w * qdz * s011 * theta
160 + 2.0 * q0w * qdy * s021 * theta
161 + 2.0 * q0x * qdz * s021 * theta
162 + q0y
163 * (q0x * (-s011 + s111)
164 + q0w * (-s021 + s121)
165 + 2.0 * (-2.0 * qdy * s001 + qdx * s011 + qdw * s021) * theta)
166 + q0z
167 * (q0w * (s011 - s111) + q0x * (-s021 + s121)
168 - 2.0 * (2.0 * qdz * s001 + qdw * s011 - qdx * s021) * theta),
169 -(qdy * qdy * s002) - qdz * qdz * s002 + qdx * qdy * s012 - qdw * qdz * s012
170 + qdw * qdy * s022
171 + qdx * qdz * s022
172 + q0y * q0y * (s002 - s102)
173 + q0z * q0z * (s002 - s102)
174 + qdy * qdy * s102
175 + qdz * qdz * s102
176 - qdx * qdy * s112
177 + qdw * qdz * s112
178 - qdw * qdy * s122
179 - qdx * qdz * s122
180 + 2.0 * q0x * qdy * s012 * theta
181 - 2.0 * q0w * qdz * s012 * theta
182 + 2.0 * q0w * qdy * s022 * theta
183 + 2.0 * q0x * qdz * s022 * theta
184 + q0y
185 * (q0x * (-s012 + s112)
186 + q0w * (-s022 + s122)
187 + 2.0 * (-2.0 * qdy * s002 + qdx * s012 + qdw * s022) * theta)
188 + q0z
189 * (q0w * (s012 - s112) + q0x * (-s022 + s122)
190 - 2.0 * (2.0 * qdz * s002 + qdw * s012 - qdx * s022) * theta),
191 );
192
193 let c3_0 = DerivativeTerm::new(
194 0.0,
195 -2.0 * (q0x * qdy * s010 - q0w * qdz * s010 + q0w * qdy * s020 + q0x * qdz * s020
196 - q0x * qdy * s110
197 + q0w * qdz * s110
198 - q0w * qdy * s120
199 - q0x * qdz * s120
200 + q0y
201 * (-2.0 * qdy * s000 + qdx * s010 + qdw * s020 + 2.0 * qdy * s100
202 - qdx * s110
203 - qdw * s120)
204 + q0z
205 * (-2.0 * qdz * s000 - qdw * s010 + qdx * s020 + 2.0 * qdz * s100 + qdw * s110
206 - qdx * s120))
207 * theta,
208 -2.0 * (q0x * qdy * s011 - q0w * qdz * s011 + q0w * qdy * s021 + q0x * qdz * s021
209 - q0x * qdy * s111
210 + q0w * qdz * s111
211 - q0w * qdy * s121
212 - q0x * qdz * s121
213 + q0y
214 * (-2.0 * qdy * s001 + qdx * s011 + qdw * s021 + 2.0 * qdy * s101
215 - qdx * s111
216 - qdw * s121)
217 + q0z
218 * (-2.0 * qdz * s001 - qdw * s011 + qdx * s021 + 2.0 * qdz * s101 + qdw * s111
219 - qdx * s121))
220 * theta,
221 -2.0 * (q0x * qdy * s012 - q0w * qdz * s012 + q0w * qdy * s022 + q0x * qdz * s022
222 - q0x * qdy * s112
223 + q0w * qdz * s112
224 - q0w * qdy * s122
225 - q0x * qdz * s122
226 + q0y
227 * (-2.0 * qdy * s002 + qdx * s012 + qdw * s022 + 2.0 * qdy * s102
228 - qdx * s112
229 - qdw * s122)
230 + q0z
231 * (-2.0 * qdz * s002 - qdw * s012 + qdx * s022 + 2.0 * qdz * s102 + qdw * s112
232 - qdx * s122))
233 * theta,
234 );
235
236 let c4_0 = DerivativeTerm::new(
237 0.0,
238 -(q0x * qdy * s010) + q0w * qdz * s010 - q0w * qdy * s020 - q0x * qdz * s020
239 + q0x * qdy * s110
240 - q0w * qdz * s110
241 + q0w * qdy * s120
242 + q0x * qdz * s120
243 + 2.0 * q0y * q0y * s000 * theta
244 + 2.0 * q0z * q0z * s000 * theta
245 - 2.0 * qdy * qdy * s000 * theta
246 - 2.0 * qdz * qdz * s000 * theta
247 + 2.0 * qdx * qdy * s010 * theta
248 - 2.0 * qdw * qdz * s010 * theta
249 + 2.0 * qdw * qdy * s020 * theta
250 + 2.0 * qdx * qdz * s020 * theta
251 + q0y
252 * (-(qdx * s010) - qdw * s020
253 + 2.0 * qdy * (s000 - s100)
254 + qdx * s110
255 + qdw * s120
256 - 2.0 * q0x * s010 * theta
257 - 2.0 * q0w * s020 * theta)
258 + q0z
259 * (2.0 * qdz * s000 + qdw * s010 - qdx * s020 - 2.0 * qdz * s100 - qdw * s110
260 + qdx * s120
261 + 2.0 * q0w * s010 * theta
262 - 2.0 * q0x * s020 * theta),
263 -(q0x * qdy * s011) + q0w * qdz * s011 - q0w * qdy * s021 - q0x * qdz * s021
264 + q0x * qdy * s111
265 - q0w * qdz * s111
266 + q0w * qdy * s121
267 + q0x * qdz * s121
268 + 2.0 * q0y * q0y * s001 * theta
269 + 2.0 * q0z * q0z * s001 * theta
270 - 2.0 * qdy * qdy * s001 * theta
271 - 2.0 * qdz * qdz * s001 * theta
272 + 2.0 * qdx * qdy * s011 * theta
273 - 2.0 * qdw * qdz * s011 * theta
274 + 2.0 * qdw * qdy * s021 * theta
275 + 2.0 * qdx * qdz * s021 * theta
276 + q0y
277 * (-(qdx * s011) - qdw * s021
278 + 2.0 * qdy * (s001 - s101)
279 + qdx * s111
280 + qdw * s121
281 - 2.0 * q0x * s011 * theta
282 - 2.0 * q0w * s021 * theta)
283 + q0z
284 * (2.0 * qdz * s001 + qdw * s011 - qdx * s021 - 2.0 * qdz * s101 - qdw * s111
285 + qdx * s121
286 + 2.0 * q0w * s011 * theta
287 - 2.0 * q0x * s021 * theta),
288 -(q0x * qdy * s012) + q0w * qdz * s012 - q0w * qdy * s022 - q0x * qdz * s022
289 + q0x * qdy * s112
290 - q0w * qdz * s112
291 + q0w * qdy * s122
292 + q0x * qdz * s122
293 + 2.0 * q0y * q0y * s002 * theta
294 + 2.0 * q0z * q0z * s002 * theta
295 - 2.0 * qdy * qdy * s002 * theta
296 - 2.0 * qdz * qdz * s002 * theta
297 + 2.0 * qdx * qdy * s012 * theta
298 - 2.0 * qdw * qdz * s012 * theta
299 + 2.0 * qdw * qdy * s022 * theta
300 + 2.0 * qdx * qdz * s022 * theta
301 + q0y
302 * (-(qdx * s012) - qdw * s022
303 + 2.0 * qdy * (s002 - s102)
304 + qdx * s112
305 + qdw * s122
306 - 2.0 * q0x * s012 * theta
307 - 2.0 * q0w * s022 * theta)
308 + q0z
309 * (2.0 * qdz * s002 + qdw * s012 - qdx * s022 - 2.0 * qdz * s102 - qdw * s112
310 + qdx * s122
311 + 2.0 * q0w * s012 * theta
312 - 2.0 * q0x * s022 * theta),
313 );
314
315 let c5_0 = DerivativeTerm::new(
316 0.0,
317 2.0 * (qdy * qdy * s000 + qdz * qdz * s000 - qdx * qdy * s010 + qdw * qdz * s010
318 - qdw * qdy * s020
319 - qdx * qdz * s020
320 - qdy * qdy * s100
321 - qdz * qdz * s100
322 + q0y * q0y * (-s000 + s100)
323 + q0z * q0z * (-s000 + s100)
324 + qdx * qdy * s110
325 - qdw * qdz * s110
326 + q0y * (q0x * (s010 - s110) + q0w * (s020 - s120))
327 + qdw * qdy * s120
328 + qdx * qdz * s120
329 + q0z * (-(q0w * s010) + q0x * s020 + q0w * s110 - q0x * s120))
330 * theta,
331 2.0 * (qdy * qdy * s001 + qdz * qdz * s001 - qdx * qdy * s011 + qdw * qdz * s011
332 - qdw * qdy * s021
333 - qdx * qdz * s021
334 - qdy * qdy * s101
335 - qdz * qdz * s101
336 + q0y * q0y * (-s001 + s101)
337 + q0z * q0z * (-s001 + s101)
338 + qdx * qdy * s111
339 - qdw * qdz * s111
340 + q0y * (q0x * (s011 - s111) + q0w * (s021 - s121))
341 + qdw * qdy * s121
342 + qdx * qdz * s121
343 + q0z * (-(q0w * s011) + q0x * s021 + q0w * s111 - q0x * s121))
344 * theta,
345 2.0 * (qdy * qdy * s002 + qdz * qdz * s002 - qdx * qdy * s012 + qdw * qdz * s012
346 - qdw * qdy * s022
347 - qdx * qdz * s022
348 - qdy * qdy * s102
349 - qdz * qdz * s102
350 + q0y * q0y * (-s002 + s102)
351 + q0z * q0z * (-s002 + s102)
352 + qdx * qdy * s112
353 - qdw * qdz * s112
354 + q0y * (q0x * (s012 - s112) + q0w * (s022 - s122))
355 + qdw * qdy * s122
356 + qdx * qdz * s122
357 + q0z * (-(q0w * s012) + q0x * s022 + q0w * s112 - q0x * s122))
358 * theta,
359 );
360
361 let c1_1 = DerivativeTerm::new(
362 -t0y + t1y,
363 -(qdx * qdy * s000) - qdw * qdz * s000 - s010
364 + q0z * q0z * s010
365 + qdx * qdx * s010
366 + qdz * qdz * s010
367 - q0y * q0z * s020
368 + qdw * qdx * s020
369 - qdy * qdz * s020
370 + qdx * qdy * s100
371 + qdw * qdz * s100
372 + q0w * q0z * (-s000 + s100)
373 + q0x * q0x * (s010 - s110)
374 + s110
375 - q0z * q0z * s110
376 - qdx * qdx * s110
377 - qdz * qdz * s110
378 + q0x * (q0y * (-s000 + s100) + q0w * (s020 - s120))
379 + q0y * q0z * s120
380 - qdw * qdx * s120
381 + qdy * qdz * s120,
382 -(qdx * qdy * s001) - qdw * qdz * s001 - s011
383 + q0z * q0z * s011
384 + qdx * qdx * s011
385 + qdz * qdz * s011
386 - q0y * q0z * s021
387 + qdw * qdx * s021
388 - qdy * qdz * s021
389 + qdx * qdy * s101
390 + qdw * qdz * s101
391 + q0w * q0z * (-s001 + s101)
392 + q0x * q0x * (s011 - s111)
393 + s111
394 - q0z * q0z * s111
395 - qdx * qdx * s111
396 - qdz * qdz * s111
397 + q0x * (q0y * (-s001 + s101) + q0w * (s021 - s121))
398 + q0y * q0z * s121
399 - qdw * qdx * s121
400 + qdy * qdz * s121,
401 -(qdx * qdy * s002) - qdw * qdz * s002 - s012
402 + q0z * q0z * s012
403 + qdx * qdx * s012
404 + qdz * qdz * s012
405 - q0y * q0z * s022
406 + qdw * qdx * s022
407 - qdy * qdz * s022
408 + qdx * qdy * s102
409 + qdw * qdz * s102
410 + q0w * q0z * (-s002 + s102)
411 + q0x * q0x * (s012 - s112)
412 + s112
413 - q0z * q0z * s112
414 - qdx * qdx * s112
415 - qdz * qdz * s112
416 + q0x * (q0y * (-s002 + s102) + q0w * (s022 - s122))
417 + q0y * q0z * s122
418 - qdw * qdx * s122
419 + qdy * qdz * s122,
420 );
421
422 let c2_1 = DerivativeTerm::new(
423 0.0,
424 qdx * qdy * s000 + qdw * qdz * s000 + q0z * q0z * s010
425 - qdx * qdx * s010
426 - qdz * qdz * s010
427 - q0y * q0z * s020
428 - qdw * qdx * s020
429 + qdy * qdz * s020
430 - qdx * qdy * s100
431 - qdw * qdz * s100
432 + q0x * q0x * (s010 - s110)
433 - q0z * q0z * s110
434 + qdx * qdx * s110
435 + qdz * qdz * s110
436 + q0y * q0z * s120
437 + qdw * qdx * s120
438 - qdy * qdz * s120
439 + 2.0 * q0z * qdw * s000 * theta
440 + 2.0 * q0y * qdx * s000 * theta
441 - 4.0 * q0z * qdz * s010 * theta
442 + 2.0 * q0z * qdy * s020 * theta
443 + 2.0 * q0y * qdz * s020 * theta
444 + q0x
445 * (q0w * s020 + q0y * (-s000 + s100) - q0w * s120 + 2.0 * qdy * s000 * theta
446 - 4.0 * qdx * s010 * theta
447 - 2.0 * qdw * s020 * theta)
448 + q0w
449 * (-(q0z * s000) + q0z * s100 + 2.0 * qdz * s000 * theta
450 - 2.0 * qdx * s020 * theta),
451 qdx * qdy * s001 + qdw * qdz * s001 + q0z * q0z * s011
452 - qdx * qdx * s011
453 - qdz * qdz * s011
454 - q0y * q0z * s021
455 - qdw * qdx * s021
456 + qdy * qdz * s021
457 - qdx * qdy * s101
458 - qdw * qdz * s101
459 + q0x * q0x * (s011 - s111)
460 - q0z * q0z * s111
461 + qdx * qdx * s111
462 + qdz * qdz * s111
463 + q0y * q0z * s121
464 + qdw * qdx * s121
465 - qdy * qdz * s121
466 + 2.0 * q0z * qdw * s001 * theta
467 + 2.0 * q0y * qdx * s001 * theta
468 - 4.0 * q0z * qdz * s011 * theta
469 + 2.0 * q0z * qdy * s021 * theta
470 + 2.0 * q0y * qdz * s021 * theta
471 + q0x
472 * (q0w * s021 + q0y * (-s001 + s101) - q0w * s121 + 2.0 * qdy * s001 * theta
473 - 4.0 * qdx * s011 * theta
474 - 2.0 * qdw * s021 * theta)
475 + q0w
476 * (-(q0z * s001) + q0z * s101 + 2.0 * qdz * s001 * theta
477 - 2.0 * qdx * s021 * theta),
478 qdx * qdy * s002 + qdw * qdz * s002 + q0z * q0z * s012
479 - qdx * qdx * s012
480 - qdz * qdz * s012
481 - q0y * q0z * s022
482 - qdw * qdx * s022
483 + qdy * qdz * s022
484 - qdx * qdy * s102
485 - qdw * qdz * s102
486 + q0x * q0x * (s012 - s112)
487 - q0z * q0z * s112
488 + qdx * qdx * s112
489 + qdz * qdz * s112
490 + q0y * q0z * s122
491 + qdw * qdx * s122
492 - qdy * qdz * s122
493 + 2.0 * q0z * qdw * s002 * theta
494 + 2.0 * q0y * qdx * s002 * theta
495 - 4.0 * q0z * qdz * s012 * theta
496 + 2.0 * q0z * qdy * s022 * theta
497 + 2.0 * q0y * qdz * s022 * theta
498 + q0x
499 * (q0w * s022 + q0y * (-s002 + s102) - q0w * s122 + 2.0 * qdy * s002 * theta
500 - 4.0 * qdx * s012 * theta
501 - 2.0 * qdw * s022 * theta)
502 + q0w
503 * (-(q0z * s002) + q0z * s102 + 2.0 * qdz * s002 * theta
504 - 2.0 * qdx * s022 * theta),
505 );
506
507 let c3_1 = DerivativeTerm::new(
508 0.0,
509 2.0 * (-(q0x * qdy * s000) - q0w * qdz * s000
510 + 2.0 * q0x * qdx * s010
511 + q0x * qdw * s020
512 + q0w * qdx * s020
513 + q0x * qdy * s100
514 + q0w * qdz * s100
515 - 2.0 * q0x * qdx * s110
516 - q0x * qdw * s120
517 - q0w * qdx * s120
518 + q0z
519 * (2.0 * qdz * s010 - qdy * s020 + qdw * (-s000 + s100) - 2.0 * qdz * s110
520 + qdy * s120)
521 + q0y * (-(qdx * s000) - qdz * s020 + qdx * s100 + qdz * s120))
522 * theta,
523 2.0 * (-(q0x * qdy * s001) - q0w * qdz * s001
524 + 2.0 * q0x * qdx * s011
525 + q0x * qdw * s021
526 + q0w * qdx * s021
527 + q0x * qdy * s101
528 + q0w * qdz * s101
529 - 2.0 * q0x * qdx * s111
530 - q0x * qdw * s121
531 - q0w * qdx * s121
532 + q0z
533 * (2.0 * qdz * s011 - qdy * s021 + qdw * (-s001 + s101) - 2.0 * qdz * s111
534 + qdy * s121)
535 + q0y * (-(qdx * s001) - qdz * s021 + qdx * s101 + qdz * s121))
536 * theta,
537 2.0 * (-(q0x * qdy * s002) - q0w * qdz * s002
538 + 2.0 * q0x * qdx * s012
539 + q0x * qdw * s022
540 + q0w * qdx * s022
541 + q0x * qdy * s102
542 + q0w * qdz * s102
543 - 2.0 * q0x * qdx * s112
544 - q0x * qdw * s122
545 - q0w * qdx * s122
546 + q0z
547 * (2.0 * qdz * s012 - qdy * s022 + qdw * (-s002 + s102) - 2.0 * qdz * s112
548 + qdy * s122)
549 + q0y * (-(qdx * s002) - qdz * s022 + qdx * s102 + qdz * s122))
550 * theta,
551 );
552
553 let c4_1 = DerivativeTerm::new(
554 0.0,
555 -(q0x * qdy * s000) - q0w * qdz * s000
556 + 2.0 * q0x * qdx * s010
557 + q0x * qdw * s020
558 + q0w * qdx * s020
559 + q0x * qdy * s100
560 + q0w * qdz * s100
561 - 2.0 * q0x * qdx * s110
562 - q0x * qdw * s120
563 - q0w * qdx * s120
564 + 2.0 * qdx * qdy * s000 * theta
565 + 2.0 * qdw * qdz * s000 * theta
566 + 2.0 * q0x * q0x * s010 * theta
567 + 2.0 * q0z * q0z * s010 * theta
568 - 2.0 * qdx * qdx * s010 * theta
569 - 2.0 * qdz * qdz * s010 * theta
570 + 2.0 * q0w * q0x * s020 * theta
571 - 2.0 * qdw * qdx * s020 * theta
572 + 2.0 * qdy * qdz * s020 * theta
573 + q0y
574 * (-(qdx * s000) - qdz * s020 + qdx * s100 + qdz * s120 - 2.0 * q0x * s000 * theta)
575 + q0z
576 * (2.0 * qdz * s010 - qdy * s020 + qdw * (-s000 + s100) - 2.0 * qdz * s110
577 + qdy * s120
578 - 2.0 * q0w * s000 * theta
579 - 2.0 * q0y * s020 * theta),
580 -(q0x * qdy * s001) - q0w * qdz * s001
581 + 2.0 * q0x * qdx * s011
582 + q0x * qdw * s021
583 + q0w * qdx * s021
584 + q0x * qdy * s101
585 + q0w * qdz * s101
586 - 2.0 * q0x * qdx * s111
587 - q0x * qdw * s121
588 - q0w * qdx * s121
589 + 2.0 * qdx * qdy * s001 * theta
590 + 2.0 * qdw * qdz * s001 * theta
591 + 2.0 * q0x * q0x * s011 * theta
592 + 2.0 * q0z * q0z * s011 * theta
593 - 2.0 * qdx * qdx * s011 * theta
594 - 2.0 * qdz * qdz * s011 * theta
595 + 2.0 * q0w * q0x * s021 * theta
596 - 2.0 * qdw * qdx * s021 * theta
597 + 2.0 * qdy * qdz * s021 * theta
598 + q0y
599 * (-(qdx * s001) - qdz * s021 + qdx * s101 + qdz * s121 - 2.0 * q0x * s001 * theta)
600 + q0z
601 * (2.0 * qdz * s011 - qdy * s021 + qdw * (-s001 + s101) - 2.0 * qdz * s111
602 + qdy * s121
603 - 2.0 * q0w * s001 * theta
604 - 2.0 * q0y * s021 * theta),
605 -(q0x * qdy * s002) - q0w * qdz * s002
606 + 2.0 * q0x * qdx * s012
607 + q0x * qdw * s022
608 + q0w * qdx * s022
609 + q0x * qdy * s102
610 + q0w * qdz * s102
611 - 2.0 * q0x * qdx * s112
612 - q0x * qdw * s122
613 - q0w * qdx * s122
614 + 2.0 * qdx * qdy * s002 * theta
615 + 2.0 * qdw * qdz * s002 * theta
616 + 2.0 * q0x * q0x * s012 * theta
617 + 2.0 * q0z * q0z * s012 * theta
618 - 2.0 * qdx * qdx * s012 * theta
619 - 2.0 * qdz * qdz * s012 * theta
620 + 2.0 * q0w * q0x * s022 * theta
621 - 2.0 * qdw * qdx * s022 * theta
622 + 2.0 * qdy * qdz * s022 * theta
623 + q0y
624 * (-(qdx * s002) - qdz * s022 + qdx * s102 + qdz * s122 - 2.0 * q0x * s002 * theta)
625 + q0z
626 * (2.0 * qdz * s012 - qdy * s022 + qdw * (-s002 + s102) - 2.0 * qdz * s112
627 + qdy * s122
628 - 2.0 * q0w * s002 * theta
629 - 2.0 * q0y * s022 * theta),
630 );
631
632 let c5_1 = DerivativeTerm::new(
633 0.0,
634 -2.0 * (qdx * qdy * s000 + qdw * qdz * s000 + q0z * q0z * s010
635 - qdx * qdx * s010
636 - qdz * qdz * s010
637 - q0y * q0z * s020
638 - qdw * qdx * s020
639 + qdy * qdz * s020
640 - qdx * qdy * s100
641 - qdw * qdz * s100
642 + q0w * q0z * (-s000 + s100)
643 + q0x * q0x * (s010 - s110)
644 - q0z * q0z * s110
645 + qdx * qdx * s110
646 + qdz * qdz * s110
647 + q0x * (q0y * (-s000 + s100) + q0w * (s020 - s120))
648 + q0y * q0z * s120
649 + qdw * qdx * s120
650 - qdy * qdz * s120)
651 * theta,
652 -2.0 * (qdx * qdy * s001 + qdw * qdz * s001 + q0z * q0z * s011
653 - qdx * qdx * s011
654 - qdz * qdz * s011
655 - q0y * q0z * s021
656 - qdw * qdx * s021
657 + qdy * qdz * s021
658 - qdx * qdy * s101
659 - qdw * qdz * s101
660 + q0w * q0z * (-s001 + s101)
661 + q0x * q0x * (s011 - s111)
662 - q0z * q0z * s111
663 + qdx * qdx * s111
664 + qdz * qdz * s111
665 + q0x * (q0y * (-s001 + s101) + q0w * (s021 - s121))
666 + q0y * q0z * s121
667 + qdw * qdx * s121
668 - qdy * qdz * s121)
669 * theta,
670 -2.0 * (qdx * qdy * s002 + qdw * qdz * s002 + q0z * q0z * s012
671 - qdx * qdx * s012
672 - qdz * qdz * s012
673 - q0y * q0z * s022
674 - qdw * qdx * s022
675 + qdy * qdz * s022
676 - qdx * qdy * s102
677 - qdw * qdz * s102
678 + q0w * q0z * (-s002 + s102)
679 + q0x * q0x * (s012 - s112)
680 - q0z * q0z * s112
681 + qdx * qdx * s112
682 + qdz * qdz * s112
683 + q0x * (q0y * (-s002 + s102) + q0w * (s022 - s122))
684 + q0y * q0z * s122
685 + qdw * qdx * s122
686 - qdy * qdz * s122)
687 * theta,
688 );
689
690 let c1_2 = DerivativeTerm::new(
691 -t0z + t1z,
692 qdw * qdy * s000
693 - qdx * qdz * s000
694 - q0y * q0z * s010
695 - qdw * qdx * s010
696 - qdy * qdz * s010
697 - s020
698 + q0y * q0y * s020
699 + qdx * qdx * s020
700 + qdy * qdy * s020
701 - qdw * qdy * s100
702 + qdx * qdz * s100
703 + q0x * q0z * (-s000 + s100)
704 + q0y * q0z * s110
705 + qdw * qdx * s110
706 + qdy * qdz * s110
707 + q0w * (q0y * (s000 - s100) + q0x * (-s010 + s110))
708 + q0x * q0x * (s020 - s120)
709 + s120
710 - q0y * q0y * s120
711 - qdx * qdx * s120
712 - qdy * qdy * s120,
713 qdw * qdy * s001
714 - qdx * qdz * s001
715 - q0y * q0z * s011
716 - qdw * qdx * s011
717 - qdy * qdz * s011
718 - s021
719 + q0y * q0y * s021
720 + qdx * qdx * s021
721 + qdy * qdy * s021
722 - qdw * qdy * s101
723 + qdx * qdz * s101
724 + q0x * q0z * (-s001 + s101)
725 + q0y * q0z * s111
726 + qdw * qdx * s111
727 + qdy * qdz * s111
728 + q0w * (q0y * (s001 - s101) + q0x * (-s011 + s111))
729 + q0x * q0x * (s021 - s121)
730 + s121
731 - q0y * q0y * s121
732 - qdx * qdx * s121
733 - qdy * qdy * s121,
734 qdw * qdy * s002
735 - qdx * qdz * s002
736 - q0y * q0z * s012
737 - qdw * qdx * s012
738 - qdy * qdz * s012
739 - s022
740 + q0y * q0y * s022
741 + qdx * qdx * s022
742 + qdy * qdy * s022
743 - qdw * qdy * s102
744 + qdx * qdz * s102
745 + q0x * q0z * (-s002 + s102)
746 + q0y * q0z * s112
747 + qdw * qdx * s112
748 + qdy * qdz * s112
749 + q0w * (q0y * (s002 - s102) + q0x * (-s012 + s112))
750 + q0x * q0x * (s022 - s122)
751 + s122
752 - q0y * q0y * s122
753 - qdx * qdx * s122
754 - qdy * qdy * s122,
755 );
756
757 let c2_2 = DerivativeTerm::new(
758 0.0,
759 q0w * q0y * s000 - q0x * q0z * s000 - qdw * qdy * s000 + qdx * qdz * s000
760 - q0w * q0x * s010
761 - q0y * q0z * s010
762 + qdw * qdx * s010
763 + qdy * qdz * s010
764 + q0x * q0x * s020
765 + q0y * q0y * s020
766 - qdx * qdx * s020
767 - qdy * qdy * s020
768 - q0w * q0y * s100
769 + q0x * q0z * s100
770 + qdw * qdy * s100
771 - qdx * qdz * s100
772 + q0w * q0x * s110
773 + q0y * q0z * s110
774 - qdw * qdx * s110
775 - qdy * qdz * s110
776 - q0x * q0x * s120
777 - q0y * q0y * s120
778 + qdx * qdx * s120
779 + qdy * qdy * s120
780 - 2.0 * q0y * qdw * s000 * theta
781 + 2.0 * q0z * qdx * s000 * theta
782 - 2.0 * q0w * qdy * s000 * theta
783 + 2.0 * q0x * qdz * s000 * theta
784 + 2.0 * q0x * qdw * s010 * theta
785 + 2.0 * q0w * qdx * s010 * theta
786 + 2.0 * q0z * qdy * s010 * theta
787 + 2.0 * q0y * qdz * s010 * theta
788 - 4.0 * q0x * qdx * s020 * theta
789 - 4.0 * q0y * qdy * s020 * theta,
790 q0w * q0y * s001 - q0x * q0z * s001 - qdw * qdy * s001 + qdx * qdz * s001
791 - q0w * q0x * s011
792 - q0y * q0z * s011
793 + qdw * qdx * s011
794 + qdy * qdz * s011
795 + q0x * q0x * s021
796 + q0y * q0y * s021
797 - qdx * qdx * s021
798 - qdy * qdy * s021
799 - q0w * q0y * s101
800 + q0x * q0z * s101
801 + qdw * qdy * s101
802 - qdx * qdz * s101
803 + q0w * q0x * s111
804 + q0y * q0z * s111
805 - qdw * qdx * s111
806 - qdy * qdz * s111
807 - q0x * q0x * s121
808 - q0y * q0y * s121
809 + qdx * qdx * s121
810 + qdy * qdy * s121
811 - 2.0 * q0y * qdw * s001 * theta
812 + 2.0 * q0z * qdx * s001 * theta
813 - 2.0 * q0w * qdy * s001 * theta
814 + 2.0 * q0x * qdz * s001 * theta
815 + 2.0 * q0x * qdw * s011 * theta
816 + 2.0 * q0w * qdx * s011 * theta
817 + 2.0 * q0z * qdy * s011 * theta
818 + 2.0 * q0y * qdz * s011 * theta
819 - 4.0 * q0x * qdx * s021 * theta
820 - 4.0 * q0y * qdy * s021 * theta,
821 q0w * q0y * s002 - q0x * q0z * s002 - qdw * qdy * s002 + qdx * qdz * s002
822 - q0w * q0x * s012
823 - q0y * q0z * s012
824 + qdw * qdx * s012
825 + qdy * qdz * s012
826 + q0x * q0x * s022
827 + q0y * q0y * s022
828 - qdx * qdx * s022
829 - qdy * qdy * s022
830 - q0w * q0y * s102
831 + q0x * q0z * s102
832 + qdw * qdy * s102
833 - qdx * qdz * s102
834 + q0w * q0x * s112
835 + q0y * q0z * s112
836 - qdw * qdx * s112
837 - qdy * qdz * s112
838 - q0x * q0x * s122
839 - q0y * q0y * s122
840 + qdx * qdx * s122
841 + qdy * qdy * s122
842 - 2.0 * q0y * qdw * s002 * theta
843 + 2.0 * q0z * qdx * s002 * theta
844 - 2.0 * q0w * qdy * s002 * theta
845 + 2.0 * q0x * qdz * s002 * theta
846 + 2.0 * q0x * qdw * s012 * theta
847 + 2.0 * q0w * qdx * s012 * theta
848 + 2.0 * q0z * qdy * s012 * theta
849 + 2.0 * q0y * qdz * s012 * theta
850 - 4.0 * q0x * qdx * s022 * theta
851 - 4.0 * q0y * qdy * s022 * theta,
852 );
853
854 let c3_2 = DerivativeTerm::new(
855 0.0,
856 -2.0 * (-(q0w * qdy * s000) + q0x * qdz * s000 + q0x * qdw * s010 + q0w * qdx * s010
857 - 2.0 * q0x * qdx * s020
858 + q0w * qdy * s100
859 - q0x * qdz * s100
860 - q0x * qdw * s110
861 - q0w * qdx * s110
862 + q0z * (qdx * s000 + qdy * s010 - qdx * s100 - qdy * s110)
863 + 2.0 * q0x * qdx * s120
864 + q0y
865 * (qdz * s010 - 2.0 * qdy * s020 + qdw * (-s000 + s100) - qdz * s110
866 + 2.0 * qdy * s120))
867 * theta,
868 -2.0 * (-(q0w * qdy * s001) + q0x * qdz * s001 + q0x * qdw * s011 + q0w * qdx * s011
869 - 2.0 * q0x * qdx * s021
870 + q0w * qdy * s101
871 - q0x * qdz * s101
872 - q0x * qdw * s111
873 - q0w * qdx * s111
874 + q0z * (qdx * s001 + qdy * s011 - qdx * s101 - qdy * s111)
875 + 2.0 * q0x * qdx * s121
876 + q0y
877 * (qdz * s011 - 2.0 * qdy * s021 + qdw * (-s001 + s101) - qdz * s111
878 + 2.0 * qdy * s121))
879 * theta,
880 -2.0 * (-(q0w * qdy * s002) + q0x * qdz * s002 + q0x * qdw * s012 + q0w * qdx * s012
881 - 2.0 * q0x * qdx * s022
882 + q0w * qdy * s102
883 - q0x * qdz * s102
884 - q0x * qdw * s112
885 - q0w * qdx * s112
886 + q0z * (qdx * s002 + qdy * s012 - qdx * s102 - qdy * s112)
887 + 2.0 * q0x * qdx * s122
888 + q0y
889 * (qdz * s012 - 2.0 * qdy * s022 + qdw * (-s002 + s102) - qdz * s112
890 + 2.0 * qdy * s122))
891 * theta,
892 );
893
894 let c4_2 = DerivativeTerm::new(
895 0.0,
896 q0w * qdy * s000 - q0x * qdz * s000 - q0x * qdw * s010 - q0w * qdx * s010
897 + 2.0 * q0x * qdx * s020
898 - q0w * qdy * s100
899 + q0x * qdz * s100
900 + q0x * qdw * s110
901 + q0w * qdx * s110
902 - 2.0 * q0x * qdx * s120
903 - 2.0 * qdw * qdy * s000 * theta
904 + 2.0 * qdx * qdz * s000 * theta
905 - 2.0 * q0w * q0x * s010 * theta
906 + 2.0 * qdw * qdx * s010 * theta
907 + 2.0 * qdy * qdz * s010 * theta
908 + 2.0 * q0x * q0x * s020 * theta
909 + 2.0 * q0y * q0y * s020 * theta
910 - 2.0 * qdx * qdx * s020 * theta
911 - 2.0 * qdy * qdy * s020 * theta
912 + q0z
913 * (-(qdx * s000) - qdy * s010 + qdx * s100 + qdy * s110 - 2.0 * q0x * s000 * theta)
914 + q0y
915 * (-(qdz * s010) + 2.0 * qdy * s020 + qdw * (s000 - s100) + qdz * s110
916 - 2.0 * qdy * s120
917 + 2.0 * q0w * s000 * theta
918 - 2.0 * q0z * s010 * theta),
919 q0w * qdy * s001 - q0x * qdz * s001 - q0x * qdw * s011 - q0w * qdx * s011
920 + 2.0 * q0x * qdx * s021
921 - q0w * qdy * s101
922 + q0x * qdz * s101
923 + q0x * qdw * s111
924 + q0w * qdx * s111
925 - 2.0 * q0x * qdx * s121
926 - 2.0 * qdw * qdy * s001 * theta
927 + 2.0 * qdx * qdz * s001 * theta
928 - 2.0 * q0w * q0x * s011 * theta
929 + 2.0 * qdw * qdx * s011 * theta
930 + 2.0 * qdy * qdz * s011 * theta
931 + 2.0 * q0x * q0x * s021 * theta
932 + 2.0 * q0y * q0y * s021 * theta
933 - 2.0 * qdx * qdx * s021 * theta
934 - 2.0 * qdy * qdy * s021 * theta
935 + q0z
936 * (-(qdx * s001) - qdy * s011 + qdx * s101 + qdy * s111 - 2.0 * q0x * s001 * theta)
937 + q0y
938 * (-(qdz * s011) + 2.0 * qdy * s021 + qdw * (s001 - s101) + qdz * s111
939 - 2.0 * qdy * s121
940 + 2.0 * q0w * s001 * theta
941 - 2.0 * q0z * s011 * theta),
942 q0w * qdy * s002 - q0x * qdz * s002 - q0x * qdw * s012 - q0w * qdx * s012
943 + 2.0 * q0x * qdx * s022
944 - q0w * qdy * s102
945 + q0x * qdz * s102
946 + q0x * qdw * s112
947 + q0w * qdx * s112
948 - 2.0 * q0x * qdx * s122
949 - 2.0 * qdw * qdy * s002 * theta
950 + 2.0 * qdx * qdz * s002 * theta
951 - 2.0 * q0w * q0x * s012 * theta
952 + 2.0 * qdw * qdx * s012 * theta
953 + 2.0 * qdy * qdz * s012 * theta
954 + 2.0 * q0x * q0x * s022 * theta
955 + 2.0 * q0y * q0y * s022 * theta
956 - 2.0 * qdx * qdx * s022 * theta
957 - 2.0 * qdy * qdy * s022 * theta
958 + q0z
959 * (-(qdx * s002) - qdy * s012 + qdx * s102 + qdy * s112 - 2.0 * q0x * s002 * theta)
960 + q0y
961 * (-(qdz * s012) + 2.0 * qdy * s022 + qdw * (s002 - s102) + qdz * s112
962 - 2.0 * qdy * s122
963 + 2.0 * q0w * s002 * theta
964 - 2.0 * q0z * s012 * theta),
965 );
966
967 let c5_2 = DerivativeTerm::new(
968 0.0,
969 2.0 * (qdw * qdy * s000 - qdx * qdz * s000 + q0y * q0z * s010
970 - qdw * qdx * s010
971 - qdy * qdz * s010
972 - q0y * q0y * s020
973 + qdx * qdx * s020
974 + qdy * qdy * s020
975 + q0x * q0z * (s000 - s100)
976 - qdw * qdy * s100
977 + qdx * qdz * s100
978 + q0w * (q0y * (-s000 + s100) + q0x * (s010 - s110))
979 - q0y * q0z * s110
980 + qdw * qdx * s110
981 + qdy * qdz * s110
982 + q0y * q0y * s120
983 - qdx * qdx * s120
984 - qdy * qdy * s120
985 + q0x * q0x * (-s020 + s120))
986 * theta,
987 2.0 * (qdw * qdy * s001 - qdx * qdz * s001 + q0y * q0z * s011
988 - qdw * qdx * s011
989 - qdy * qdz * s011
990 - q0y * q0y * s021
991 + qdx * qdx * s021
992 + qdy * qdy * s021
993 + q0x * q0z * (s001 - s101)
994 - qdw * qdy * s101
995 + qdx * qdz * s101
996 + q0w * (q0y * (-s001 + s101) + q0x * (s011 - s111))
997 - q0y * q0z * s111
998 + qdw * qdx * s111
999 + qdy * qdz * s111
1000 + q0y * q0y * s121
1001 - qdx * qdx * s121
1002 - qdy * qdy * s121
1003 + q0x * q0x * (-s021 + s121))
1004 * theta,
1005 2.0 * (qdw * qdy * s002 - qdx * qdz * s002 + q0y * q0z * s012
1006 - qdw * qdx * s012
1007 - qdy * qdz * s012
1008 - q0y * q0y * s022
1009 + qdx * qdx * s022
1010 + qdy * qdy * s022
1011 + q0x * q0z * (s002 - s102)
1012 - qdw * qdy * s102
1013 + qdx * qdz * s102
1014 + q0w * (q0y * (-s002 + s102) + q0x * (s012 - s112))
1015 - q0y * q0z * s112
1016 + qdw * qdx * s112
1017 + qdy * qdz * s112
1018 + q0y * q0y * s122
1019 - qdx * qdx * s122
1020 - qdy * qdy * s122
1021 + q0x * q0x * (-s022 + s122))
1022 * theta,
1023 );
1024
1025 let c1 = [c1_0, c1_1, c1_2];
1026 let c2 = [c2_0, c2_1, c2_2];
1027 let c3 = [c3_0, c3_1, c3_2];
1028 let c4 = [c4_0, c4_1, c4_2];
1029 let c5 = [c5_0, c5_1, c5_2];
1030 return [c1, c2, c3, c4, c5];
1031}