Updated to Box2D 2.3.0
[qml-box2d:qml-box2d-folibis.git] / Box2D / Collision / Shapes / b2PolygonShape.cpp
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
22 b2Shape* 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
30 void 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
44 void 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
69 int32 b2PolygonShape::GetChildCount() const
70 {
71         return 1;
72 }
73
74 static 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
120 void 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)
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         m_count = m;
221
222         // Copy vertices.
223         for (int32 i = 0; i < m; ++i)
224         {
225                 m_vertices[i] = ps[hull[i]];
226         }
227
228         // Compute normals. Ensure the edges have non-zero length.
229         for (int32 i = 0; i < m; ++i)
230         {
231                 int32 i1 = i;
232                 int32 i2 = i + 1 < m ? i + 1 : 0;
233                 b2Vec2 edge = m_vertices[i2] - m_vertices[i1];
234                 b2Assert(edge.LengthSquared() > b2_epsilon * b2_epsilon);
235                 m_normals[i] = b2Cross(edge, 1.0f);
236                 m_normals[i].Normalize();
237         }
238
239         // Compute the polygon centroid.
240         m_centroid = ComputeCentroid(m_vertices, m);
241 }
242
243 bool b2PolygonShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const
244 {
245         b2Vec2 pLocal = b2MulT(xf.q, p - xf.p);
246
247         for (int32 i = 0; i < m_count; ++i)
248         {
249                 float32 dot = b2Dot(m_normals[i], pLocal - m_vertices[i]);
250                 if (dot > 0.0f)
251                 {
252                         return false;
253                 }
254         }
255
256         return true;
257 }
258
259 bool b2PolygonShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
260                                                                 const b2Transform& xf, int32 childIndex) const
261 {
262         B2_NOT_USED(childIndex);
263
264         // Put the ray into the polygon's frame of reference.
265         b2Vec2 p1 = b2MulT(xf.q, input.p1 - xf.p);
266         b2Vec2 p2 = b2MulT(xf.q, input.p2 - xf.p);
267         b2Vec2 d = p2 - p1;
268
269         float32 lower = 0.0f, upper = input.maxFraction;
270
271         int32 index = -1;
272
273         for (int32 i = 0; i < m_count; ++i)
274         {
275                 // p = p1 + a * d
276                 // dot(normal, p - v) = 0
277                 // dot(normal, p1 - v) + a * dot(normal, d) = 0
278                 float32 numerator = b2Dot(m_normals[i], m_vertices[i] - p1);
279                 float32 denominator = b2Dot(m_normals[i], d);
280
281                 if (denominator == 0.0f)
282                 {       
283                         if (numerator < 0.0f)
284                         {
285                                 return false;
286                         }
287                 }
288                 else
289                 {
290                         // Note: we want this predicate without division:
291                         // lower < numerator / denominator, where denominator < 0
292                         // Since denominator < 0, we have to flip the inequality:
293                         // lower < numerator / denominator <==> denominator * lower > numerator.
294                         if (denominator < 0.0f && numerator < lower * denominator)
295                         {
296                                 // Increase lower.
297                                 // The segment enters this half-space.
298                                 lower = numerator / denominator;
299                                 index = i;
300                         }
301                         else if (denominator > 0.0f && numerator < upper * denominator)
302                         {
303                                 // Decrease upper.
304                                 // The segment exits this half-space.
305                                 upper = numerator / denominator;
306                         }
307                 }
308
309                 // The use of epsilon here causes the assert on lower to trip
310                 // in some cases. Apparently the use of epsilon was to make edge
311                 // shapes work, but now those are handled separately.
312                 //if (upper < lower - b2_epsilon)
313                 if (upper < lower)
314                 {
315                         return false;
316                 }
317         }
318
319         b2Assert(0.0f <= lower && lower <= input.maxFraction);
320
321         if (index >= 0)
322         {
323                 output->fraction = lower;
324                 output->normal = b2Mul(xf.q, m_normals[index]);
325                 return true;
326         }
327
328         return false;
329 }
330
331 void b2PolygonShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const
332 {
333         B2_NOT_USED(childIndex);
334
335         b2Vec2 lower = b2Mul(xf, m_vertices[0]);
336         b2Vec2 upper = lower;
337
338         for (int32 i = 1; i < m_count; ++i)
339         {
340                 b2Vec2 v = b2Mul(xf, m_vertices[i]);
341                 lower = b2Min(lower, v);
342                 upper = b2Max(upper, v);
343         }
344
345         b2Vec2 r(m_radius, m_radius);
346         aabb->lowerBound = lower - r;
347         aabb->upperBound = upper + r;
348 }
349
350 void b2PolygonShape::ComputeMass(b2MassData* massData, float32 density) const
351 {
352         // Polygon mass, centroid, and inertia.
353         // Let rho be the polygon density in mass per unit area.
354         // Then:
355         // mass = rho * int(dA)
356         // centroid.x = (1/mass) * rho * int(x * dA)
357         // centroid.y = (1/mass) * rho * int(y * dA)
358         // I = rho * int((x*x + y*y) * dA)
359         //
360         // We can compute these integrals by summing all the integrals
361         // for each triangle of the polygon. To evaluate the integral
362         // for a single triangle, we make a change of variables to
363         // the (u,v) coordinates of the triangle:
364         // x = x0 + e1x * u + e2x * v
365         // y = y0 + e1y * u + e2y * v
366         // where 0 <= u && 0 <= v && u + v <= 1.
367         //
368         // We integrate u from [0,1-v] and then v from [0,1].
369         // We also need to use the Jacobian of the transformation:
370         // D = cross(e1, e2)
371         //
372         // Simplification: triangle centroid = (1/3) * (p1 + p2 + p3)
373         //
374         // The rest of the derivation is handled by computer algebra.
375
376         b2Assert(m_count >= 3);
377
378         b2Vec2 center; center.Set(0.0f, 0.0f);
379         float32 area = 0.0f;
380         float32 I = 0.0f;
381
382         // s is the reference point for forming triangles.
383         // It's location doesn't change the result (except for rounding error).
384         b2Vec2 s(0.0f, 0.0f);
385
386         // This code would put the reference point inside the polygon.
387         for (int32 i = 0; i < m_count; ++i)
388         {
389                 s += m_vertices[i];
390         }
391         s *= 1.0f / m_count;
392
393         const float32 k_inv3 = 1.0f / 3.0f;
394
395         for (int32 i = 0; i < m_count; ++i)
396         {
397                 // Triangle vertices.
398                 b2Vec2 e1 = m_vertices[i] - s;
399                 b2Vec2 e2 = i + 1 < m_count ? m_vertices[i+1] - s : m_vertices[0] - s;
400
401                 float32 D = b2Cross(e1, e2);
402
403                 float32 triangleArea = 0.5f * D;
404                 area += triangleArea;
405
406                 // Area weighted centroid
407                 center += triangleArea * k_inv3 * (e1 + e2);
408
409                 float32 ex1 = e1.x, ey1 = e1.y;
410                 float32 ex2 = e2.x, ey2 = e2.y;
411
412                 float32 intx2 = ex1*ex1 + ex2*ex1 + ex2*ex2;
413                 float32 inty2 = ey1*ey1 + ey2*ey1 + ey2*ey2;
414
415                 I += (0.25f * k_inv3 * D) * (intx2 + inty2);
416         }
417
418         // Total mass
419         massData->mass = density * area;
420
421         // Center of mass
422         b2Assert(area > b2_epsilon);
423         center *= 1.0f / area;
424         massData->center = center + s;
425
426         // Inertia tensor relative to the local origin (point s).
427         massData->I = density * I;
428         
429         // Shift to center of mass then to original body origin.
430         massData->I += massData->mass * (b2Dot(massData->center, massData->center) - b2Dot(center, center));
431 }
432
433 bool b2PolygonShape::Validate() const
434 {
435         for (int32 i = 0; i < m_count; ++i)
436         {
437                 int32 i1 = i;
438                 int32 i2 = i < m_count - 1 ? i1 + 1 : 0;
439                 b2Vec2 p = m_vertices[i1];
440                 b2Vec2 e = m_vertices[i2] - p;
441
442                 for (int32 j = 0; j < m_count; ++j)
443                 {
444                         if (j == i1 || j == i2)
445                         {
446                                 continue;
447                         }
448
449                         b2Vec2 v = m_vertices[j] - p;
450                         float32 c = b2Cross(e, v);
451                         if (c < 0.0f)
452                         {
453                                 return false;
454                         }
455                 }
456         }
457
458         return true;
459 }