FCL  0.6.0
Flexible Collision Library
tbv_motion_bound_visitor-inl.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011-2014, Willow Garage, Inc.
5  * Copyright (c) 2014-2016, Open Source Robotics Foundation
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Open Source Robotics Foundation nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  */
35 
38 #ifndef FCL_CCD_TBVMOTIONBOUNDVISITOR_INL_H
39 #define FCL_CCD_TBVMOTIONBOUNDVISITOR_INL_H
40 
41 #include "fcl/math/motion/tbv_motion_bound_visitor.h"
42 
43 namespace fcl
44 {
45 
46 //==============================================================================
47 template<typename BV>
48 TBVMotionBoundVisitor<BV>::TBVMotionBoundVisitor(
49  const BV& bv_, const Vector3<typename BV::S>& n_)
50  : bv(bv_), n(n_)
51 {
52  // Do nothing
53 }
54 
55 //==============================================================================
56 template <typename S, typename BV, typename MotionT>
58 {
59  static S run(
60  const TBVMotionBoundVisitor<BV>& /*visitor*/,
61  const MotionT& /*motion*/)
62  {
63  return 0;
64  }
65 };
66 
67 //==============================================================================
68 template<typename BV>
70  const MotionBase<S>& motion) const
71 {
72  return 0;
73 }
74 
75 //==============================================================================
76 template<typename BV>
78  const SplineMotion<S>& motion) const
79 {
80  using S = typename BV::S;
81 
83  S, BV, SplineMotion<S>>::run(*this, motion);
84 }
85 
86 //==============================================================================
87 template<typename BV>
89  const ScrewMotion<S>& motion) const
90 {
91  using S = typename BV::S;
92 
94  S, BV, ScrewMotion<S>>::run(*this, motion);
95 }
96 
97 //==============================================================================
98 template<typename BV>
100  const InterpMotion<S>& motion) const
101 {
102  using S = typename BV::S;
103 
105  S, BV, InterpMotion<S>>::run(*this, motion);
106 }
107 
108 //==============================================================================
109 template<typename BV>
110 typename BV::S TBVMotionBoundVisitor<BV>::visit(
111  const TranslationMotion<S>& motion) const
112 {
113  using S = typename BV::S;
114 
116  S, BV, TranslationMotion<S>>::run(*this, motion);
117 }
118 
119 //==============================================================================
120 template <typename S>
122 {
123  static S run(
124  const TBVMotionBoundVisitor<RSS<S>>& visitor,
125  const SplineMotion<S>& motion)
126  {
127  S T_bound = motion.computeTBound(visitor.n);
128  S tf_t = motion.getCurrentTime();
129 
130  Vector3<S> c1 = visitor.bv.To;
131  Vector3<S> c2 = visitor.bv.To + visitor.bv.axis.col(0) * visitor.bv.l[0];
132  Vector3<S> c3 = visitor.bv.To + visitor.bv.axis.col(1) * visitor.bv.l[1];
133  Vector3<S> c4 = visitor.bv.To + visitor.bv.axis.col(0) * visitor.bv.l[0] + visitor.bv.axis.col(1) * visitor.bv.l[1];
134 
135  S tmp;
136  // max_i |c_i * n|
137  S cn_max = std::abs(c1.dot(visitor.n));
138  tmp = std::abs(c2.dot(visitor.n));
139  if(tmp > cn_max) cn_max = tmp;
140  tmp = std::abs(c3.dot(visitor.n));
141  if(tmp > cn_max) cn_max = tmp;
142  tmp = std::abs(c4.dot(visitor.n));
143  if(tmp > cn_max) cn_max = tmp;
144 
145  // max_i ||c_i||
146  S cmax = c1.squaredNorm();
147  tmp = c2.squaredNorm();
148  if(tmp > cmax) cmax = tmp;
149  tmp = c3.squaredNorm();
150  if(tmp > cmax) cmax = tmp;
151  tmp = c4.squaredNorm();
152  if(tmp > cmax) cmax = tmp;
153  cmax = sqrt(cmax);
154 
155  // max_i ||c_i x n||
156  S cxn_max = (c1.cross(visitor.n)).squaredNorm();
157  tmp = (c2.cross(visitor.n)).squaredNorm();
158  if(tmp > cxn_max) cxn_max = tmp;
159  tmp = (c3.cross(visitor.n)).squaredNorm();
160  if(tmp > cxn_max) cxn_max = tmp;
161  tmp = (c4.cross(visitor.n)).squaredNorm();
162  if(tmp > cxn_max) cxn_max = tmp;
163  cxn_max = sqrt(cxn_max);
164 
165  S dWdW_max = motion.computeDWMax();
166  S ratio = std::min(1 - tf_t, dWdW_max);
167 
168  S R_bound = 2 * (cn_max + cmax + cxn_max + 3 * visitor.bv.r) * ratio;
169 
170 
171  // std::cout << R_bound << " " << T_bound << std::endl;
172 
173  return R_bound + T_bound;
174  }
175 };
176 
177 //==============================================================================
182 template <typename S>
184 {
185  static S run(
186  const TBVMotionBoundVisitor<RSS<S>>& visitor,
187  const ScrewMotion<S>& motion)
188  {
189  Transform3<S> tf;
190  motion.getCurrentTransform(tf);
191 
192  const Vector3<S>& axis = motion.getAxis();
193  S linear_vel = motion.getLinearVelocity();
194  S angular_vel = motion.getAngularVelocity();
195  const Vector3<S>& p = motion.getAxisOrigin();
196 
197  S c_proj_max = ((tf.linear() * visitor.bv.To).cross(axis)).squaredNorm();
198  S tmp;
199  tmp = ((tf.linear() * (visitor.bv.To + visitor.bv.axis.col(0) * visitor.bv.l[0])).cross(axis)).squaredNorm();
200  if(tmp > c_proj_max) c_proj_max = tmp;
201  tmp = ((tf.linear() * (visitor.bv.To + visitor.bv.axis.col(1) * visitor.bv.l[1])).cross(axis)).squaredNorm();
202  if(tmp > c_proj_max) c_proj_max = tmp;
203  tmp = ((tf.linear() * (visitor.bv.To + visitor.bv.axis.col(0) * visitor.bv.l[0] + visitor.bv.axis.col(1) * visitor.bv.l[1])).cross(axis)).squaredNorm();
204  if(tmp > c_proj_max) c_proj_max = tmp;
205 
206  c_proj_max = sqrt(c_proj_max);
207 
208  S v_dot_n = axis.dot(visitor.n) * linear_vel;
209  S w_cross_n = (axis.cross(visitor.n)).norm() * angular_vel;
210  S origin_proj = ((tf.translation() - p).cross(axis)).norm();
211 
212  S mu = v_dot_n + w_cross_n * (c_proj_max + visitor.bv.r + origin_proj);
213 
214  return mu;
215  }
216 };
217 
218 //==============================================================================
223 template <typename S>
225 {
226  static S run(
227  const TBVMotionBoundVisitor<RSS<S>>& visitor,
228  const InterpMotion<S>& motion)
229  {
230  Transform3<S> tf;
231  motion.getCurrentTransform(tf);
232 
233  const Vector3<S>& reference_p = motion.getReferencePoint();
234  const Vector3<S>& angular_axis = motion.getAngularAxis();
235  S angular_vel = motion.getAngularVelocity();
236  const Vector3<S>& linear_vel = motion.getLinearVelocity();
237 
238  S c_proj_max = ((tf.linear() * (visitor.bv.To - reference_p)).cross(angular_axis)).squaredNorm();
239  S tmp;
240  tmp = ((tf.linear() * (visitor.bv.To + visitor.bv.axis.col(0) * visitor.bv.l[0] - reference_p)).cross(angular_axis)).squaredNorm();
241  if(tmp > c_proj_max) c_proj_max = tmp;
242  tmp = ((tf.linear() * (visitor.bv.To + visitor.bv.axis.col(1) * visitor.bv.l[1] - reference_p)).cross(angular_axis)).squaredNorm();
243  if(tmp > c_proj_max) c_proj_max = tmp;
244  tmp = ((tf.linear() * (visitor.bv.To + visitor.bv.axis.col(0) * visitor.bv.l[0] + visitor.bv.axis.col(1) * visitor.bv.l[1] - reference_p)).cross(angular_axis)).squaredNorm();
245  if(tmp > c_proj_max) c_proj_max = tmp;
246 
247  c_proj_max = std::sqrt(c_proj_max);
248 
249  S v_dot_n = linear_vel.dot(visitor.n);
250  S w_cross_n = (angular_axis.cross(visitor.n)).norm() * angular_vel;
251  S mu = v_dot_n + w_cross_n * (visitor.bv.r + c_proj_max);
252 
253  return mu;
254  }
255 };
256 
257 //==============================================================================
259 template <typename S>
261 {
262  static S run(
263  const TBVMotionBoundVisitor<RSS<S>>& visitor,
264  const TranslationMotion<S>& motion)
265  {
266  return motion.getVelocity().dot(visitor.n);
267  }
268 };
269 
270 } // namespace fcl
271 
272 #endif
Definition: tbv_motion_bound_visitor-inl.h:57
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
Definition: bv_motion_bound_visitor.h:51
void getCurrentTransform(Transform3< S > &tf_) const
Get the rotation and translation in current step.
Definition: interp_motion-inl.h:156
Definition: bv_motion_bound_visitor.h:57
void getCurrentTransform(Transform3< S > &tf_) const
Get the rotation and translation in current step.
Definition: screw_motion-inl.h:125
Definition: bv_motion_bound_visitor.h:48
A class for rectangle sphere-swept bounding volume.
Definition: RSS.h:49
Linear interpolation motion Each Motion is assumed to have constant linear velocity and angular veloc...
Definition: bv_motion_bound_visitor.h:54
Definition: tbv_motion_bound_visitor.h:65
Definition: bv_motion_bound_visitor.h:45