82 bezier_wp_t::t_point_t wps;
84 const int DIM_VAR = 3;
86 std::vector<Matrix3> Cpi;
87 for (std::size_t i = 0; i < pi.size(); ++i) {
88 Cpi.push_back(
skew(pi[i]));
92 const double T2 = T * T;
93 const double alpha = 1 / (T2);
97 w0.second.head<3>() = (20 * pi[0] - 40 * pi[1] + 20 * pi[2]) * alpha;
100 (1.0 * Cg * T2 * pi[0] - 40.0 * Cpi[0] * pi[1] + 20.0 * Cpi[0] * pi[2]) *
104 w1.first.block<3, 3>(0, 0) = 8.57142857142857 * alpha * Matrix3::Identity();
105 w1.first.block<3, 3>(3, 0) = 8.57142857142857 * Cpi[0] * alpha;
106 w1.second.head<3>() = 1.0 *
107 (11.4285714285714 * pi[0] - 14.2857142857143 * pi[1] -
108 5.71428571428572 * pi[2]) *
110 w1.second.tail<3>() =
112 (0.285714285714286 * Cg * T2 * pi[0] +
113 0.714285714285714 * Cg * T2 * pi[1] - 20.0 * Cpi[0] * pi[2] +
114 14.2857142857143 * Cpi[1] * pi[2]) *
118 w2.first.block<3, 3>(0, 0) = 5.71428571428571 * alpha * Matrix3::Identity();
119 w2.first.block<3, 3>(3, 0) =
120 1.0 * (-8.57142857142857 * Cpi[0] + 14.2857142857143 * Cpi[1]) * alpha;
121 w2.second.head<3>() = 1.0 *
122 (5.71428571428571 * pi[0] - 14.2857142857143 * pi[2] +
123 2.85714285714286 * pi[4]) *
125 w2.second.tail<3>() =
127 (0.0476190476190479 * Cg * T2 * pi[0] +
128 0.476190476190476 * Cg * T2 * pi[1] +
129 0.476190476190476 * Cg * T2 * pi[2] + 2.85714285714286 * Cpi[0] * pi[4] -
130 14.2857142857143 * Cpi[1] * pi[2]) *
134 w3.first.block<3, 3>(0, 0) = -2.85714285714286 * alpha * Matrix3::Identity();
135 w3.first.block<3, 3>(3, 0) =
137 (0.285714285714286 * Cg * T2 - 14.2857142857143 * Cpi[1] +
138 11.4285714285714 * Cpi[2]) *
140 w3.second.head<3>() = 1.0 *
141 (2.28571428571429 * pi[0] + 5.71428571428571 * pi[1] -
142 11.4285714285714 * pi[2] + 5.71428571428571 * pi[4] +
143 0.571428571428571 * pi[5]) *
145 w3.second.tail<3>() =
147 (0.142857142857143 * Cg * T2 * pi[1] +
148 0.571428571428571 * Cg * T2 * pi[2] - 2.85714285714286 * Cpi[0] * pi[4] +
149 0.571428571428571 * Cpi[0] * pi[5] + 8.57142857142857 * Cpi[1] * pi[4]) *
153 w4.first.block<3, 3>(0, 0) = -11.4285714285714 * alpha * Matrix3::Identity();
154 w4.first.block<3, 3>(3, 0) =
155 1.0 * (0.571428571428571 * Cg * T2 - 11.4285714285714 * Cpi[2]) * alpha;
156 w4.second.head<3>() = 1.0 *
157 (0.571428571428571 * pi[0] + 5.71428571428571 * pi[1] -
158 2.85714285714286 * pi[2] + 5.71428571428571 * pi[4] +
159 2.28571428571429 * pi[5]) *
161 w4.second.tail<3>() =
163 (0.285714285714286 * Cg * T2 * pi[2] +
164 0.142857142857143 * Cg * T2 * pi[4] -
165 0.571428571428572 * Cpi[0] * pi[5] - 8.57142857142857 * Cpi[1] * pi[4] +
166 2.85714285714286 * Cpi[1] * pi[5] + 14.2857142857143 * Cpi[2] * pi[4]) *
170 w5.
first.block<3, 3>(0, 0) = -14.2857142857143 * alpha * Matrix3::Identity();
171 w5.
first.block<3, 3>(3, 0) =
172 1.0 * (0.476190476190476 * Cg * T2 - 14.2857142857143 * Cpi[4]) * alpha;
173 w5.
second.head<3>() = 1.0 *
174 (2.85714285714286 * pi[1] + 5.71428571428571 * pi[2] +
175 5.71428571428571 * pi[5]) *
179 (0.476190476190476 * Cg * T2 * pi[4] +
180 0.0476190476190476 * Cg * T2 * pi[5] -
181 2.85714285714286 * Cpi[1] * pi[5] - 14.2857142857143 * Cpi[2] * pi[4] +
182 8.57142857142857 * Cpi[2] * pi[5]) *
186 w6.
first.block<3, 3>(0, 0) = -5.71428571428572 * alpha * Matrix3::Identity();
187 w6.
first.block<3, 3>(3, 0) =
188 1.0 * (14.2857142857143 * Cpi[4] - 20.0 * Cpi[5]) * alpha;
189 w6.
second.head<3>() = 1.0 *
190 (8.57142857142857 * pi[2] - 14.2857142857143 * pi[4] +
191 11.4285714285714 * pi[5]) *
193 w6.
second.tail<3>() = 1.0 *
194 (0.714285714285714 * Cg * T2 * pi[4] +
195 0.285714285714286 * Cg * T2 * pi[5] -
196 8.57142857142858 * Cpi[2] * pi[5]) *
200 w7.
first.block<3, 3>(0, 0) = 20 * alpha * Matrix3::Identity();
201 w7.
first.block<3, 3>(3, 0) = 1.0 * (20.0 * Cpi[5]) * alpha;
202 w7.
second.head<3>() = (-40 * pi[4] + 20 * pi[5]) * alpha;
204 1.0 * (1.0 * Cg * T2 * pi[5] + 40.0 * Cpi[4] * pi[5]) * alpha;