b2PolygonShape.cpp
Go to the documentation of this file.
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 
20 #include <new>
21 
23 {
24  void* mem = allocator->Allocate(sizeof(b2PolygonShape));
25  b2PolygonShape* clone = new (mem) b2PolygonShape;
26  *clone = *this;
27  return clone;
28 }
29 
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);
42 }
43 
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 
70 {
71  return 1;
72 }
73 
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 
130 
131  // Perform welding and copy vertices into local buffer.
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 
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];
243  m_normals[i] = b2Cross(edge, 1.0f);
244  m_normals[i].Normalize();
245  }
246 
247  // Compute the polygon centroid.
249 }
250 
251 bool 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 
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);
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 
339 void 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 
354  aabb->lowerBound = lower - r;
355  aabb->upperBound = upper + r;
356 }
357 
358 void 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 
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 }
d
const GLdouble * v
float32 b2Dot(const b2Vec2 &a, const b2Vec2 &b)
Perform the dot product on two vectors.
Definition: b2Math.h:405
GLint GLint GLint GLint GLint GLint y
b2Vec2 b2Mul(const b2Mat22 &A, const b2Vec2 &v)
Definition: b2Math.h:432
b2Vec2 p
Definition: b2Math.h:371
CvPoint2D32f * center
#define b2_linearSlop
Definition: b2Settings.h:68
b2Vec2 lowerBound
the lower vertex
Definition: b2Collision.h:214
b2Rot q
Definition: b2Math.h:372
int32 GetChildCount() const
const GLfloat * c
GLenum GLenum GLenum input
float32 I
The rotational inertia of the shape about the local origin.
Definition: b2Shape.h:36
bool RayCast(b2RayCastOutput *output, const b2RayCastInput &input, const b2Transform &transform, int32 childIndex) const
Implement b2Shape.
Ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).
Definition: b2Collision.h:147
#define B2_NOT_USED(x)
Definition: b2Settings.h:26
#define b2_epsilon
Definition: b2Settings.h:39
float32 LengthSquared() const
Definition: b2Math.h:107
T b2Max(T a, T b)
Definition: b2Math.h:642
b2Vec2 center
The position of the shape&#39;s centroid relative to the shape&#39;s origin.
Definition: b2Shape.h:33
float32 m_radius
Definition: b2Shape.h:93
void SetZero()
Set this vector to all zeros.
Definition: b2Math.h:61
A 2D column vector.
Definition: b2Math.h:52
bool Validate() const
INT32 * numerator
GLdouble s
signed int int32
Definition: b2Settings.h:31
float32 fraction
Definition: b2Collision.h:158
float32 b2Cross(const b2Vec2 &a, const b2Vec2 &b)
Perform the cross product on two vectors. In 2D this produces a scalar.
Definition: b2Math.h:411
GLsizei n
void ComputeMass(b2MassData *massData, float32 density) const
float32 mass
The mass of the shape, usually in kilograms.
Definition: b2Shape.h:30
void * Allocate(int32 size)
Allocate memory. This will use b2Alloc if the size is larger than b2_maxBlockSize.
float32 b2DistanceSquared(const b2Vec2 &a, const b2Vec2 &b)
Definition: b2Math.h:472
INT32 INT32 * denominator
b2Shape * Clone(b2BlockAllocator *allocator) const
Implement b2Shape.
bool TestPoint(const b2Transform &transform, const b2Vec2 &p) const
GLint GLint GLint GLint GLint x
float32 maxFraction
Definition: b2Collision.h:150
GLfloat GLfloat p
b2Vec2 m_vertices[b2_maxPolygonVertices]
#define b2_maxPolygonVertices
Definition: b2Settings.h:54
void SetAsBox(float32 hx, float32 hy)
b2Vec2 b2MulT(const b2Mat22 &A, const b2Vec2 &v)
Definition: b2Math.h:439
float32 y
Definition: b2Math.h:139
GLuint index
An axis aligned bounding box.
Definition: b2Collision.h:162
void Set(const b2Vec2 *points, int32 count)
#define b2Assert(A)
Definition: b2Settings.h:27
void ComputeAABB(b2AABB *aabb, const b2Transform &transform, int32 childIndex) const
void Set(float32 angle)
Set using an angle in radians.
Definition: b2Math.h:311
GLdouble GLdouble GLdouble r
const GLdouble * m
GLuint GLuint GLsizei count
GLdouble angle
T b2Min(T a, T b)
Definition: b2Math.h:631
float32 x
Definition: b2Math.h:139
void Set(float32 x_, float32 y_)
Set this vector to some specified coordinates.
Definition: b2Math.h:64
This holds the mass data computed for a shape.
Definition: b2Shape.h:27
b2Vec2 m_normals[b2_maxPolygonVertices]
static b2Vec2 ComputeCentroid(const b2Vec2 *vs, int32 count)
float float32
Definition: b2Settings.h:35
b2Vec2 upperBound
the upper vertex
Definition: b2Collision.h:215
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f


mvsim
Author(s):
autogenerated on Fri May 7 2021 03:05:51