90 bezier_wp_t::t_point_t wps;
92 const int DIM_VAR = 3;
94 std::vector<Matrix3> Cpi;
95 for (std::size_t i = 0; i < pi.size(); ++i) {
96 Cpi.push_back(
skew(pi[i]));
100 const double T2 = T * T;
101 const double alpha = 1 / (T2);
105 w0.second.head<3>() = (30 * pi[0] - 60 * pi[1] + 30 * pi[2]) * alpha;
106 w0.second.tail<3>() =
108 (1.0 * Cg * T2 * pi[0] - 60.0 * Cpi[0] * pi[1] + 30.0 * Cpi[0] * pi[2]) *
112 w1.first.block<3, 3>(0, 0) = 13.3333333333333 * alpha * Matrix3::Identity();
113 w1.first.block<3, 3>(3, 0) = 13.3333333333333 * Cpi[0] * alpha;
114 w1.second.head<3>() =
115 1.0 * (16.6666666666667 * pi[0] - 20.0 * pi[1] - 10.0 * pi[2]) * alpha;
116 w1.second.tail<3>() = 1.0 *
117 (0.333333333333333 * Cg * T2 * pi[0] +
118 0.666666666666667 * Cg * T2 * pi[1] -
119 30.0 * Cpi[0] * pi[2] + 20.0 * Cpi[1] * pi[2]) *
123 w2.first.block<3, 3>(0, 0) = 6.66666666666667 * alpha * Matrix3::Identity();
124 w2.first.block<3, 3>(3, 0) =
125 1.0 * (-13.3333333333333 * Cpi[0] + 20.0 * Cpi[1]) * alpha;
126 w2.second.head<3>() =
127 1.0 * (8.33333333333333 * pi[0] - 20.0 * pi[2] + 5.0 * pi[4]) * alpha;
128 w2.second.tail<3>() =
130 (0.0833333333333334 * Cg * T2 * pi[0] + 0.5 * Cg * T2 * pi[1] +
131 0.416666666666667 * Cg * T2 * pi[2] + 5.0 * Cpi[0] * pi[4] -
132 20.0 * Cpi[1] * pi[2]) *
136 w3.first.block<3, 3>(0, 0) = -5.71428571428572 * alpha * Matrix3::Identity();
137 w3.first.block<3, 3>(3, 0) = 1.0 *
138 (0.238095238095238 * Cg * T2 - 20.0 * Cpi[1] +
139 14.2857142857143 * Cpi[2]) *
141 w3.second.head<3>() = 1.0 *
142 (3.57142857142857 * pi[0] + 7.14285714285714 * pi[1] -
143 14.2857142857143 * pi[2] + 7.85714285714286 * pi[4] +
144 1.42857142857143 * pi[5]) *
146 w3.second.tail<3>() =
148 (0.0119047619047619 * Cg * T2 * pi[0] +
149 0.214285714285714 * Cg * T2 * pi[1] +
150 0.535714285714286 * Cg * T2 * pi[2] - 5.0 * Cpi[0] * pi[4] +
151 1.42857142857143 * Cpi[0] * pi[5] + 12.8571428571429 * Cpi[1] * pi[4]) *
155 w4.first.block<3, 3>(0, 0) = -14.2857142857143 * alpha * Matrix3::Identity();
156 w4.first.block<3, 3>(3, 0) =
157 1.0 * (0.476190476190476 * Cg * T2 - 14.2857142857143 * Cpi[2]) * alpha;
158 w4.second.head<3>() = 1.0 *
159 (1.19047619047619 * pi[0] + 7.14285714285714 * pi[1] -
160 3.57142857142857 * pi[2] + 5.0 * pi[4] +
161 4.28571428571429 * pi[5] + 0.238095238095238 * pi[6]) *
163 w4.second.tail<3>() =
165 (0.0476190476190471 * Cg * T2 * pi[1] +
166 0.357142857142857 * Cg * T2 * pi[2] +
167 0.119047619047619 * Cg * T2 * pi[4] - 1.42857142857143 * Cpi[0] * pi[5] +
168 0.238095238095238 * Cpi[0] * pi[6] - 12.8571428571429 * Cpi[1] * pi[4] +
169 5.71428571428571 * Cpi[1] * pi[5] + 17.8571428571429 * Cpi[2] * pi[4]) *
173 w5.
first.block<3, 3>(0, 0) = -14.2857142857143 * alpha * Matrix3::Identity();
174 w5.
first.block<3, 3>(3, 0) =
175 1.0 * (0.476190476190476 * Cg * T2 - 14.2857142857143 * Cpi[4]) * alpha;
176 w5.
second.head<3>() = 1.0 *
177 (0.238095238095238 * pi[0] + 4.28571428571429 * pi[1] +
178 5.0 * pi[2] - 3.57142857142857 * pi[4] +
179 7.14285714285714 * pi[5] + 1.19047619047619 * pi[6]) *
183 (+0.11904761904762 * Cg * T2 * pi[2] +
184 0.357142857142857 * Cg * T2 * pi[4] +
185 0.0476190476190476 * Cg * T2 * pi[5] -
186 0.238095238095238 * Cpi[0] * pi[6] - 5.71428571428572 * Cpi[1] * pi[5] +
187 1.42857142857143 * Cpi[1] * pi[6] - 17.8571428571429 * Cpi[2] * pi[4] +
188 12.8571428571429 * Cpi[2] * pi[5]) *
192 w6.
first.block<3, 3>(0, 0) = -5.71428571428571 * alpha * Matrix3::Identity();
193 w6.
first.block<3, 3>(3, 0) = 1.0 *
194 (0.238095238095238 * Cg * T2 +
195 14.2857142857143 * Cpi[4] - 20.0 * Cpi[5]) *
197 w6.
second.head<3>() = 1.0 *
198 (1.42857142857143 * pi[1] + 7.85714285714286 * pi[2] -
199 14.2857142857143 * pi[4] + 7.14285714285715 * pi[5] +
200 3.57142857142857 * pi[6]) *
204 (0.535714285714286 * Cg * T2 * pi[4] +
205 0.214285714285714 * Cg * T2 * pi[5] +
206 0.0119047619047619 * Cg * T2 * pi[6] -
207 1.42857142857143 * Cpi[1] * pi[6] - 12.8571428571429 * Cpi[2] * pi[5] +
208 5.0 * Cpi[2] * pi[6]) *
212 w7.
first.block<3, 3>(0, 0) = 6.66666666666667 * alpha * Matrix3::Identity();
213 w7.
first.block<3, 3>(3, 0) =
214 1.0 * (20.0 * Cpi[5] - 13.3333333333333 * Cpi[6]) * alpha;
216 1.0 * (5.0 * pi[2] - 20.0 * pi[4] + 8.33333333333333 * pi[6]) * alpha;
219 (0.416666666666667 * Cg * T2 * pi[4] + 0.5 * Cg * T2 * pi[5] +
220 0.0833333333333333 * Cg * T2 * pi[6] - 5.0 * Cpi[2] * pi[6] +
221 20.0 * Cpi[4] * pi[5]) *
225 w8.
first.block<3, 3>(0, 0) = 13.3333333333333 * alpha * Matrix3::Identity();
226 w8.
first.block<3, 3>(3, 0) = 1.0 * (13.3333333333333 * Cpi[6]) * alpha;
229 (-9.99999999999999 * pi[4] - 20.0 * pi[5] + 16.6666666666667 * pi[6]) *
231 w8.
second.tail<3>() = 1.0 *
232 (0.666666666666667 * Cg * T2 * pi[5] +
233 0.333333333333333 * Cg * T2 * pi[6] -
234 20.0 * Cpi[4] * pi[5] + 30.0 * Cpi[4] * pi[6]) *
238 w9.
second.head<3>() = (30 * pi[4] - 60 * pi[5] + 30 * pi[6]) * alpha;
241 (1.0 * Cg * T2 * pi[6] - 30.0 * Cpi[4] * pi[6] + 60.0 * Cpi[5] * pi[6]) *