00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 #include <Box2D/Collision/Shapes/b2PolygonShape.h>
00020 #include <new>
00021
00022 b2Shape* b2PolygonShape::Clone(b2BlockAllocator* allocator) const
00023 {
00024 void* mem = allocator->Allocate(sizeof(b2PolygonShape));
00025 b2PolygonShape* clone = new (mem) b2PolygonShape;
00026 *clone = *this;
00027 return clone;
00028 }
00029
00030 void b2PolygonShape::SetAsBox(float32 hx, float32 hy)
00031 {
00032 m_count = 4;
00033 m_vertices[0].Set(-hx, -hy);
00034 m_vertices[1].Set( hx, -hy);
00035 m_vertices[2].Set( hx, hy);
00036 m_vertices[3].Set(-hx, hy);
00037 m_normals[0].Set(0.0f, -1.0f);
00038 m_normals[1].Set(1.0f, 0.0f);
00039 m_normals[2].Set(0.0f, 1.0f);
00040 m_normals[3].Set(-1.0f, 0.0f);
00041 m_centroid.SetZero();
00042 }
00043
00044 void b2PolygonShape::SetAsBox(float32 hx, float32 hy, const b2Vec2& center, float32 angle)
00045 {
00046 m_count = 4;
00047 m_vertices[0].Set(-hx, -hy);
00048 m_vertices[1].Set( hx, -hy);
00049 m_vertices[2].Set( hx, hy);
00050 m_vertices[3].Set(-hx, hy);
00051 m_normals[0].Set(0.0f, -1.0f);
00052 m_normals[1].Set(1.0f, 0.0f);
00053 m_normals[2].Set(0.0f, 1.0f);
00054 m_normals[3].Set(-1.0f, 0.0f);
00055 m_centroid = center;
00056
00057 b2Transform xf;
00058 xf.p = center;
00059 xf.q.Set(angle);
00060
00061
00062 for (int32 i = 0; i < m_count; ++i)
00063 {
00064 m_vertices[i] = b2Mul(xf, m_vertices[i]);
00065 m_normals[i] = b2Mul(xf.q, m_normals[i]);
00066 }
00067 }
00068
00069 int32 b2PolygonShape::GetChildCount() const
00070 {
00071 return 1;
00072 }
00073
00074 static b2Vec2 ComputeCentroid(const b2Vec2* vs, int32 count)
00075 {
00076 b2Assert(count >= 3);
00077
00078 b2Vec2 c; c.Set(0.0f, 0.0f);
00079 float32 area = 0.0f;
00080
00081
00082
00083 b2Vec2 pRef(0.0f, 0.0f);
00084 #if 0
00085
00086 for (int32 i = 0; i < count; ++i)
00087 {
00088 pRef += vs[i];
00089 }
00090 pRef *= 1.0f / count;
00091 #endif
00092
00093 const float32 inv3 = 1.0f / 3.0f;
00094
00095 for (int32 i = 0; i < count; ++i)
00096 {
00097
00098 b2Vec2 p1 = pRef;
00099 b2Vec2 p2 = vs[i];
00100 b2Vec2 p3 = i + 1 < count ? vs[i+1] : vs[0];
00101
00102 b2Vec2 e1 = p2 - p1;
00103 b2Vec2 e2 = p3 - p1;
00104
00105 float32 D = b2Cross(e1, e2);
00106
00107 float32 triangleArea = 0.5f * D;
00108 area += triangleArea;
00109
00110
00111 c += triangleArea * inv3 * (p1 + p2 + p3);
00112 }
00113
00114
00115 b2Assert(area > b2_epsilon);
00116 c *= 1.0f / area;
00117 return c;
00118 }
00119
00120 void b2PolygonShape::Set(const b2Vec2* vertices, int32 count)
00121 {
00122 b2Assert(3 <= count && count <= b2_maxPolygonVertices);
00123 if (count < 3)
00124 {
00125 SetAsBox(1.0f, 1.0f);
00126 return;
00127 }
00128
00129 int32 n = b2Min(count, b2_maxPolygonVertices);
00130
00131
00132 b2Vec2 ps[b2_maxPolygonVertices];
00133 int32 tempCount = 0;
00134 for (int32 i = 0; i < n; ++i)
00135 {
00136 b2Vec2 v = vertices[i];
00137
00138 bool unique = true;
00139 for (int32 j = 0; j < tempCount; ++j)
00140 {
00141 if (b2DistanceSquared(v, ps[j]) < 0.5f * b2_linearSlop)
00142 {
00143 unique = false;
00144 break;
00145 }
00146 }
00147
00148 if (unique)
00149 {
00150 ps[tempCount++] = v;
00151 }
00152 }
00153
00154 n = tempCount;
00155 if (n < 3)
00156 {
00157
00158 b2Assert(false);
00159 SetAsBox(1.0f, 1.0f);
00160 return;
00161 }
00162
00163
00164
00165
00166
00167 int32 i0 = 0;
00168 float32 x0 = ps[0].x;
00169 for (int32 i = 1; i < n; ++i)
00170 {
00171 float32 x = ps[i].x;
00172 if (x > x0 || (x == x0 && ps[i].y < ps[i0].y))
00173 {
00174 i0 = i;
00175 x0 = x;
00176 }
00177 }
00178
00179 int32 hull[b2_maxPolygonVertices];
00180 int32 m = 0;
00181 int32 ih = i0;
00182
00183 for (;;)
00184 {
00185 hull[m] = ih;
00186
00187 int32 ie = 0;
00188 for (int32 j = 1; j < n; ++j)
00189 {
00190 if (ie == ih)
00191 {
00192 ie = j;
00193 continue;
00194 }
00195
00196 b2Vec2 r = ps[ie] - ps[hull[m]];
00197 b2Vec2 v = ps[j] - ps[hull[m]];
00198 float32 c = b2Cross(r, v);
00199 if (c < 0.0f)
00200 {
00201 ie = j;
00202 }
00203
00204
00205 if (c == 0.0f && v.LengthSquared() > r.LengthSquared())
00206 {
00207 ie = j;
00208 }
00209 }
00210
00211 ++m;
00212 ih = ie;
00213
00214 if (ie == i0)
00215 {
00216 break;
00217 }
00218 }
00219
00220 if (m < 3)
00221 {
00222
00223 b2Assert(false);
00224 SetAsBox(1.0f, 1.0f);
00225 return;
00226 }
00227
00228 m_count = m;
00229
00230
00231 for (int32 i = 0; i < m; ++i)
00232 {
00233 m_vertices[i] = ps[hull[i]];
00234 }
00235
00236
00237 for (int32 i = 0; i < m; ++i)
00238 {
00239 int32 i1 = i;
00240 int32 i2 = i + 1 < m ? i + 1 : 0;
00241 b2Vec2 edge = m_vertices[i2] - m_vertices[i1];
00242 b2Assert(edge.LengthSquared() > b2_epsilon * b2_epsilon);
00243 m_normals[i] = b2Cross(edge, 1.0f);
00244 m_normals[i].Normalize();
00245 }
00246
00247
00248 m_centroid = ComputeCentroid(m_vertices, m);
00249 }
00250
00251 bool b2PolygonShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const
00252 {
00253 b2Vec2 pLocal = b2MulT(xf.q, p - xf.p);
00254
00255 for (int32 i = 0; i < m_count; ++i)
00256 {
00257 float32 dot = b2Dot(m_normals[i], pLocal - m_vertices[i]);
00258 if (dot > 0.0f)
00259 {
00260 return false;
00261 }
00262 }
00263
00264 return true;
00265 }
00266
00267 bool b2PolygonShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
00268 const b2Transform& xf, int32 childIndex) const
00269 {
00270 B2_NOT_USED(childIndex);
00271
00272
00273 b2Vec2 p1 = b2MulT(xf.q, input.p1 - xf.p);
00274 b2Vec2 p2 = b2MulT(xf.q, input.p2 - xf.p);
00275 b2Vec2 d = p2 - p1;
00276
00277 float32 lower = 0.0f, upper = input.maxFraction;
00278
00279 int32 index = -1;
00280
00281 for (int32 i = 0; i < m_count; ++i)
00282 {
00283
00284
00285
00286 float32 numerator = b2Dot(m_normals[i], m_vertices[i] - p1);
00287 float32 denominator = b2Dot(m_normals[i], d);
00288
00289 if (denominator == 0.0f)
00290 {
00291 if (numerator < 0.0f)
00292 {
00293 return false;
00294 }
00295 }
00296 else
00297 {
00298
00299
00300
00301
00302 if (denominator < 0.0f && numerator < lower * denominator)
00303 {
00304
00305
00306 lower = numerator / denominator;
00307 index = i;
00308 }
00309 else if (denominator > 0.0f && numerator < upper * denominator)
00310 {
00311
00312
00313 upper = numerator / denominator;
00314 }
00315 }
00316
00317
00318
00319
00320
00321 if (upper < lower)
00322 {
00323 return false;
00324 }
00325 }
00326
00327 b2Assert(0.0f <= lower && lower <= input.maxFraction);
00328
00329 if (index >= 0)
00330 {
00331 output->fraction = lower;
00332 output->normal = b2Mul(xf.q, m_normals[index]);
00333 return true;
00334 }
00335
00336 return false;
00337 }
00338
00339 void b2PolygonShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const
00340 {
00341 B2_NOT_USED(childIndex);
00342
00343 b2Vec2 lower = b2Mul(xf, m_vertices[0]);
00344 b2Vec2 upper = lower;
00345
00346 for (int32 i = 1; i < m_count; ++i)
00347 {
00348 b2Vec2 v = b2Mul(xf, m_vertices[i]);
00349 lower = b2Min(lower, v);
00350 upper = b2Max(upper, v);
00351 }
00352
00353 b2Vec2 r(m_radius, m_radius);
00354 aabb->lowerBound = lower - r;
00355 aabb->upperBound = upper + r;
00356 }
00357
00358 void b2PolygonShape::ComputeMass(b2MassData* massData, float32 density) const
00359 {
00360
00361
00362
00363
00364
00365
00366
00367
00368
00369
00370
00371
00372
00373
00374
00375
00376
00377
00378
00379
00380
00381
00382
00383
00384 b2Assert(m_count >= 3);
00385
00386 b2Vec2 center; center.Set(0.0f, 0.0f);
00387 float32 area = 0.0f;
00388 float32 I = 0.0f;
00389
00390
00391
00392 b2Vec2 s(0.0f, 0.0f);
00393
00394
00395 for (int32 i = 0; i < m_count; ++i)
00396 {
00397 s += m_vertices[i];
00398 }
00399 s *= 1.0f / m_count;
00400
00401 const float32 k_inv3 = 1.0f / 3.0f;
00402
00403 for (int32 i = 0; i < m_count; ++i)
00404 {
00405
00406 b2Vec2 e1 = m_vertices[i] - s;
00407 b2Vec2 e2 = i + 1 < m_count ? m_vertices[i+1] - s : m_vertices[0] - s;
00408
00409 float32 D = b2Cross(e1, e2);
00410
00411 float32 triangleArea = 0.5f * D;
00412 area += triangleArea;
00413
00414
00415 center += triangleArea * k_inv3 * (e1 + e2);
00416
00417 float32 ex1 = e1.x, ey1 = e1.y;
00418 float32 ex2 = e2.x, ey2 = e2.y;
00419
00420 float32 intx2 = ex1*ex1 + ex2*ex1 + ex2*ex2;
00421 float32 inty2 = ey1*ey1 + ey2*ey1 + ey2*ey2;
00422
00423 I += (0.25f * k_inv3 * D) * (intx2 + inty2);
00424 }
00425
00426
00427 massData->mass = density * area;
00428
00429
00430 b2Assert(area > b2_epsilon);
00431 center *= 1.0f / area;
00432 massData->center = center + s;
00433
00434
00435 massData->I = density * I;
00436
00437
00438 massData->I += massData->mass * (b2Dot(massData->center, massData->center) - b2Dot(center, center));
00439 }
00440
00441 bool b2PolygonShape::Validate() const
00442 {
00443 for (int32 i = 0; i < m_count; ++i)
00444 {
00445 int32 i1 = i;
00446 int32 i2 = i < m_count - 1 ? i1 + 1 : 0;
00447 b2Vec2 p = m_vertices[i1];
00448 b2Vec2 e = m_vertices[i2] - p;
00449
00450 for (int32 j = 0; j < m_count; ++j)
00451 {
00452 if (j == i1 || j == i2)
00453 {
00454 continue;
00455 }
00456
00457 b2Vec2 v = m_vertices[j] - p;
00458 float32 c = b2Cross(e, v);
00459 if (c < 0.0f)
00460 {
00461 return false;
00462 }
00463 }
00464 }
00465
00466 return true;
00467 }