File indexing completed on 2024-06-02 03:42:36
0001 /* 0002 * Copyright (c) 2006-2009 Erin Catto http://www.box2d.org 0003 * 0004 * This software is provided 'as-is', without any express or implied 0005 * warranty. In no event will the authors be held liable for any damages 0006 * arising from the use of this software. 0007 * Permission is granted to anyone to use this software for any purpose, 0008 * including commercial applications, and to alter it and redistribute it 0009 * freely, subject to the following restrictions: 0010 * 1. The origin of this software must not be misrepresented; you must not 0011 * claim that you wrote the original software. If you use this software 0012 * in a product, an acknowledgment in the product documentation would be 0013 * appreciated but is not required. 0014 * 2. Altered source versions must be plainly marked as such, and must not be 0015 * misrepresented as being the original software. 0016 * 3. This notice may not be removed or altered from any source distribution. 0017 */ 0018 0019 #include <Box2D/Collision/Shapes/b2PolygonShape.h> 0020 #include <new> 0021 0022 b2Shape* b2PolygonShape::Clone(b2BlockAllocator* allocator) const 0023 { 0024 void* mem = allocator->Allocate(sizeof(b2PolygonShape)); 0025 b2PolygonShape* clone = new (mem) b2PolygonShape; 0026 *clone = *this; 0027 return clone; 0028 } 0029 0030 void b2PolygonShape::SetAsBox(float32 hx, float32 hy) 0031 { 0032 m_count = 4; 0033 m_vertices[0].Set(-hx, -hy); 0034 m_vertices[1].Set( hx, -hy); 0035 m_vertices[2].Set( hx, hy); 0036 m_vertices[3].Set(-hx, hy); 0037 m_normals[0].Set(0.0f, -1.0f); 0038 m_normals[1].Set(1.0f, 0.0f); 0039 m_normals[2].Set(0.0f, 1.0f); 0040 m_normals[3].Set(-1.0f, 0.0f); 0041 m_centroid.SetZero(); 0042 } 0043 0044 void b2PolygonShape::SetAsBox(float32 hx, float32 hy, const b2Vec2& center, float32 angle) 0045 { 0046 m_count = 4; 0047 m_vertices[0].Set(-hx, -hy); 0048 m_vertices[1].Set( hx, -hy); 0049 m_vertices[2].Set( hx, hy); 0050 m_vertices[3].Set(-hx, hy); 0051 m_normals[0].Set(0.0f, -1.0f); 0052 m_normals[1].Set(1.0f, 0.0f); 0053 m_normals[2].Set(0.0f, 1.0f); 0054 m_normals[3].Set(-1.0f, 0.0f); 0055 m_centroid = center; 0056 0057 b2Transform xf; 0058 xf.p = center; 0059 xf.q.Set(angle); 0060 0061 // Transform vertices and normals. 0062 for (int32 i = 0; i < m_count; ++i) 0063 { 0064 m_vertices[i] = b2Mul(xf, m_vertices[i]); 0065 m_normals[i] = b2Mul(xf.q, m_normals[i]); 0066 } 0067 } 0068 0069 int32 b2PolygonShape::GetChildCount() const 0070 { 0071 return 1; 0072 } 0073 0074 static b2Vec2 ComputeCentroid(const b2Vec2* vs, int32 count) 0075 { 0076 b2Assert(count >= 3); 0077 0078 b2Vec2 c; c.Set(0.0f, 0.0f); 0079 float32 area = 0.0f; 0080 0081 // pRef is the reference point for forming triangles. 0082 // It's location doesn't change the result (except for rounding error). 0083 b2Vec2 pRef(0.0f, 0.0f); 0084 #if 0 0085 // This code would put the reference point inside the polygon. 0086 for (int32 i = 0; i < count; ++i) 0087 { 0088 pRef += vs[i]; 0089 } 0090 pRef *= 1.0f / count; 0091 #endif 0092 0093 const float32 inv3 = 1.0f / 3.0f; 0094 0095 for (int32 i = 0; i < count; ++i) 0096 { 0097 // Triangle vertices. 0098 b2Vec2 p1 = pRef; 0099 b2Vec2 p2 = vs[i]; 0100 b2Vec2 p3 = i + 1 < count ? vs[i+1] : vs[0]; 0101 0102 b2Vec2 e1 = p2 - p1; 0103 b2Vec2 e2 = p3 - p1; 0104 0105 float32 D = b2Cross(e1, e2); 0106 0107 float32 triangleArea = 0.5f * D; 0108 area += triangleArea; 0109 0110 // Area weighted centroid 0111 c += triangleArea * inv3 * (p1 + p2 + p3); 0112 } 0113 0114 // Centroid 0115 b2Assert(area > b2_epsilon); 0116 c *= 1.0f / area; 0117 return c; 0118 } 0119 0120 void b2PolygonShape::Set(const b2Vec2* vertices, int32 count) 0121 { 0122 b2Assert(3 <= count && count <= b2_maxPolygonVertices); 0123 if (count < 3) 0124 { 0125 SetAsBox(1.0f, 1.0f); 0126 return; 0127 } 0128 0129 int32 n = b2Min(count, b2_maxPolygonVertices); 0130 0131 // Perform welding and copy vertices into local buffer. 0132 b2Vec2 ps[b2_maxPolygonVertices]; 0133 int32 tempCount = 0; 0134 for (int32 i = 0; i < n; ++i) 0135 { 0136 b2Vec2 v = vertices[i]; 0137 0138 bool unique = true; 0139 for (int32 j = 0; j < tempCount; ++j) 0140 { 0141 if (b2DistanceSquared(v, ps[j]) < 0.5f * b2_linearSlop) 0142 { 0143 unique = false; 0144 break; 0145 } 0146 } 0147 0148 if (unique) 0149 { 0150 ps[tempCount++] = v; 0151 } 0152 } 0153 0154 n = tempCount; 0155 if (n < 3) 0156 { 0157 // Polygon is degenerate. 0158 b2Assert(false); 0159 SetAsBox(1.0f, 1.0f); 0160 return; 0161 } 0162 0163 // Create the convex hull using the Gift wrapping algorithm 0164 // http://en.wikipedia.org/wiki/Gift_wrapping_algorithm 0165 0166 // Find the right most point on the hull 0167 int32 i0 = 0; 0168 float32 x0 = ps[0].x; 0169 for (int32 i = 1; i < n; ++i) 0170 { 0171 float32 x = ps[i].x; 0172 if (x > x0 || (x == x0 && ps[i].y < ps[i0].y)) 0173 { 0174 i0 = i; 0175 x0 = x; 0176 } 0177 } 0178 0179 int32 hull[b2_maxPolygonVertices]; 0180 int32 m = 0; 0181 int32 ih = i0; 0182 0183 for (;;) 0184 { 0185 hull[m] = ih; 0186 0187 int32 ie = 0; 0188 for (int32 j = 1; j < n; ++j) 0189 { 0190 if (ie == ih) 0191 { 0192 ie = j; 0193 continue; 0194 } 0195 0196 b2Vec2 r = ps[ie] - ps[hull[m]]; 0197 b2Vec2 v = ps[j] - ps[hull[m]]; 0198 float32 c = b2Cross(r, v); 0199 if (c < 0.0f) 0200 { 0201 ie = j; 0202 } 0203 0204 // Collinearity check 0205 if (c == 0.0f && v.LengthSquared() > r.LengthSquared()) 0206 { 0207 ie = j; 0208 } 0209 } 0210 0211 ++m; 0212 ih = ie; 0213 0214 if (ie == i0) 0215 { 0216 break; 0217 } 0218 } 0219 0220 if (m < 3) 0221 { 0222 // Polygon is degenerate. 0223 b2Assert(false); 0224 SetAsBox(1.0f, 1.0f); 0225 return; 0226 } 0227 0228 m_count = m; 0229 0230 // Copy vertices. 0231 for (int32 i = 0; i < m; ++i) 0232 { 0233 m_vertices[i] = ps[hull[i]]; 0234 } 0235 0236 // Compute normals. Ensure the edges have non-zero length. 0237 for (int32 i = 0; i < m; ++i) 0238 { 0239 int32 i1 = i; 0240 int32 i2 = i + 1 < m ? i + 1 : 0; 0241 b2Vec2 edge = m_vertices[i2] - m_vertices[i1]; 0242 b2Assert(edge.LengthSquared() > b2_epsilon * b2_epsilon); 0243 m_normals[i] = b2Cross(edge, 1.0f); 0244 m_normals[i].Normalize(); 0245 } 0246 0247 // Compute the polygon centroid. 0248 m_centroid = ComputeCentroid(m_vertices, m); 0249 } 0250 0251 bool b2PolygonShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const 0252 { 0253 b2Vec2 pLocal = b2MulT(xf.q, p - xf.p); 0254 0255 for (int32 i = 0; i < m_count; ++i) 0256 { 0257 float32 dot = b2Dot(m_normals[i], pLocal - m_vertices[i]); 0258 if (dot > 0.0f) 0259 { 0260 return false; 0261 } 0262 } 0263 0264 return true; 0265 } 0266 0267 bool b2PolygonShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, 0268 const b2Transform& xf, int32 childIndex) const 0269 { 0270 B2_NOT_USED(childIndex); 0271 0272 // Put the ray into the polygon's frame of reference. 0273 b2Vec2 p1 = b2MulT(xf.q, input.p1 - xf.p); 0274 b2Vec2 p2 = b2MulT(xf.q, input.p2 - xf.p); 0275 b2Vec2 d = p2 - p1; 0276 0277 float32 lower = 0.0f, upper = input.maxFraction; 0278 0279 int32 index = -1; 0280 0281 for (int32 i = 0; i < m_count; ++i) 0282 { 0283 // p = p1 + a * d 0284 // dot(normal, p - v) = 0 0285 // dot(normal, p1 - v) + a * dot(normal, d) = 0 0286 float32 numerator = b2Dot(m_normals[i], m_vertices[i] - p1); 0287 float32 denominator = b2Dot(m_normals[i], d); 0288 0289 if (denominator == 0.0f) 0290 { 0291 if (numerator < 0.0f) 0292 { 0293 return false; 0294 } 0295 } 0296 else 0297 { 0298 // Note: we want this predicate without division: 0299 // lower < numerator / denominator, where denominator < 0 0300 // Since denominator < 0, we have to flip the inequality: 0301 // lower < numerator / denominator <==> denominator * lower > numerator. 0302 if (denominator < 0.0f && numerator < lower * denominator) 0303 { 0304 // Increase lower. 0305 // The segment enters this half-space. 0306 lower = numerator / denominator; 0307 index = i; 0308 } 0309 else if (denominator > 0.0f && numerator < upper * denominator) 0310 { 0311 // Decrease upper. 0312 // The segment exits this half-space. 0313 upper = numerator / denominator; 0314 } 0315 } 0316 0317 // The use of epsilon here causes the assert on lower to trip 0318 // in some cases. Apparently the use of epsilon was to make edge 0319 // shapes work, but now those are handled separately. 0320 //if (upper < lower - b2_epsilon) 0321 if (upper < lower) 0322 { 0323 return false; 0324 } 0325 } 0326 0327 b2Assert(0.0f <= lower && lower <= input.maxFraction); 0328 0329 if (index >= 0) 0330 { 0331 output->fraction = lower; 0332 output->normal = b2Mul(xf.q, m_normals[index]); 0333 return true; 0334 } 0335 0336 return false; 0337 } 0338 0339 void b2PolygonShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const 0340 { 0341 B2_NOT_USED(childIndex); 0342 0343 b2Vec2 lower = b2Mul(xf, m_vertices[0]); 0344 b2Vec2 upper = lower; 0345 0346 for (int32 i = 1; i < m_count; ++i) 0347 { 0348 b2Vec2 v = b2Mul(xf, m_vertices[i]); 0349 lower = b2Min(lower, v); 0350 upper = b2Max(upper, v); 0351 } 0352 0353 b2Vec2 r(m_radius, m_radius); 0354 aabb->lowerBound = lower - r; 0355 aabb->upperBound = upper + r; 0356 } 0357 0358 void b2PolygonShape::ComputeMass(b2MassData* massData, float32 density) const 0359 { 0360 // Polygon mass, centroid, and inertia. 0361 // Let rho be the polygon density in mass per unit area. 0362 // Then: 0363 // mass = rho * int(dA) 0364 // centroid.x = (1/mass) * rho * int(x * dA) 0365 // centroid.y = (1/mass) * rho * int(y * dA) 0366 // I = rho * int((x*x + y*y) * dA) 0367 // 0368 // We can compute these integrals by summing all the integrals 0369 // for each triangle of the polygon. To evaluate the integral 0370 // for a single triangle, we make a change of variables to 0371 // the (u,v) coordinates of the triangle: 0372 // x = x0 + e1x * u + e2x * v 0373 // y = y0 + e1y * u + e2y * v 0374 // where 0 <= u && 0 <= v && u + v <= 1. 0375 // 0376 // We integrate u from [0,1-v] and then v from [0,1]. 0377 // We also need to use the Jacobian of the transformation: 0378 // D = cross(e1, e2) 0379 // 0380 // Simplification: triangle centroid = (1/3) * (p1 + p2 + p3) 0381 // 0382 // The rest of the derivation is handled by computer algebra. 0383 0384 b2Assert(m_count >= 3); 0385 0386 b2Vec2 center; center.Set(0.0f, 0.0f); 0387 float32 area = 0.0f; 0388 float32 I = 0.0f; 0389 0390 // s is the reference point for forming triangles. 0391 // It's location doesn't change the result (except for rounding error). 0392 b2Vec2 s(0.0f, 0.0f); 0393 0394 // This code would put the reference point inside the polygon. 0395 for (int32 i = 0; i < m_count; ++i) 0396 { 0397 s += m_vertices[i]; 0398 } 0399 s *= 1.0f / m_count; 0400 0401 const float32 k_inv3 = 1.0f / 3.0f; 0402 0403 for (int32 i = 0; i < m_count; ++i) 0404 { 0405 // Triangle vertices. 0406 b2Vec2 e1 = m_vertices[i] - s; 0407 b2Vec2 e2 = i + 1 < m_count ? m_vertices[i+1] - s : m_vertices[0] - s; 0408 0409 float32 D = b2Cross(e1, e2); 0410 0411 float32 triangleArea = 0.5f * D; 0412 area += triangleArea; 0413 0414 // Area weighted centroid 0415 center += triangleArea * k_inv3 * (e1 + e2); 0416 0417 float32 ex1 = e1.x, ey1 = e1.y; 0418 float32 ex2 = e2.x, ey2 = e2.y; 0419 0420 float32 intx2 = ex1*ex1 + ex2*ex1 + ex2*ex2; 0421 float32 inty2 = ey1*ey1 + ey2*ey1 + ey2*ey2; 0422 0423 I += (0.25f * k_inv3 * D) * (intx2 + inty2); 0424 } 0425 0426 // Total mass 0427 massData->mass = density * area; 0428 0429 // Center of mass 0430 b2Assert(area > b2_epsilon); 0431 center *= 1.0f / area; 0432 massData->center = center + s; 0433 0434 // Inertia tensor relative to the local origin (point s). 0435 massData->I = density * I; 0436 0437 // Shift to center of mass then to original body origin. 0438 massData->I += massData->mass * (b2Dot(massData->center, massData->center) - b2Dot(center, center)); 0439 } 0440 0441 bool b2PolygonShape::Validate() const 0442 { 0443 for (int32 i = 0; i < m_count; ++i) 0444 { 0445 int32 i1 = i; 0446 int32 i2 = i < m_count - 1 ? i1 + 1 : 0; 0447 b2Vec2 p = m_vertices[i1]; 0448 b2Vec2 e = m_vertices[i2] - p; 0449 0450 for (int32 j = 0; j < m_count; ++j) 0451 { 0452 if (j == i1 || j == i2) 0453 { 0454 continue; 0455 } 0456 0457 b2Vec2 v = m_vertices[j] - p; 0458 float32 c = b2Cross(e, v); 0459 if (c < 0.0f) 0460 { 0461 return false; 0462 } 0463 } 0464 } 0465 0466 return true; 0467 }