38 #ifndef FCL_CCD_TRIANGLEMOTIONBOUNDVISITOR_INL_H
39 #define FCL_CCD_TRIANGLEMOTIONBOUNDVISITOR_INL_H
61 : a(a_), b(b_), c(c_), n(n_)
67 template <
typename S,
typename MotionT>
119 template <
typename S>
134 S proj_max = ((tf.linear() * visitor.
a + tf.translation() - p).cross(axis)).squaredNorm();
136 tmp = ((tf.linear() * visitor.
b + tf.translation() - p).cross(axis)).squaredNorm();
137 if(tmp > proj_max) proj_max = tmp;
138 tmp = ((tf.linear() * visitor.
c + tf.translation() - p).cross(axis)).squaredNorm();
139 if(tmp > proj_max) proj_max = tmp;
141 proj_max = std::sqrt(proj_max);
143 S v_dot_n = axis.dot(visitor.
n) * linear_vel;
144 S w_cross_n = (axis.cross(visitor.
n)).norm() * angular_vel;
145 S mu = v_dot_n + w_cross_n * proj_max;
156 template <
typename S>
171 S proj_max = ((tf.linear() * (visitor.
a - reference_p)).cross(angular_axis)).squaredNorm();
173 tmp = ((tf.linear() * (visitor.
b - reference_p)).cross(angular_axis)).squaredNorm();
174 if(tmp > proj_max) proj_max = tmp;
175 tmp = ((tf.linear() * (visitor.
c - reference_p)).cross(angular_axis)).squaredNorm();
176 if(tmp > proj_max) proj_max = tmp;
178 proj_max = std::sqrt(proj_max);
180 S v_dot_n = linear_vel.dot(visitor.
n);
181 S w_cross_n = (angular_axis.cross(visitor.
n)).norm() * angular_vel;
182 S mu = v_dot_n + w_cross_n * proj_max;
189 template <
typename S>
199 S R_bound = std::abs(visitor.
a.dot(visitor.
n)) + visitor.
a.norm() + (visitor.
a.cross(visitor.
n)).norm();
200 S R_bound_tmp = std::abs(visitor.
b.dot(visitor.
n)) + visitor.
b.norm() + (visitor.
b.cross(visitor.
n)).norm();
201 if(R_bound_tmp > R_bound) R_bound = R_bound_tmp;
202 R_bound_tmp = std::abs(visitor.
c.dot(visitor.
n)) + visitor.
c.norm() + (visitor.
c.cross(visitor.
n)).norm();
203 if(R_bound_tmp > R_bound) R_bound = R_bound_tmp;
206 S ratio =
std::min(1 - tf_t, dWdW_max);
208 R_bound *= 2 * ratio;
212 return R_bound + T_bound;
218 template <
typename S>