All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends

distance.cpp

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/distance.h"
00038 #include "fcl/distance_func_matrix.h"
00039 #include "fcl/narrowphase/narrowphase.h"
00040 
00041 #include <iostream>
00042 
00043 namespace fcl
00044 {
00045 
00046 template<typename GJKSolver>
00047 DistanceFunctionMatrix<GJKSolver>& getDistanceFunctionLookTable()
00048 {
00049   static DistanceFunctionMatrix<GJKSolver> table;
00050   return table;
00051 }
00052 
00053 template<typename NarrowPhaseSolver>
00054 FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const NarrowPhaseSolver* nsolver,
00055                   const DistanceRequest& request, DistanceResult& result)
00056 {
00057   return distance<NarrowPhaseSolver>(o1->getCollisionGeometry(), o1->getTransform(), o2->getCollisionGeometry(), o2->getTransform(), nsolver,
00058                                      request, result);
00059 }
00060 
00061 template<typename NarrowPhaseSolver>
00062 FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, 
00063                   const CollisionGeometry* o2, const Transform3f& tf2,
00064                   const NarrowPhaseSolver* nsolver_,
00065                   const DistanceRequest& request, DistanceResult& result)
00066 {
00067   const NarrowPhaseSolver* nsolver = nsolver_;
00068   if(!nsolver_) 
00069     nsolver = new NarrowPhaseSolver();
00070 
00071   const DistanceFunctionMatrix<NarrowPhaseSolver>& looktable = getDistanceFunctionLookTable<NarrowPhaseSolver>();
00072 
00073   OBJECT_TYPE object_type1 = o1->getObjectType();
00074   NODE_TYPE node_type1 = o1->getNodeType();
00075   OBJECT_TYPE object_type2 = o2->getObjectType();
00076   NODE_TYPE node_type2 = o2->getNodeType();
00077 
00078   FCL_REAL res = std::numeric_limits<FCL_REAL>::max();
00079 
00080   if(object_type1 == OT_GEOM && object_type2 == OT_BVH)
00081   {
00082     if(!looktable.distance_matrix[node_type2][node_type1])
00083     {
00084       std::cerr << "Warning: distance function between node type " << node_type1 << " and node type " << node_type2 << " is not supported" << std::endl;
00085     }
00086     else
00087     {
00088       res = looktable.distance_matrix[node_type2][node_type1](o2, tf2, o1, tf1, nsolver, request, result);
00089     }
00090   }
00091   else
00092   {
00093     if(!looktable.distance_matrix[node_type1][node_type2])
00094     {
00095       std::cerr << "Warning: distance function between node type " << node_type1 << " and node type " << node_type2 << " is not supported" << std::endl;
00096     }
00097     else
00098     {
00099       res = looktable.distance_matrix[node_type1][node_type2](o1, tf1, o2, tf2, nsolver, request, result);    
00100     }
00101   }
00102 
00103   if(!nsolver_)
00104     delete nsolver;
00105 
00106   return res;
00107 }
00108 
00109 template FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const GJKSolver_libccd* nsolver, const DistanceRequest& request, DistanceResult& result);
00110 template FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const GJKSolver_indep* nsolver, const DistanceRequest& request, DistanceResult& result);
00111 template FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver_libccd* nsolver, const DistanceRequest& request, DistanceResult& result);
00112 template FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver_indep* nsolver, const DistanceRequest& request, DistanceResult& result);
00113 
00114 FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const DistanceRequest& request, DistanceResult& result)
00115 {
00116   GJKSolver_libccd solver;
00117   return distance<GJKSolver_libccd>(o1, o2, &solver, request, result);
00118 }
00119 
00120 FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1,
00121                   const CollisionGeometry* o2, const Transform3f& tf2,
00122                   const DistanceRequest& request, DistanceResult& result)
00123 {
00124   GJKSolver_libccd solver;
00125   return distance<GJKSolver_libccd>(o1, tf1, o2, tf2, &solver, request, result);
00126   // GJKSolver_indep solver;
00127   // return distance<GJKSolver_indep>(o1, tf1, o2, tf2, &solver, request, result);
00128 }
00129 
00130 
00131 }