38 #ifndef FCL_CCD_SPLINEMOTION_INL_H
39 #define FCL_CCD_SPLINEMOTION_INL_H
81 TB = (
Td[0] -
Td[1] * 2 +
Td[2]) * 3;
85 RB = (
Rd[0] -
Rd[1] * 2 +
Rd[2]) * 3;
108 template <
typename S>
120 template <
typename S>
125 Vector3<S> cur_T = Td[0] * getWeight0(dt) + Td[1] * getWeight1(dt) + Td[2] * getWeight2(dt) + Td[3] * getWeight3(dt);
126 Vector3<S> cur_w = Rd[0] * getWeight0(dt) + Rd[1] * getWeight1(dt) + Rd[2] * getWeight2(dt) + Rd[3] * getWeight3(dt);
127 S cur_angle = cur_w.norm();
128 if (cur_angle > 0.0) {
132 tf.linear() =
AngleAxis<S>(cur_angle, cur_w).toRotationMatrix();
133 tf.translation() = cur_T;
141 template <
typename S>
144 return mb_visitor.
visit(*
this);
148 template <
typename S>
151 return mb_visitor.
visit(*
this);
155 template <
typename S>
162 template <
typename S>
167 c[0] = (Td[0] + Td[1] * 4 + Td[2] + Td[3]) * (1/6.0);
168 c[1] = (-Td[0] + Td[2]) * (1/2.0);
169 c[2] = (Td[0] - Td[1] * 2 + Td[2]) * (1/2.0);
170 c[3] = (-Td[0] + Td[1] * 3 - Td[2] * 3 + Td[3]) * (1/6.0);
172 for(std::size_t i = 0; i < 3; ++i)
174 for(std::size_t j = 0; j < 4; ++j)
176 tv[i].coeff(j) = c[j][i];
184 Vector3<S> Rt0 = (Rd[0] + Rd[1] * 23 + Rd[2] * 23 + Rd[3]) * (1 / 48.0);
185 S Rt0_len = Rt0.norm();
186 S inv_Rt0_len = 1.0 / Rt0_len;
187 S inv_Rt0_len_3 = inv_Rt0_len * inv_Rt0_len * inv_Rt0_len;
188 S inv_Rt0_len_5 = inv_Rt0_len_3 * inv_Rt0_len * inv_Rt0_len;
190 S costheta0 = cos(theta0);
191 S sintheta0 = sin(theta0);
197 Matrix3<S> Mt0 = I + hatWt0 * sintheta0 + hatWt0_sqr * (1 - costheta0);
201 Vector3<S> dRt0 = (-Rd[0] - Rd[1] * 5 + Rd[2] * 5 + Rd[3]) * (1 / 8.0);
202 S Rt0_dot_dRt0 = Rt0.dot(dRt0);
203 S dtheta0 = Rt0_dot_dRt0 * inv_Rt0_len;
204 Vector3<S> dWt0 = dRt0 * inv_Rt0_len - Rt0 * (Rt0_dot_dRt0 * inv_Rt0_len_3);
207 Matrix3<S> dMt0 = hatdWt0 * sintheta0 + hatWt0 * (costheta0 * dtheta0) + hatWt0_sqr * (sintheta0 * dtheta0) + (hatWt0 * hatdWt0 + hatdWt0 * hatWt0) * (1 - costheta0);
210 Vector3<S> ddRt0 = (Rd[0] - Rd[1] - Rd[2] + Rd[3]) * 0.5;
211 S Rt0_dot_ddRt0 = Rt0.dot(ddRt0);
212 S dRt0_dot_dRt0 = dRt0.squaredNorm();
213 S ddtheta0 = (Rt0_dot_ddRt0 + dRt0_dot_dRt0) * inv_Rt0_len - Rt0_dot_dRt0 * Rt0_dot_dRt0 * inv_Rt0_len_3;
214 Vector3<S> ddWt0 = ddRt0 * inv_Rt0_len - (dRt0 * (2 * Rt0_dot_dRt0) + Rt0 * (Rt0_dot_ddRt0 + dRt0_dot_dRt0)) * inv_Rt0_len_3 + (Rt0 * (3 * Rt0_dot_dRt0 * Rt0_dot_dRt0)) * inv_Rt0_len_5;
216 hat(hatddWt0, ddWt0);
218 hatddWt0 * sintheta0 +
219 hatWt0 * (costheta0 * dtheta0 - sintheta0 * dtheta0 * dtheta0 + costheta0 * ddtheta0) +
220 hatdWt0 * (costheta0 * dtheta0) +
221 (hatWt0 * hatdWt0 + hatdWt0 * hatWt0) * (sintheta0 * dtheta0 * 2) +
222 hatdWt0 * hatdWt0 * (2 * (1 - costheta0)) +
223 hatWt0 * hatWt0 * (costheta0 * dtheta0 * dtheta0 + sintheta0 * ddtheta0) +
224 (hatWt0 * hatddWt0 + hatddWt0 + hatWt0) * (1 - costheta0);
227 for(std::size_t i = 0; i < 3; ++i)
229 for(std::size_t j = 0; j < 3; ++j)
231 tm(i, j).coeff(0) = Mt0(i, j) - dMt0(i, j) * 0.5 + ddMt0(i, j) * 0.25 * 0.5;
232 tm(i, j).coeff(1) = dMt0(i, j) - ddMt0(i, j) * 0.5;
233 tm(i, j).coeff(2) = ddMt0(i, j) * 0.5;
234 tm(i, j).coeff(3) = 0;
236 tm(i, j).remainder() =
Interval<S>(-1/48.0, 1/48.0);
242 template <
typename S>
249 template <
typename S>
256 std::vector<S> T_potential;
257 T_potential.push_back(tf_t);
258 T_potential.push_back(1);
259 if(Tb * Tb - 3 * Ta * Tc >= 0)
265 S tmp = -Tc / (2 * Tb);
266 if(tmp < 1 && tmp > tf_t)
267 T_potential.push_back(tmp);
272 S tmp_delta = sqrt(Tb * Tb - 3 * Ta * Tc);
273 S tmp1 = (-Tb + tmp_delta) / (3 * Ta);
274 S tmp2 = (-Tb - tmp_delta) / (3 * Ta);
275 if(tmp1 < 1 && tmp1 > tf_t)
276 T_potential.push_back(tmp1);
277 if(tmp2 < 1 && tmp2 > tf_t)
278 T_potential.push_back(tmp2);
282 S T_bound = Ta * T_potential[0] * T_potential[0] * T_potential[0] + Tb * T_potential[0] * T_potential[0] + Tc * T_potential[0];
283 for(
unsigned int i = 1; i < T_potential.size(); ++i)
285 S T_bound_tmp = Ta * T_potential[i] * T_potential[i] * T_potential[i] + Tb * T_potential[i] * T_potential[i] + Tc * T_potential[i];
286 if(T_bound_tmp > T_bound) T_bound = T_bound_tmp;
290 S cur_delta = Ta * tf_t * tf_t * tf_t + Tb * tf_t * tf_t + Tc * tf_t;
292 T_bound -= cur_delta;
299 template <
typename S>
303 int a00[5] = {1,-4,6,-4,1};
304 int a01[5] = {-3,10,-11,4,0};
305 int a02[5] = {3,-8,6,0,-1};
306 int a03[5] = {-1,2,-1,0,0};
307 int a11[5] = {9,-24,16,0,0};
308 int a12[5] = {-9,18,-5,-4,0};
309 int a13[5] = {3,-4,0,0,0};
310 int a22[5] = {9,-12,-2,4,1};
311 int a23[5] = {-3,2,1,0,0};
312 int a33[5] = {1,0,0,0,0};
316 for(
int i = 0; i < 5; ++i)
318 a[i] = Rd0Rd0 * a00[i] + Rd0Rd1 * a01[i] + Rd0Rd2 * a02[i] + Rd0Rd3 * a03[i]
319 + Rd0Rd1 * a01[i] + Rd1Rd1 * a11[i] + Rd1Rd2 * a12[i] + Rd1Rd3 * a13[i]
320 + Rd0Rd2 * a02[i] + Rd1Rd2 * a12[i] + Rd2Rd2 * a22[i] + Rd2Rd3 * a23[i]
321 + Rd0Rd3 * a03[i] + Rd1Rd3 * a13[i] + Rd2Rd3 * a23[i] + Rd3Rd3 * a33[i];
326 int da00[4] = {4,-12,12,-4};
327 int da01[4] = {-12,30,-22,4};
328 int da02[4] = {12,-24,12,0};
329 int da03[4] = {-4,6,-2,0};
330 int da11[4] = {36,-72,32,0};
331 int da12[4] = {-36,54,-10,-4};
332 int da13[4] = {12,-12,0,0};
333 int da22[4] = {36,-36,-4,4};
334 int da23[4] = {-12,6,2,0};
335 int da33[4] = {4,0,0,0};
338 for(
int i = 0; i < 4; ++i)
340 da[i] = Rd0Rd0 * da00[i] + Rd0Rd1 * da01[i] + Rd0Rd2 * da02[i] + Rd0Rd3 * da03[i]
341 + Rd0Rd1 * da01[i] + Rd1Rd1 * da11[i] + Rd1Rd2 * da12[i] + Rd1Rd3 * da13[i]
342 + Rd0Rd2 * da02[i] + Rd1Rd2 * da12[i] + Rd2Rd2 * da22[i] + Rd2Rd3 * da23[i]
343 + Rd0Rd3 * da03[i] + Rd1Rd3 * da13[i] + Rd2Rd3 * da23[i] + Rd3Rd3 * da33[i];
351 S dWdW_max = a[0] * tf_t * tf_t * tf_t + a[1] * tf_t * tf_t * tf_t + a[2] * tf_t * tf_t + a[3] * tf_t + a[4];
352 S dWdW_1 = a[0] + a[1] + a[2] + a[3] + a[4];
353 if(dWdW_max < dWdW_1) dWdW_max = dWdW_1;
354 for(
int i = 0; i < root_num; ++i)
358 if(v >= tf_t && v <= 1)
360 S value = a[0] * v * v * v * v + a[1] * v * v * v + a[2] * v * v + a[3] * v + a[4];
361 if(value > dWdW_max) dWdW_max = value;
365 return sqrt(dWdW_max);
369 template <
typename S>
376 template <
typename S>
379 return (1 - 3 * t + 3 * t * t - t * t * t) / 6.0;
383 template <
typename S>
386 return (4 - 6 * t * t + 3 * t * t * t) / 6.0;
390 template <
typename S>
393 return (1 + 3 * t + 3 * t * t - 3 * t * t * t) / 6.0;
397 template <
typename S>
400 return t * t * t / 6.0;