FCL  0.6.0
Flexible Collision Library
triangle_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_TRIANGLEMOTIONBOUNDVISITOR_INL_H
39 #define FCL_CCD_TRIANGLEMOTIONBOUNDVISITOR_INL_H
40 
41 #include "fcl/math/motion/triangle_motion_bound_visitor.h"
42 
43 #include "fcl/math/motion/spline_motion.h"
44 #include "fcl/math/motion/screw_motion.h"
45 #include "fcl/math/motion/interp_motion.h"
46 #include "fcl/math/motion/translation_motion.h"
47 #include "fcl/math/motion/triangle_motion_bound_visitor.h"
48 
49 namespace fcl
50 {
51 
52 //==============================================================================
53 extern template
54 class TriangleMotionBoundVisitor<double>;
55 
56 //==============================================================================
57 template<typename S>
58 TriangleMotionBoundVisitor<S>::TriangleMotionBoundVisitor(
59  const Vector3<S>& a_, const Vector3<S>& b_,
60  const Vector3<S>& c_, const Vector3<S>& n_)
61  : a(a_), b(b_), c(c_), n(n_)
62 {
63  // Do nothing
64 }
65 
66 //==============================================================================
67 template <typename S, typename MotionT>
69 {
70  static S run(
71  const TriangleMotionBoundVisitor<S>& /*visitor*/,
72  const MotionT& /*motion*/)
73  {
74  return 0;
75  }
76 };
77 
78 //==============================================================================
79 template<typename S>
81  const SplineMotion<S>& motion) const
82 {
84  S, SplineMotion<S>>::run(*this, motion);
85 }
86 
87 //==============================================================================
88 template<typename S>
90  const ScrewMotion<S>& motion) const
91 {
93  S, ScrewMotion<S>>::run(*this, motion);
94 }
95 
96 //==============================================================================
97 template<typename S>
99  const InterpMotion<S>& motion) const
100 {
102  S, InterpMotion<S>>::run(*this, motion);
103 }
104 
105 //==============================================================================
106 template<typename S>
108  const TranslationMotion<S>& motion) const
109 {
111  S, TranslationMotion<S>>::run(*this, motion);
112 }
113 
114 //==============================================================================
119 template <typename S>
121 {
122  static S run(
123  const TriangleMotionBoundVisitor<S>& visitor,
124  const ScrewMotion<S>& motion)
125  {
126  Transform3<S> tf;
127  motion.getCurrentTransform(tf);
128 
129  const Vector3<S>& axis = motion.getAxis();
130  S linear_vel = motion.getLinearVelocity();
131  S angular_vel = motion.getAngularVelocity();
132  const Vector3<S>& p = motion.getAxisOrigin();
133 
134  S proj_max = ((tf.linear() * visitor.a + tf.translation() - p).cross(axis)).squaredNorm();
135  S tmp;
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;
140 
141  proj_max = std::sqrt(proj_max);
142 
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;
146 
147  return mu;
148  }
149 };
150 
151 //==============================================================================
156 template <typename S>
158 {
159  static S run(
160  const TriangleMotionBoundVisitor<S>& visitor,
161  const InterpMotion<S>& motion)
162  {
163  Transform3<S> tf;
164  motion.getCurrentTransform(tf);
165 
166  const Vector3<S>& reference_p = motion.getReferencePoint();
167  const Vector3<S>& angular_axis = motion.getAngularAxis();
168  S angular_vel = motion.getAngularVelocity();
169  const Vector3<S>& linear_vel = motion.getLinearVelocity();
170 
171  S proj_max = ((tf.linear() * (visitor.a - reference_p)).cross(angular_axis)).squaredNorm();
172  S tmp;
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;
177 
178  proj_max = std::sqrt(proj_max);
179 
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;
183 
184  return mu;
185  }
186 };
187 
188 //==============================================================================
189 template <typename S>
191 {
192  static S run(
193  const TriangleMotionBoundVisitor<S>& visitor,
194  const SplineMotion<S>& motion)
195  {
196  S T_bound = motion.computeTBound(visitor.n);
197  S tf_t = motion.getCurrentTime();
198 
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;
204 
205  S dWdW_max = motion.computeDWMax();
206  S ratio = std::min(1 - tf_t, dWdW_max);
207 
208  R_bound *= 2 * ratio;
209 
210  // std::cout << R_bound << " " << T_bound << std::endl;
211 
212  return R_bound + T_bound;
213  }
214 };
215 
216 //==============================================================================
218 template <typename S>
220 {
221  static S run(
222  const TriangleMotionBoundVisitor<S>& visitor,
223  const TranslationMotion<S>& motion)
224  {
225  return motion.getVelocity().dot(visitor.n);
226  }
227 };
228 
229 } // namespace fcl
230 
231 #endif
Definition: motion_base.h:52
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
Linear interpolation motion Each Motion is assumed to have constant linear velocity and angular veloc...
Definition: bv_motion_bound_visitor.h:54
Definition: triangle_motion_bound_visitor-inl.h:68