fcl: motion.cpp Source File

Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  */
00034 
00038 #include "fcl/ccd/motion.h"
00039 
00040 namespace fcl
00041 {
00042 
00043 template<>
00044 FCL_REAL InterpMotion<RSS>::computeMotionBound(const RSS& bv, const Vec3f& n) const
00045 {
00046   FCL_REAL c_proj_max = ((tf.getQuatRotation().transform(bv.Tr - reference_p)).cross(angular_axis)).sqrLength();
00047   FCL_REAL tmp;
00048   tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[0] * bv.l[0] - reference_p)).cross(angular_axis)).sqrLength();
00049   if(tmp > c_proj_max) c_proj_max = tmp;
00050   tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[1] * bv.l[1] - reference_p)).cross(angular_axis)).sqrLength();
00051   if(tmp > c_proj_max) c_proj_max = tmp;
00052   tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[0] * bv.l[0] + bv.axis[1] * bv.l[1] - reference_p)).cross(angular_axis)).sqrLength();
00053   if(tmp > c_proj_max) c_proj_max = tmp;
00054 
00055   c_proj_max = sqrt(c_proj_max);
00056 
00057   FCL_REAL v_dot_n = linear_vel.dot(n);
00058   FCL_REAL w_cross_n = (angular_axis.cross(n)).length() * angular_vel;
00059   FCL_REAL mu = v_dot_n + w_cross_n * (bv.r + c_proj_max);
00060 
00061   return mu;
00062 }
00063 
00064 template<>
00065 FCL_REAL ScrewMotion<RSS>::computeMotionBound(const RSS& bv, const Vec3f& n) const
00066 {
00067   FCL_REAL c_proj_max = ((tf.getQuatRotation().transform(bv.Tr)).cross(axis)).sqrLength();
00068   FCL_REAL tmp;
00069   tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[0] * bv.l[0])).cross(axis)).sqrLength();
00070   if(tmp > c_proj_max) c_proj_max = tmp;
00071   tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[1] * bv.l[1])).cross(axis)).sqrLength();
00072   if(tmp > c_proj_max) c_proj_max = tmp;
00073   tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[0] * bv.l[0] + bv.axis[1] * bv.l[1])).cross(axis)).sqrLength();
00074   if(tmp > c_proj_max) c_proj_max = tmp;
00075 
00076   c_proj_max = sqrt(c_proj_max);
00077 
00078   FCL_REAL v_dot_n = axis.dot(n) * linear_vel;
00079   FCL_REAL w_cross_n = (axis.cross(n)).length() * angular_vel;
00080   FCL_REAL origin_proj = ((tf.getTranslation() - p).cross(axis)).length();
00081 
00082   FCL_REAL mu = v_dot_n + w_cross_n * (c_proj_max + bv.r + origin_proj);
00083 
00084   return mu;
00085 }
00086 
00087 template<>
00088 FCL_REAL SplineMotion<RSS>::computeMotionBound(const RSS& bv, const Vec3f& n) const
00089 {
00090   FCL_REAL T_bound = computeTBound(n);
00091 
00092   Vec3f c1 = bv.Tr;
00093   Vec3f c2 = bv.Tr + bv.axis[0] * bv.l[0];
00094   Vec3f c3 = bv.Tr + bv.axis[1] * bv.l[1];
00095   Vec3f c4 = bv.Tr + bv.axis[0] * bv.l[0] + bv.axis[1] * bv.l[1];
00096 
00097   FCL_REAL tmp;
00098   // max_i |c_i * n|
00099   FCL_REAL cn_max = fabs(c1.dot(n));
00100   tmp = fabs(c2.dot(n));
00101   if(tmp > cn_max) cn_max = tmp;
00102   tmp = fabs(c3.dot(n));
00103   if(tmp > cn_max) cn_max = tmp;
00104   tmp = fabs(c4.dot(n));
00105   if(tmp > cn_max) cn_max = tmp;
00106 
00107   // max_i ||c_i||
00108   FCL_REAL cmax = c1.sqrLength();
00109   tmp = c2.sqrLength();
00110   if(tmp > cmax) cmax = tmp;
00111   tmp = c3.sqrLength();
00112   if(tmp > cmax) cmax = tmp;
00113   tmp = c4.sqrLength();
00114   if(tmp > cmax) cmax = tmp;
00115   cmax = sqrt(cmax);
00116 
00117   // max_i ||c_i x n||
00118   FCL_REAL cxn_max = (c1.cross(n)).sqrLength();
00119   tmp = (c2.cross(n)).sqrLength();
00120   if(tmp > cxn_max) cxn_max = tmp;
00121   tmp = (c3.cross(n)).sqrLength();
00122   if(tmp > cxn_max) cxn_max = tmp;
00123   tmp = (c4.cross(n)).sqrLength();
00124   if(tmp > cxn_max) cxn_max = tmp;
00125   cxn_max = sqrt(cxn_max);
00126 
00127   FCL_REAL dWdW_max = computeDWMax();
00128   FCL_REAL ratio = std::min(1 - tf_t, dWdW_max);
00129 
00130   FCL_REAL R_bound = 2 * (cn_max + cmax + cxn_max + 3 * bv.r) * ratio;
00131 
00132 
00133   // std::cout << R_bound << " " << T_bound << std::endl;
00134 
00135   return R_bound + T_bound;
00136 }
00137 
00138 }