geometric_shapes.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 
00038 #include "fcl/shape/geometric_shapes.h"
00039 #include "fcl/shape/geometric_shapes_utility.h"
00040 
00041 namespace fcl
00042 {
00043 
00044 void Convex::fillEdges()
00045 {
00046   int* points_in_poly = polygons;
00047   if(edges) delete [] edges;
00048 
00049   int num_edges_alloc = 0;
00050   for(int i = 0; i < num_planes; ++i)
00051   {
00052     num_edges_alloc += *points_in_poly;
00053     points_in_poly += (*points_in_poly + 1);
00054   }
00055 
00056   edges = new Edge[num_edges_alloc];
00057 
00058   points_in_poly = polygons;
00059   int* index = polygons + 1;
00060   num_edges = 0;
00061   Edge e;
00062   bool isinset;
00063   for(int i = 0; i < num_planes; ++i)
00064   {
00065     for(int j = 0; j < *points_in_poly; ++j)
00066     {
00067       e.first = std::min(index[j], index[(j+1)%*points_in_poly]);
00068       e.second = std::max(index[j], index[(j+1)%*points_in_poly]);
00069       isinset = false;
00070       for(int k = 0; k < num_edges; ++k)
00071       {
00072         if((edges[k].first == e.first) && (edges[k].second == e.second))
00073         {
00074           isinset = true;
00075           break;
00076         }
00077       }
00078 
00079       if(!isinset)
00080       {
00081         edges[num_edges].first = e.first;
00082         edges[num_edges].second = e.second;
00083         ++num_edges;
00084       }
00085     }
00086 
00087     points_in_poly += (*points_in_poly + 1);
00088     index = points_in_poly + 1;
00089   }
00090 
00091   if(num_edges < num_edges_alloc)
00092   {
00093     Edge* tmp = new Edge[num_edges];
00094     memcpy(tmp, edges, num_edges * sizeof(Edge));
00095     delete [] edges;
00096     edges = tmp;
00097   }
00098 }
00099 
00100 void Halfspace::unitNormalTest()
00101 {
00102   FCL_REAL l = n.length();
00103   if(l > 0)
00104   {
00105     FCL_REAL inv_l = 1.0 / l;
00106     n *= inv_l;
00107     d *= inv_l;
00108   }
00109   else
00110   {
00111     n.setValue(1, 0, 0);
00112     d = 0;
00113   }  
00114 }
00115 
00116 void Plane::unitNormalTest()
00117 {
00118   FCL_REAL l = n.length();
00119   if(l > 0)
00120   {
00121     FCL_REAL inv_l = 1.0 / l;
00122     n *= inv_l;
00123     d *= inv_l;
00124   }
00125   else
00126   {
00127     n.setValue(1, 0, 0);
00128     d = 0;
00129   }
00130 }
00131 
00132 
00133 void Box::computeLocalAABB()
00134 {
00135   computeBV<AABB>(*this, Transform3f(), aabb_local);
00136   aabb_center = aabb_local.center();
00137   aabb_radius = (aabb_local.min_ - aabb_center).length();
00138 }
00139 
00140 void Sphere::computeLocalAABB()
00141 {
00142   computeBV<AABB>(*this, Transform3f(), aabb_local);
00143   aabb_center = aabb_local.center();
00144   aabb_radius = radius;
00145 }
00146 
00147 void Capsule::computeLocalAABB()
00148 {
00149   computeBV<AABB>(*this, Transform3f(), aabb_local);
00150   aabb_center = aabb_local.center();
00151   aabb_radius = (aabb_local.min_ - aabb_center).length();
00152 }
00153 
00154 void Cone::computeLocalAABB()
00155 {
00156   computeBV<AABB>(*this, Transform3f(), aabb_local);
00157   aabb_center = aabb_local.center();
00158   aabb_radius = (aabb_local.min_ - aabb_center).length();
00159 }
00160 
00161 void Cylinder::computeLocalAABB()
00162 {
00163   computeBV<AABB>(*this, Transform3f(), aabb_local);
00164   aabb_center = aabb_local.center();
00165   aabb_radius = (aabb_local.min_ - aabb_center).length();
00166 }
00167 
00168 void Convex::computeLocalAABB()
00169 {
00170   computeBV<AABB>(*this, Transform3f(), aabb_local);
00171   aabb_center = aabb_local.center();
00172   aabb_radius = (aabb_local.min_ - aabb_center).length();
00173 }
00174 
00175 void Halfspace::computeLocalAABB()
00176 {
00177   computeBV<AABB>(*this, Transform3f(), aabb_local);
00178   aabb_center = aabb_local.center();
00179   aabb_radius = (aabb_local.min_ - aabb_center).length();
00180 }
00181 
00182 void Plane::computeLocalAABB()
00183 {
00184   computeBV<AABB>(*this, Transform3f(), aabb_local);
00185   aabb_center = aabb_local.center();
00186   aabb_radius = (aabb_local.min_ - aabb_center).length();
00187 }
00188 
00189 void Triangle2::computeLocalAABB()
00190 {
00191   computeBV<AABB>(*this, Transform3f(), aabb_local);
00192   aabb_center = aabb_local.center();
00193   aabb_radius = (aabb_local.min_ - aabb_center).length();
00194 }
00195 
00196 
00197 }
 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