conservative_advancement.cpp
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 
00037 #include "fcl/ccd/conservative_advancement.h"
00038 #include "fcl/ccd/motion.h"
00039 #include "fcl/collision_node.h"
00040 #include "fcl/traversal/traversal_node_bvhs.h"
00041 #include "fcl/traversal/traversal_node_setup.h"
00042 #include "fcl/traversal/traversal_recurse.h"
00043 #include "fcl/collision.h"
00044 
00045 
00046 namespace fcl
00047 
00048 {
00049 
00050 template<typename BV>
00051 int conservativeAdvancement(const CollisionGeometry* o1,
00052                             MotionBase<BV>* motion1,
00053                             const CollisionGeometry* o2,
00054                             MotionBase<BV>* motion2,
00055                             const CollisionRequest& request,
00056                             CollisionResult& result,
00057                             FCL_REAL& toc)
00058 {
00059   if(request.num_max_contacts == 0)
00060   {
00061     std::cerr << "Warning: should stop early as num_max_contact is " << request.num_max_contacts << " !" << std::endl;
00062     return 0;
00063   }
00064 
00065   const OBJECT_TYPE object_type1 = o1->getObjectType();
00066   const NODE_TYPE node_type1 = o1->getNodeType();
00067 
00068   const OBJECT_TYPE object_type2 = o2->getObjectType();
00069   const NODE_TYPE node_type2 = o2->getNodeType();
00070 
00071   if(object_type1 != OT_BVH || object_type2 != OT_BVH)
00072     return 0;
00073 
00074   if(node_type1 != BV_RSS || node_type2 != BV_RSS)
00075     return 0;
00076 
00077 
00078   const BVHModel<RSS>* model1 = (const BVHModel<RSS>*)o1;
00079   const BVHModel<RSS>* model2 = (const BVHModel<RSS>*)o2;
00080 
00081   Transform3f tf1, tf2;
00082   motion1->getCurrentTransform(tf1);
00083   motion2->getCurrentTransform(tf2);
00084 
00085   // whether the first start configuration is in collision
00086   MeshCollisionTraversalNodeRSS cnode;
00087   if(!initialize(cnode, *model1, tf1, *model2, tf2, request, result))
00088     std::cout << "initialize error" << std::endl;
00089 
00090   relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(), tf2.getTranslation(), cnode.R, cnode.T);
00091 
00092   cnode.enable_statistics = false;
00093   cnode.request = request;
00094 
00095   collide(&cnode);
00096 
00097   int b = result.numContacts();
00098 
00099   if(b > 0)
00100   {
00101     toc = 0;
00102     // std::cout << "zero collide" << std::endl;
00103     return b;
00104   }
00105 
00106 
00107   MeshConservativeAdvancementTraversalNodeRSS node;
00108 
00109   initialize(node, *model1, *model2, tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(), tf2.getTranslation());
00110 
00111   node.motion1 = motion1;
00112   node.motion2 = motion2;
00113 
00114 
00115   do
00116   {
00117     Matrix3f R1_t, R2_t;
00118     Vec3f T1_t, T2_t;
00119 
00120     node.motion1->getCurrentTransform(R1_t, T1_t);
00121     node.motion2->getCurrentTransform(R2_t, T2_t);
00122 
00123     // compute the transformation from 1 to 2
00124     relativeTransform(R1_t, T1_t, R2_t, T2_t, node.R, node.T);
00125 
00126     node.delta_t = 1;
00127     node.min_distance = std::numeric_limits<FCL_REAL>::max();
00128 
00129     distanceRecurse(&node, 0, 0, NULL);
00130 
00131     if(node.delta_t <= node.t_err)
00132     {
00133       // std::cout << node.delta_t << " " << node.t_err << std::endl;
00134       break;
00135     }
00136 
00137     node.toc += node.delta_t;
00138     if(node.toc > 1)
00139     {
00140       node.toc = 1;
00141       break;
00142     }
00143 
00144     node.motion1->integrate(node.toc);
00145     node.motion2->integrate(node.toc);
00146   }
00147   while(1);
00148 
00149   toc = node.toc;
00150 
00151   if(node.toc < 1)
00152     return 1;
00153 
00154   return 0;
00155 }
00156 
00157 
00158 template int conservativeAdvancement<RSS>(const CollisionGeometry* o1,
00159                                           MotionBase<RSS>* motion1,
00160                                           const CollisionGeometry* o2,
00161                                           MotionBase<RSS>* motion2,
00162                                           const CollisionRequest& request,
00163                                           CollisionResult& result,
00164                                           FCL_REAL& toc);
00165 
00166 
00167 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines


fcl
Author(s): Jia Pan
autogenerated on Tue Jan 15 2013 16:05:30