00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
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 Plane::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
00117 void Box::computeLocalAABB()
00118 {
00119 computeBV<AABB>(*this, Transform3f(), aabb_local);
00120 aabb_center = aabb_local.center();
00121 aabb_radius = (aabb_local.min_ - aabb_center).length();
00122 }
00123
00124 void Sphere::computeLocalAABB()
00125 {
00126 computeBV<AABB>(*this, Transform3f(), aabb_local);
00127 aabb_center = aabb_local.center();
00128 aabb_radius = radius;
00129 }
00130
00131 void Capsule::computeLocalAABB()
00132 {
00133 computeBV<AABB>(*this, Transform3f(), aabb_local);
00134 aabb_center = aabb_local.center();
00135 aabb_radius = (aabb_local.min_ - aabb_center).length();
00136 }
00137
00138 void Cone::computeLocalAABB()
00139 {
00140 computeBV<AABB>(*this, Transform3f(), aabb_local);
00141 aabb_center = aabb_local.center();
00142 aabb_radius = (aabb_local.min_ - aabb_center).length();
00143 }
00144
00145 void Cylinder::computeLocalAABB()
00146 {
00147 computeBV<AABB>(*this, Transform3f(), aabb_local);
00148 aabb_center = aabb_local.center();
00149 aabb_radius = (aabb_local.min_ - aabb_center).length();
00150 }
00151
00152 void Convex::computeLocalAABB()
00153 {
00154 computeBV<AABB>(*this, Transform3f(), aabb_local);
00155 aabb_center = aabb_local.center();
00156 aabb_radius = (aabb_local.min_ - aabb_center).length();
00157 }
00158
00159 void Plane::computeLocalAABB()
00160 {
00161 computeBV<AABB>(*this, Transform3f(), aabb_local);
00162 aabb_center = aabb_local.center();
00163 aabb_radius = (aabb_local.min_ - aabb_center).length();
00164 }
00165
00166 void Triangle2::computeLocalAABB()
00167 {
00168 computeBV<AABB>(*this, Transform3f(), aabb_local);
00169 aabb_center = aabb_local.center();
00170 aabb_radius = (aabb_local.min_ - aabb_center).length();
00171 }
00172
00173
00174 }