32 const double t2 = t * t;
33 const double t3 = t2 * t;
34 const double t4 = t3 * t;
35 const double t5 = t4 * t;
36 const double t6 = t5 * t;
37 const double t7 = t6 * t;
38 const double t8 = t7 * t;
40 wp.first = 70.0 * t8 - 280.0 * t7 + 420.0 * t6 - 280.0 * t5 + 70.0 * t4;
41 wp.second = 1.0 * pi[8] * t8 - 8.0 * pi[8] * t7 + 28.0 * pi[8] * t6 -
42 56.0 * pi[8] * t5 + 70.0 * pi[8] * t4 - 56.0 * pi[8] * t3 +
43 28.0 * pi[8] * t2 - 8.0 * pi[8] * t + 1.0 * pi[8] -
44 8.0 * pi[1] * t8 + 56.0 * pi[1] * t7 - 168.0 * pi[1] * t6 +
45 280.0 * pi[1] * t5 - 280.0 * pi[1] * t4 + 168.0 * pi[1] * t3 -
46 56.0 * pi[1] * t2 + 8.0 * pi[1] * t + 28.0 * pi[2] * t8 -
47 168.0 * pi[2] * t7 + 420.0 * pi[2] * t6 - 560.0 * pi[2] * t5 +
48 420.0 * pi[2] * t4 - 168.0 * pi[2] * t3 + 28.0 * pi[2] * t2 -
49 56.0 * pi[3] * pow(t, 8) + 280.0 * pi[3] * t7 -
50 560.0 * pi[3] * t6 + 560.0 * pi[3] * t5 - 280.0 * pi[3] * t4 +
51 56.0 * pi[3] * t3 - 56.0 * pi[5] * t8 + 168.0 * pi[5] * t7 -
52 168.0 * pi[5] * t6 + 56.0 * pi[5] * pow(t, 5) +
53 28.0 * pi[6] * t8 - 56.0 * pi[6] * t7 + 28.0 * pi[6] * t6 -
54 8.0 * pi[7] * t8 + 8.0 * pi[7] * t7 + 1.0 * pi[8] * t8;
61 const double alpha = 1. / (T);
62 const double t2 = t * t;
63 const double t3 = t2 * t;
64 const double t4 = t3 * t;
65 const double t5 = t4 * t;
66 const double t6 = t5 * t;
67 const double t7 = t6 * t;
70 (560.0 * t7 - 1960.0 * t6 + 2520.0 * t5 - 1400.0 * t4 + 280.0 * t3) *
72 wp.second = (8.0 * pi[8] * t7 - 56.0 * pi[8] * t6 + 168.0 * pi[8] * t5 -
73 280.0 * pi[8] * t4 + 280.0 * pi[8] * t3 - 168.0 * pi[8] * t2 +
74 56.0 * pi[8] * t - 8.0 * pi[8] - 64.0 * pi[1] * t7 +
75 392.0 * pi[1] * t6 - 1008.0 * pi[1] * t5 + 1400.0 * pi[1] * t4 -
76 1120.0 * pi[1] * t3 + 504.0 * pi[1] * t2 - 112.0 * pi[1] * t +
77 8.0 * pi[1] + 224.0 * pi[2] * t7 - 1176.0 * pi[2] * t6 +
78 2520.0 * pi[2] * t5 - 2800.0 * pi[2] * t4 + 1680.0 * pi[2] * t3 -
79 504.0 * pi[2] * t2 + 56.0 * pi[2] * t - 448.0 * pi[3] * t7 +
80 1960.0 * pi[3] * t6 - 3360.0 * pi[3] * t5 + 2800.0 * pi[3] * t4 -
81 1120.0 * pi[3] * t3 + 168.0 * pi[3] * t2 - 448.0 * pi[5] * t7 +
82 1176.0 * pi[5] * t6 - 1008.0 * pi[5] * t5 + 280.0 * pi[5] * t4 +
83 224.0 * pi[6] * t7 - 392.0 * pi[6] * t6 + 168.0 * pi[6] * t5 -
84 64.0 * pi[7] * t7 + 56.0 * pi[7] * t6 + 8.0 * pi[8] * t7) *
92 const double alpha = 1. / (T * T);
93 const double t2 = t * t;
94 const double t3 = t2 * t;
95 const double t4 = t3 * t;
96 const double t5 = t4 * t;
97 const double t6 = t5 * t;
100 ((3920.0 * t6 - 11760.0 * t5 + 12600.0 * t4 - 5600.0 * t3 + 840.0 * t2)) *
102 wp.second = (56.0 * pi[8] * t6 - 336.0 * pi[8] * t5 + 840.0 * pi[8] * t4 -
103 1120.0 * pi[8] * t3 + 840.0 * pi[8] * t2 - 336.0 * pi[8] * t +
104 56.0 * pi[8] - 448.0 * pi[1] * t6 + 2352.0 * pi[1] * t5 -
105 5040.0 * pi[1] * t4 + 5600.0 * pi[1] * t3 - 3360.0 * pi[1] * t2 +
106 1008.0 * pi[1] * t - 112.0 * pi[1] + 1568.0 * pi[2] * t6 -
107 7056.0 * pi[2] * t5 + 12600.0 * pi[2] * t4 -
108 11200.0 * pi[2] * t3 + 5040.0 * pi[2] * t2 - 1008.0 * pi[2] * t +
109 56.0 * pi[2] - 3136.0 * pi[3] * t6 + 11760.0 * pi[3] * t5 -
110 16800.0 * pi[3] * t4 + 11200.0 * pi[3] * t3 -
111 3360.0 * pi[3] * t2 + 336.0 * pi[3] * t - 3136.0 * pi[5] * t6 +
112 7056.0 * pi[5] * t5 - 5040.0 * pi[5] * t4 + 1120.0 * pi[5] * t3 +
113 1568.0 * pi[6] * t6 - 2352.0 * pi[6] * t5 + 840.0 * pi[6] * t4 -
114 448.0 * pi[7] * t6 + 336.0 * pi[7] * t5 + 56.0 * pi[8] * t6) *
122 const double alpha = 1. / (T * T * T);
123 const double t2 = t * t;
124 const double t3 = t2 * t;
125 const double t4 = t3 * t;
126 const double t5 = t4 * t;
129 (23520.0 * t5 - 58800.0 * t4 + 50400.0 * t3 - 16800.0 * t2 + 1680.0 * t) *
133 (336.0 * pi[0] * t5 - 1680.0 * pi[0] * t4 + 3360.0 * pi[0] * t3 -
134 3360.0 * pi[0] * t2 + 1680.0 * pi[0] * t - 336.0 * pi[0] -
135 2688.0 * pi[1] * t5 + 11760.0 * pi[1] * t4 - 20160.0 * pi[1] * t3 +
136 16800.0 * pi[1] * t2 - 6720.0 * pi[1] * t + 1008.0 * pi[1] +
137 9408.0 * pi[2] * t5 - 35280.0 * pi[2] * t4 + 50400.0 * pi[2] * t3 -
138 33600.0 * pi[2] * t2 + 10080.0 * pi[2] * t - 1008.0 * pi[2] -
139 18816.0 * pi[3] * t5 + 58800.0 * pi[3] * t4 - 67200.0 * pi[3] * t3 +
140 33600.0 * pi[3] * t2 - 6720.0 * pi[3] * t + 336.0 * pi[3] -
141 18816.0 * pi[5] * t5 + 35280.0 * pi[5] * t4 - 20160.0 * pi[5] * t3 +
142 3360.0 * pi[5] * t2 + 9408.0 * pi[6] * t5 - 11760.0 * pi[6] * t4 +
143 3360.0 * pi[6] * t3 - 2688.0 * pi[7] * t5 + 1680.0 * pi[7] * t4 +
144 336.0 * pi[8] * t5) *
210 bezier_wp_t::t_point_t wps;
212 const int DIM_VAR = 3;
214 std::vector<Matrix3> Cpi;
215 for (std::size_t i = 0; i < pi.size(); ++i) {
216 Cpi.push_back(
skew(pi[i]));
220 const double T2 = T * T;
221 const double alpha = 1 / (T2);
225 w0.second.head<3>() = (30 * pi[0] - 60 * pi[1] + 30 * pi[2]) * alpha;
226 w0.second.tail<3>() =
228 (1.0 * Cg * T2 * pi[0] - 60.0 * Cpi[0] * pi[1] + 30.0 * Cpi[0] * pi[2]) *
232 w1.first.block<3, 3>(0, 0) = 13.3333333333333 * alpha * Matrix3::Identity();
233 w1.first.block<3, 3>(3, 0) = 13.3333333333333 * Cpi[0] * alpha;
234 w1.second.head<3>() =
235 1.0 * (16.6666666666667 * pi[0] - 20.0 * pi[1] - 10.0 * pi[2]) * alpha;
236 w1.second.tail<3>() = 1.0 *
237 (0.333333333333333 * Cg * T2 * pi[0] +
238 0.666666666666667 * Cg * T2 * pi[1] -
239 30.0 * Cpi[0] * pi[2] + 20.0 * Cpi[1] * pi[2]) *
243 w2.first.block<3, 3>(0, 0) = 6.66666666666667 * alpha * Matrix3::Identity();
244 w2.first.block<3, 3>(3, 0) =
245 1.0 * (-13.3333333333333 * Cpi[0] + 20.0 * Cpi[1]) * alpha;
246 w2.second.head<3>() =
247 1.0 * (8.33333333333333 * pi[0] - 20.0 * pi[2] + 5.0 * pi[4]) * alpha;
248 w2.second.tail<3>() =
250 (0.0833333333333334 * Cg * T2 * pi[0] + 0.5 * Cg * T2 * pi[1] +
251 0.416666666666667 * Cg * T2 * pi[2] + 5.0 * Cpi[0] * pi[4] -
252 20.0 * Cpi[1] * pi[2]) *
256 w3.first.block<3, 3>(0, 0) = -5.71428571428572 * alpha * Matrix3::Identity();
257 w3.first.block<3, 3>(3, 0) = 1.0 *
258 (0.238095238095238 * Cg * T2 - 20.0 * Cpi[1] +
259 14.2857142857143 * Cpi[2]) *
261 w3.second.head<3>() = 1.0 *
262 (3.57142857142857 * pi[0] + 7.14285714285714 * pi[1] -
263 14.2857142857143 * pi[2] + 7.85714285714286 * pi[4] +
264 1.42857142857143 * pi[5]) *
266 w3.second.tail<3>() =
268 (0.0119047619047619 * Cg * T2 * pi[0] +
269 0.214285714285714 * Cg * T2 * pi[1] +
270 0.535714285714286 * Cg * T2 * pi[2] - 5.0 * Cpi[0] * pi[4] +
271 1.42857142857143 * Cpi[0] * pi[5] + 12.8571428571429 * Cpi[1] * pi[4]) *
275 w4.first.block<3, 3>(0, 0) = -14.2857142857143 * alpha * Matrix3::Identity();
276 w4.first.block<3, 3>(3, 0) =
277 1.0 * (0.476190476190476 * Cg * T2 - 14.2857142857143 * Cpi[2]) * alpha;
278 w4.second.head<3>() = 1.0 *
279 (1.19047619047619 * pi[0] + 7.14285714285714 * pi[1] -
280 3.57142857142857 * pi[2] + 5.0 * pi[4] +
281 4.28571428571429 * pi[5] + 0.238095238095238 * pi[6]) *
283 w4.second.tail<3>() =
285 (0.0476190476190471 * Cg * T2 * pi[1] +
286 0.357142857142857 * Cg * T2 * pi[2] +
287 0.119047619047619 * Cg * T2 * pi[4] - 1.42857142857143 * Cpi[0] * pi[5] +
288 0.238095238095238 * Cpi[0] * pi[6] - 12.8571428571429 * Cpi[1] * pi[4] +
289 5.71428571428571 * Cpi[1] * pi[5] + 17.8571428571429 * Cpi[2] * pi[4]) *
293 w5.
first.block<3, 3>(0, 0) = -14.2857142857143 * alpha * Matrix3::Identity();
294 w5.
first.block<3, 3>(3, 0) =
295 1.0 * (0.476190476190476 * Cg * T2 - 14.2857142857143 * Cpi[4]) * alpha;
296 w5.
second.head<3>() = 1.0 *
297 (0.238095238095238 * pi[0] + 4.28571428571429 * pi[1] +
298 5.0 * pi[2] - 3.57142857142857 * pi[4] +
299 7.14285714285714 * pi[5] + 1.19047619047619 * pi[6]) *
303 (+0.11904761904762 * Cg * T2 * pi[2] +
304 0.357142857142857 * Cg * T2 * pi[4] +
305 0.0476190476190476 * Cg * T2 * pi[5] -
306 0.238095238095238 * Cpi[0] * pi[6] - 5.71428571428572 * Cpi[1] * pi[5] +
307 1.42857142857143 * Cpi[1] * pi[6] - 17.8571428571429 * Cpi[2] * pi[4] +
308 12.8571428571429 * Cpi[2] * pi[5]) *
312 w6.
first.block<3, 3>(0, 0) = -5.71428571428571 * alpha * Matrix3::Identity();
313 w6.
first.block<3, 3>(3, 0) = 1.0 *
314 (0.238095238095238 * Cg * T2 +
315 14.2857142857143 * Cpi[4] - 20.0 * Cpi[5]) *
317 w6.
second.head<3>() = 1.0 *
318 (1.42857142857143 * pi[1] + 7.85714285714286 * pi[2] -
319 14.2857142857143 * pi[4] + 7.14285714285715 * pi[5] +
320 3.57142857142857 * pi[6]) *
324 (0.535714285714286 * Cg * T2 * pi[4] +
325 0.214285714285714 * Cg * T2 * pi[5] +
326 0.0119047619047619 * Cg * T2 * pi[6] -
327 1.42857142857143 * Cpi[1] * pi[6] - 12.8571428571429 * Cpi[2] * pi[5] +
328 5.0 * Cpi[2] * pi[6]) *
332 w7.
first.block<3, 3>(0, 0) = 6.66666666666667 * alpha * Matrix3::Identity();
333 w7.
first.block<3, 3>(3, 0) =
334 1.0 * (20.0 * Cpi[5] - 13.3333333333333 * Cpi[6]) * alpha;
336 1.0 * (5.0 * pi[2] - 20.0 * pi[4] + 8.33333333333333 * pi[6]) * alpha;
339 (0.416666666666667 * Cg * T2 * pi[4] + 0.5 * Cg * T2 * pi[5] +
340 0.0833333333333333 * Cg * T2 * pi[6] - 5.0 * Cpi[2] * pi[6] +
341 20.0 * Cpi[4] * pi[5]) *
345 w8.
first.block<3, 3>(0, 0) = 13.3333333333333 * alpha * Matrix3::Identity();
346 w8.
first.block<3, 3>(3, 0) = 1.0 * (13.3333333333333 * Cpi[6]) * alpha;
349 (-9.99999999999999 * pi[4] - 20.0 * pi[5] + 16.6666666666667 * pi[6]) *
351 w8.
second.tail<3>() = 1.0 *
352 (0.666666666666667 * Cg * T2 * pi[5] +
353 0.333333333333333 * Cg * T2 * pi[6] -
354 20.0 * Cpi[4] * pi[5] + 30.0 * Cpi[4] * pi[6]) *
358 w9.
second.head<3>() = (30 * pi[4] - 60 * pi[5] + 30 * pi[6]) * alpha;
361 (1.0 * Cg * T2 * pi[6] - 30.0 * Cpi[4] * pi[6] + 60.0 * Cpi[5] * pi[6]) *