1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011-2014, Willow Garage, Inc.
5 * Copyright (c) 2014-2016, Open Source Robotics Foundation
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of Open Source Robotics Foundation nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 */
35
36 /** \author Jia Pan */
37
38
39 #include "fcl/shape/geometric_shapes.h"
40 #include "fcl/shape/geometric_shapes_utility.h"
41
42 namespace fcl
43 {
44
fillEdges()45 void Convex::fillEdges()
46 {
47 int* points_in_poly = polygons;
48 if(edges) delete [] edges;
49
50 int num_edges_alloc = 0;
51 for(int i = 0; i < num_planes; ++i)
52 {
53 num_edges_alloc += *points_in_poly;
54 points_in_poly += (*points_in_poly + 1);
55 }
56
57 edges = new Edge[num_edges_alloc];
58
59 points_in_poly = polygons;
60 int* index = polygons + 1;
61 num_edges = 0;
62 Edge e;
63 bool isinset;
64 for(int i = 0; i < num_planes; ++i)
65 {
66 for(int j = 0; j < *points_in_poly; ++j)
67 {
68 e.first = std::min(index[j], index[(j+1)%*points_in_poly]);
69 e.second = std::max(index[j], index[(j+1)%*points_in_poly]);
70 isinset = false;
71 for(int k = 0; k < num_edges; ++k)
72 {
73 if((edges[k].first == e.first) && (edges[k].second == e.second))
74 {
75 isinset = true;
76 break;
77 }
78 }
79
80 if(!isinset)
81 {
82 edges[num_edges].first = e.first;
83 edges[num_edges].second = e.second;
84 ++num_edges;
85 }
86 }
87
88 points_in_poly += (*points_in_poly + 1);
89 index = points_in_poly + 1;
90 }
91
92 if(num_edges < num_edges_alloc)
93 {
94 Edge* tmp = new Edge[num_edges];
95 memcpy(tmp, edges, num_edges * sizeof(Edge));
96 delete [] edges;
97 edges = tmp;
98 }
99 }
100
unitNormalTest()101 void Halfspace::unitNormalTest()
102 {
103 FCL_REAL l = n.length();
104 if(l > 0)
105 {
106 FCL_REAL inv_l = 1.0 / l;
107 n *= inv_l;
108 d *= inv_l;
109 }
110 else
111 {
112 n.setValue(1, 0, 0);
113 d = 0;
114 }
115 }
116
unitNormalTest()117 void Plane::unitNormalTest()
118 {
119 FCL_REAL l = n.length();
120 if(l > 0)
121 {
122 FCL_REAL inv_l = 1.0 / l;
123 n *= inv_l;
124 d *= inv_l;
125 }
126 else
127 {
128 n.setValue(1, 0, 0);
129 d = 0;
130 }
131 }
132
133
computeLocalAABB()134 void Box::computeLocalAABB()
135 {
136 computeBV<AABB>(*this, Transform3f(), aabb_local);
137 aabb_center = aabb_local.center();
138 aabb_radius = (aabb_local.min_ - aabb_center).length();
139 }
140
computeLocalAABB()141 void Sphere::computeLocalAABB()
142 {
143 computeBV<AABB>(*this, Transform3f(), aabb_local);
144 aabb_center = aabb_local.center();
145 aabb_radius = radius;
146 }
147
computeLocalAABB()148 void Ellipsoid::computeLocalAABB()
149 {
150 computeBV<AABB>(*this, Transform3f(), aabb_local);
151 aabb_center = aabb_local.center();
152 aabb_radius = (aabb_local.min_ - aabb_center).length();
153 }
154
computeLocalAABB()155 void Capsule::computeLocalAABB()
156 {
157 computeBV<AABB>(*this, Transform3f(), aabb_local);
158 aabb_center = aabb_local.center();
159 aabb_radius = (aabb_local.min_ - aabb_center).length();
160 }
161
computeLocalAABB()162 void Cone::computeLocalAABB()
163 {
164 computeBV<AABB>(*this, Transform3f(), aabb_local);
165 aabb_center = aabb_local.center();
166 aabb_radius = (aabb_local.min_ - aabb_center).length();
167 }
168
computeLocalAABB()169 void Cylinder::computeLocalAABB()
170 {
171 computeBV<AABB>(*this, Transform3f(), aabb_local);
172 aabb_center = aabb_local.center();
173 aabb_radius = (aabb_local.min_ - aabb_center).length();
174 }
175
computeLocalAABB()176 void Convex::computeLocalAABB()
177 {
178 computeBV<AABB>(*this, Transform3f(), aabb_local);
179 aabb_center = aabb_local.center();
180 aabb_radius = (aabb_local.min_ - aabb_center).length();
181 }
182
computeLocalAABB()183 void Halfspace::computeLocalAABB()
184 {
185 computeBV<AABB>(*this, Transform3f(), aabb_local);
186 aabb_center = aabb_local.center();
187 aabb_radius = (aabb_local.min_ - aabb_center).length();
188 }
189
computeLocalAABB()190 void Plane::computeLocalAABB()
191 {
192 computeBV<AABB>(*this, Transform3f(), aabb_local);
193 aabb_center = aabb_local.center();
194 aabb_radius = (aabb_local.min_ - aabb_center).length();
195 }
196
computeLocalAABB()197 void TriangleP::computeLocalAABB()
198 {
199 computeBV<AABB>(*this, Transform3f(), aabb_local);
200 aabb_center = aabb_local.center();
201 aabb_radius = (aabb_local.min_ - aabb_center).length();
202 }
203
204
205 }
206