traversal_node_shapes.h
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 #ifndef FCL_TRAVERSAL_NODE_SHAPES_H
00039 #define FCL_TRAVERSAL_NODE_SHAPES_H
00040 
00041 #include "fcl/collision_data.h"
00042 #include "fcl/traversal/traversal_node_base.h"
00043 #include "fcl/narrowphase/narrowphase.h"
00044 #include "fcl/shape/geometric_shapes_utility.h"
00045 
00046 namespace fcl
00047 {
00048 
00049 
00051 template<typename S1, typename S2, typename NarrowPhaseSolver>
00052 class ShapeCollisionTraversalNode : public CollisionTraversalNodeBase
00053 {
00054 public:
00055   ShapeCollisionTraversalNode() : CollisionTraversalNodeBase()
00056   {
00057     model1 = NULL;
00058     model2 = NULL;
00059 
00060     nsolver = NULL;
00061   }
00062 
00064   bool BVTesting(int, int) const
00065   {
00066     return false;
00067   }
00068 
00070   void leafTesting(int, int) const
00071   {
00072     if(model1->isOccupied() && model2->isOccupied())
00073     {
00074       bool is_collision = false;
00075       if(request.enable_contact)
00076       {
00077         Vec3f contact_point, normal;
00078         FCL_REAL penetration_depth;
00079         if(nsolver->shapeIntersect(*model1, tf1, *model2, tf2, &contact_point, &penetration_depth, &normal))
00080         {
00081           is_collision = true;
00082           if(request.num_max_contacts > result->numContacts())
00083             result->addContact(Contact(model1, model2, Contact::NONE, Contact::NONE, contact_point, normal, penetration_depth));
00084         }
00085       }
00086       else
00087       {
00088         if(nsolver->shapeIntersect(*model1, tf1, *model2, tf2, NULL, NULL, NULL))
00089         {
00090           is_collision = true;
00091           if(request.num_max_contacts > result->numContacts())
00092             result->addContact(Contact(model1, model2, Contact::NONE, Contact::NONE));
00093         }
00094       }
00095 
00096       if(is_collision && request.enable_cost)
00097       {
00098         AABB aabb1, aabb2;
00099         computeBV<AABB, S1>(*model1, tf1, aabb1);
00100         computeBV<AABB, S2>(*model2, tf2, aabb2);
00101         AABB overlap_part;
00102         aabb1.overlap(aabb2, overlap_part);
00103         result->addCostSource(CostSource(overlap_part, cost_density), request.num_max_cost_sources);
00104       }
00105     }
00106     else if((!model1->isFree() && !model2->isFree()) && request.enable_cost)
00107     {
00108       if(nsolver->shapeIntersect(*model1, tf1, *model2, tf2, NULL, NULL, NULL))
00109       {
00110         AABB aabb1, aabb2;
00111         computeBV<AABB, S1>(*model1, tf1, aabb1);
00112         computeBV<AABB, S2>(*model2, tf2, aabb2);
00113         AABB overlap_part;
00114         aabb1.overlap(aabb2, overlap_part);
00115         result->addCostSource(CostSource(overlap_part, cost_density), request.num_max_cost_sources);        
00116       }      
00117     }
00118   }
00119 
00120   const S1* model1;
00121   const S2* model2;
00122 
00123   FCL_REAL cost_density;
00124 
00125   const NarrowPhaseSolver* nsolver;
00126 };
00127 
00129 template<typename S1, typename S2, typename NarrowPhaseSolver>
00130 class ShapeDistanceTraversalNode : public DistanceTraversalNodeBase
00131 {
00132 public:
00133   ShapeDistanceTraversalNode() : DistanceTraversalNodeBase()
00134   {
00135     model1 = NULL;
00136     model2 = NULL;
00137 
00138     nsolver = NULL;
00139   }
00140 
00142   FCL_REAL BVTesting(int, int) const
00143   {
00144     return -1; // should not be used 
00145   }
00146 
00148   void leafTesting(int, int) const
00149   {
00150     FCL_REAL distance;
00151     nsolver->shapeDistance(*model1, tf1, *model2, tf2, &distance);
00152     result->update(distance, model1, model2, DistanceResult::NONE, DistanceResult::NONE);
00153   }
00154 
00155   const S1* model1;
00156   const S2* model2;
00157 
00158   const NarrowPhaseSolver* nsolver;
00159 };
00160 
00161 
00162 }
00163 
00164 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines


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