1/*
2* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
3*
4* This software is provided 'as-is', without any express or implied
5* warranty. In no event will the authors be held liable for any damages
6* arising from the use of this software.
7* Permission is granted to anyone to use this software for any purpose,
8* including commercial applications, and to alter it and redistribute it
9* freely, subject to the following restrictions:
10* 1. The origin of this software must not be misrepresented; you must not
11* claim that you wrote the original software. If you use this software
12* in a product, an acknowledgment in the product documentation would be
13* appreciated but is not required.
14* 2. Altered source versions must be plainly marked as such, and must not be
15* misrepresented as being the original software.
16* 3. This notice may not be removed or altered from any source distribution.
17*/
18
19#include <Box2D/Collision/Shapes/b2PolygonShape.h>
20#include <new>
21
22b2Shape* b2PolygonShape::Clone(b2BlockAllocator* allocator) const
23{
24 void* mem = allocator->Allocate(sizeof(b2PolygonShape));
25 b2PolygonShape* clone = new (mem) b2PolygonShape;
26 *clone = *this;
27 return clone;
28}
29
30void b2PolygonShape::SetAsBox(float32 hx, float32 hy)
31{
32 m_count = 4;
33 m_vertices[0].Set(-hx, -hy);
34 m_vertices[1].Set( hx, -hy);
35 m_vertices[2].Set( hx, hy);
36 m_vertices[3].Set(-hx, hy);
37 m_normals[0].Set(0.0f, -1.0f);
38 m_normals[1].Set(1.0f, 0.0f);
39 m_normals[2].Set(0.0f, 1.0f);
40 m_normals[3].Set(-1.0f, 0.0f);
41 m_centroid.SetZero();
42}
43
44void b2PolygonShape::SetAsBox(float32 hx, float32 hy, const b2Vec2& center, float32 angle)
45{
46 m_count = 4;
47 m_vertices[0].Set(-hx, -hy);
48 m_vertices[1].Set( hx, -hy);
49 m_vertices[2].Set( hx, hy);
50 m_vertices[3].Set(-hx, hy);
51 m_normals[0].Set(0.0f, -1.0f);
52 m_normals[1].Set(1.0f, 0.0f);
53 m_normals[2].Set(0.0f, 1.0f);
54 m_normals[3].Set(-1.0f, 0.0f);
55 m_centroid = center;
56
57 b2Transform xf;
58 xf.p = center;
59 xf.q.Set(angle);
60
61 // Transform vertices and normals.
62 for (int32 i = 0; i < m_count; ++i)
63 {
64 m_vertices[i] = b2Mul(xf, m_vertices[i]);
65 m_normals[i] = b2Mul(xf.q, m_normals[i]);
66 }
67}
68
69int32 b2PolygonShape::GetChildCount() const
70{
71 return 1;
72}
73
74static b2Vec2 ComputeCentroid(const b2Vec2* vs, int32 count)
75{
76 b2Assert(count >= 3);
77
78 b2Vec2 c; c.Set(0.0f, 0.0f);
79 float32 area = 0.0f;
80
81 // pRef is the reference point for forming triangles.
82 // It's location doesn't change the result (except for rounding error).
83 b2Vec2 pRef(0.0f, 0.0f);
84#if 0
85 // This code would put the reference point inside the polygon.
86 for (int32 i = 0; i < count; ++i)
87 {
88 pRef += vs[i];
89 }
90 pRef *= 1.0f / count;
91#endif
92
93 const float32 inv3 = 1.0f / 3.0f;
94
95 for (int32 i = 0; i < count; ++i)
96 {
97 // Triangle vertices.
98 b2Vec2 p1 = pRef;
99 b2Vec2 p2 = vs[i];
100 b2Vec2 p3 = i + 1 < count ? vs[i+1] : vs[0];
101
102 b2Vec2 e1 = p2 - p1;
103 b2Vec2 e2 = p3 - p1;
104
105 float32 D = b2Cross(e1, e2);
106
107 float32 triangleArea = 0.5f * D;
108 area += triangleArea;
109
110 // Area weighted centroid
111 c += triangleArea * inv3 * (p1 + p2 + p3);
112 }
113
114 // Centroid
115 b2Assert(area > b2_epsilon);
116 c *= 1.0f / area;
117 return c;
118}
119
120void b2PolygonShape::Set(const b2Vec2* vertices, int32 count)
121{
122 b2Assert(3 <= count && count <= b2_maxPolygonVertices);
123 if (count < 3)
124 {
125 SetAsBox(1.0f, 1.0f);
126 return;
127 }
128
129 int32 n = b2Min(count, b2_maxPolygonVertices);
130
131 // Perform welding and copy vertices into local buffer.
132 b2Vec2 ps[b2_maxPolygonVertices];
133 int32 tempCount = 0;
134 for (int32 i = 0; i < n; ++i)
135 {
136 b2Vec2 v = vertices[i];
137
138 bool unique = true;
139 for (int32 j = 0; j < tempCount; ++j)
140 {
141 if (b2DistanceSquared(v, ps[j]) < ((0.5f * b2_linearSlop) * (0.5f * b2_linearSlop)))
142 {
143 unique = false;
144 break;
145 }
146 }
147
148 if (unique)
149 {
150 ps[tempCount++] = v;
151 }
152 }
153
154 n = tempCount;
155 if (n < 3)
156 {
157 // Polygon is degenerate.
158 b2Assert(false);
159 SetAsBox(1.0f, 1.0f);
160 return;
161 }
162
163 // Create the convex hull using the Gift wrapping algorithm
164 // http://en.wikipedia.org/wiki/Gift_wrapping_algorithm
165
166 // Find the right most point on the hull
167 int32 i0 = 0;
168 float32 x0 = ps[0].x;
169 for (int32 i = 1; i < n; ++i)
170 {
171 float32 x = ps[i].x;
172 if (x > x0 || (x == x0 && ps[i].y < ps[i0].y))
173 {
174 i0 = i;
175 x0 = x;
176 }
177 }
178
179 int32 hull[b2_maxPolygonVertices];
180 int32 m = 0;
181 int32 ih = i0;
182
183 for (;;)
184 {
185 hull[m] = ih;
186
187 int32 ie = 0;
188 for (int32 j = 1; j < n; ++j)
189 {
190 if (ie == ih)
191 {
192 ie = j;
193 continue;
194 }
195
196 b2Vec2 r = ps[ie] - ps[hull[m]];
197 b2Vec2 v = ps[j] - ps[hull[m]];
198 float32 c = b2Cross(r, v);
199 if (c < 0.0f)
200 {
201 ie = j;
202 }
203
204 // Collinearity check
205 if (c == 0.0f && v.LengthSquared() > r.LengthSquared())
206 {
207 ie = j;
208 }
209 }
210
211 ++m;
212 ih = ie;
213
214 if (ie == i0)
215 {
216 break;
217 }
218 }
219
220 if (m < 3)
221 {
222 // Polygon is degenerate.
223 b2Assert(false);
224 SetAsBox(1.0f, 1.0f);
225 return;
226 }
227
228 m_count = m;
229
230 // Copy vertices.
231 for (int32 i = 0; i < m; ++i)
232 {
233 m_vertices[i] = ps[hull[i]];
234 }
235
236 // Compute normals. Ensure the edges have non-zero length.
237 for (int32 i = 0; i < m; ++i)
238 {
239 int32 i1 = i;
240 int32 i2 = i + 1 < m ? i + 1 : 0;
241 b2Vec2 edge = m_vertices[i2] - m_vertices[i1];
242 b2Assert(edge.LengthSquared() > b2_epsilon * b2_epsilon);
243 m_normals[i] = b2Cross(edge, 1.0f);
244 m_normals[i].Normalize();
245 }
246
247 // Compute the polygon centroid.
248 m_centroid = ComputeCentroid(m_vertices, m);
249}
250
251bool b2PolygonShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const
252{
253 b2Vec2 pLocal = b2MulT(xf.q, p - xf.p);
254
255 for (int32 i = 0; i < m_count; ++i)
256 {
257 float32 dot = b2Dot(m_normals[i], pLocal - m_vertices[i]);
258 if (dot > 0.0f)
259 {
260 return false;
261 }
262 }
263
264 return true;
265}
266
267bool b2PolygonShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
268 const b2Transform& xf, int32 childIndex) const
269{
270 B2_NOT_USED(childIndex);
271
272 // Put the ray into the polygon's frame of reference.
273 b2Vec2 p1 = b2MulT(xf.q, input.p1 - xf.p);
274 b2Vec2 p2 = b2MulT(xf.q, input.p2 - xf.p);
275 b2Vec2 d = p2 - p1;
276
277 float32 lower = 0.0f, upper = input.maxFraction;
278
279 int32 index = -1;
280
281 for (int32 i = 0; i < m_count; ++i)
282 {
283 // p = p1 + a * d
284 // dot(normal, p - v) = 0
285 // dot(normal, p1 - v) + a * dot(normal, d) = 0
286 float32 numerator = b2Dot(m_normals[i], m_vertices[i] - p1);
287 float32 denominator = b2Dot(m_normals[i], d);
288
289 if (denominator == 0.0f)
290 {
291 if (numerator < 0.0f)
292 {
293 return false;
294 }
295 }
296 else
297 {
298 // Note: we want this predicate without division:
299 // lower < numerator / denominator, where denominator < 0
300 // Since denominator < 0, we have to flip the inequality:
301 // lower < numerator / denominator <==> denominator * lower > numerator.
302 if (denominator < 0.0f && numerator < lower * denominator)
303 {
304 // Increase lower.
305 // The segment enters this half-space.
306 lower = numerator / denominator;
307 index = i;
308 }
309 else if (denominator > 0.0f && numerator < upper * denominator)
310 {
311 // Decrease upper.
312 // The segment exits this half-space.
313 upper = numerator / denominator;
314 }
315 }
316
317 // The use of epsilon here causes the assert on lower to trip
318 // in some cases. Apparently the use of epsilon was to make edge
319 // shapes work, but now those are handled separately.
320 //if (upper < lower - b2_epsilon)
321 if (upper < lower)
322 {
323 return false;
324 }
325 }
326
327 b2Assert(0.0f <= lower && lower <= input.maxFraction);
328
329 if (index >= 0)
330 {
331 output->fraction = lower;
332 output->normal = b2Mul(xf.q, m_normals[index]);
333 return true;
334 }
335
336 return false;
337}
338
339void b2PolygonShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const
340{
341 B2_NOT_USED(childIndex);
342
343 b2Vec2 lower = b2Mul(xf, m_vertices[0]);
344 b2Vec2 upper = lower;
345
346 for (int32 i = 1; i < m_count; ++i)
347 {
348 b2Vec2 v = b2Mul(xf, m_vertices[i]);
349 lower = b2Min(lower, v);
350 upper = b2Max(upper, v);
351 }
352
353 b2Vec2 r(m_radius, m_radius);
354 aabb->lowerBound = lower - r;
355 aabb->upperBound = upper + r;
356}
357
358void b2PolygonShape::ComputeMass(b2MassData* massData, float32 density) const
359{
360 // Polygon mass, centroid, and inertia.
361 // Let rho be the polygon density in mass per unit area.
362 // Then:
363 // mass = rho * int(dA)
364 // centroid.x = (1/mass) * rho * int(x * dA)
365 // centroid.y = (1/mass) * rho * int(y * dA)
366 // I = rho * int((x*x + y*y) * dA)
367 //
368 // We can compute these integrals by summing all the integrals
369 // for each triangle of the polygon. To evaluate the integral
370 // for a single triangle, we make a change of variables to
371 // the (u,v) coordinates of the triangle:
372 // x = x0 + e1x * u + e2x * v
373 // y = y0 + e1y * u + e2y * v
374 // where 0 <= u && 0 <= v && u + v <= 1.
375 //
376 // We integrate u from [0,1-v] and then v from [0,1].
377 // We also need to use the Jacobian of the transformation:
378 // D = cross(e1, e2)
379 //
380 // Simplification: triangle centroid = (1/3) * (p1 + p2 + p3)
381 //
382 // The rest of the derivation is handled by computer algebra.
383
384 b2Assert(m_count >= 3);
385
386 b2Vec2 center; center.Set(0.0f, 0.0f);
387 float32 area = 0.0f;
388 float32 I = 0.0f;
389
390 // s is the reference point for forming triangles.
391 // It's location doesn't change the result (except for rounding error).
392 b2Vec2 s(0.0f, 0.0f);
393
394 // This code would put the reference point inside the polygon.
395 for (int32 i = 0; i < m_count; ++i)
396 {
397 s += m_vertices[i];
398 }
399 s *= 1.0f / m_count;
400
401 const float32 k_inv3 = 1.0f / 3.0f;
402
403 for (int32 i = 0; i < m_count; ++i)
404 {
405 // Triangle vertices.
406 b2Vec2 e1 = m_vertices[i] - s;
407 b2Vec2 e2 = i + 1 < m_count ? m_vertices[i+1] - s : m_vertices[0] - s;
408
409 float32 D = b2Cross(e1, e2);
410
411 float32 triangleArea = 0.5f * D;
412 area += triangleArea;
413
414 // Area weighted centroid
415 center += triangleArea * k_inv3 * (e1 + e2);
416
417 float32 ex1 = e1.x, ey1 = e1.y;
418 float32 ex2 = e2.x, ey2 = e2.y;
419
420 float32 intx2 = ex1*ex1 + ex2*ex1 + ex2*ex2;
421 float32 inty2 = ey1*ey1 + ey2*ey1 + ey2*ey2;
422
423 I += (0.25f * k_inv3 * D) * (intx2 + inty2);
424 }
425
426 // Total mass
427 massData->mass = density * area;
428
429 // Center of mass
430 b2Assert(area > b2_epsilon);
431 center *= 1.0f / area;
432 massData->center = center + s;
433
434 // Inertia tensor relative to the local origin (point s).
435 massData->I = density * I;
436
437 // Shift to center of mass then to original body origin.
438 massData->I += massData->mass * (b2Dot(massData->center, massData->center) - b2Dot(center, center));
439}
440
441bool b2PolygonShape::Validate() const
442{
443 for (int32 i = 0; i < m_count; ++i)
444 {
445 int32 i1 = i;
446 int32 i2 = i < m_count - 1 ? i1 + 1 : 0;
447 b2Vec2 p = m_vertices[i1];
448 b2Vec2 e = m_vertices[i2] - p;
449
450 for (int32 j = 0; j < m_count; ++j)
451 {
452 if (j == i1 || j == i2)
453 {
454 continue;
455 }
456
457 b2Vec2 v = m_vertices[j] - p;
458 float32 c = b2Cross(e, v);
459 if (c < 0.0f)
460 {
461 return false;
462 }
463 }
464 }
465
466 return true;
467}
468