File indexing completed on 2024-05-19 14:56:21
0001 /* 0002 * Copyright (c) 2007-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/b2Distance.h> 0020 #include <Box2D/Collision/Shapes/b2CircleShape.h> 0021 #include <Box2D/Collision/Shapes/b2EdgeShape.h> 0022 #include <Box2D/Collision/Shapes/b2ChainShape.h> 0023 #include <Box2D/Collision/Shapes/b2PolygonShape.h> 0024 0025 // GJK using Voronoi regions (Christer Ericson) and Barycentric coordinates. 0026 int32 b2_gjkCalls, b2_gjkIters, b2_gjkMaxIters; 0027 0028 void b2DistanceProxy::Set(const b2Shape* shape, int32 index) 0029 { 0030 switch (shape->GetType()) 0031 { 0032 case b2Shape::e_circle: 0033 { 0034 const b2CircleShape* circle = static_cast<const b2CircleShape*>(shape); 0035 m_vertices = &circle->m_p; 0036 m_count = 1; 0037 m_radius = circle->m_radius; 0038 } 0039 break; 0040 0041 case b2Shape::e_polygon: 0042 { 0043 const b2PolygonShape* polygon = static_cast<const b2PolygonShape*>(shape); 0044 m_vertices = polygon->m_vertices; 0045 m_count = polygon->m_count; 0046 m_radius = polygon->m_radius; 0047 } 0048 break; 0049 0050 case b2Shape::e_chain: 0051 { 0052 const b2ChainShape* chain = static_cast<const b2ChainShape*>(shape); 0053 b2Assert(0 <= index && index < chain->m_count); 0054 0055 m_buffer[0] = chain->m_vertices[index]; 0056 if (index + 1 < chain->m_count) 0057 { 0058 m_buffer[1] = chain->m_vertices[index + 1]; 0059 } 0060 else 0061 { 0062 m_buffer[1] = chain->m_vertices[0]; 0063 } 0064 0065 m_vertices = m_buffer; 0066 m_count = 2; 0067 m_radius = chain->m_radius; 0068 } 0069 break; 0070 0071 case b2Shape::e_edge: 0072 { 0073 const b2EdgeShape* edge = static_cast<const b2EdgeShape*>(shape); 0074 m_vertices = &edge->m_vertex1; 0075 m_count = 2; 0076 m_radius = edge->m_radius; 0077 } 0078 break; 0079 0080 default: 0081 b2Assert(false); 0082 } 0083 } 0084 0085 0086 struct b2SimplexVertex 0087 { 0088 b2Vec2 wA; // support point in proxyA 0089 b2Vec2 wB; // support point in proxyB 0090 b2Vec2 w; // wB - wA 0091 float32 a; // barycentric coordinate for closest point 0092 int32 indexA; // wA index 0093 int32 indexB; // wB index 0094 }; 0095 0096 struct b2Simplex 0097 { 0098 void ReadCache( const b2SimplexCache* cache, 0099 const b2DistanceProxy* proxyA, const b2Transform& transformA, 0100 const b2DistanceProxy* proxyB, const b2Transform& transformB) 0101 { 0102 b2Assert(cache->count <= 3); 0103 0104 // Copy data from cache. 0105 m_count = cache->count; 0106 b2SimplexVertex* vertices = &m_v1; 0107 for (int32 i = 0; i < m_count; ++i) 0108 { 0109 b2SimplexVertex* v = vertices + i; 0110 v->indexA = cache->indexA[i]; 0111 v->indexB = cache->indexB[i]; 0112 b2Vec2 wALocal = proxyA->GetVertex(v->indexA); 0113 b2Vec2 wBLocal = proxyB->GetVertex(v->indexB); 0114 v->wA = b2Mul(transformA, wALocal); 0115 v->wB = b2Mul(transformB, wBLocal); 0116 v->w = v->wB - v->wA; 0117 v->a = 0.0f; 0118 } 0119 0120 // Compute the new simplex metric, if it is substantially different than 0121 // old metric then flush the simplex. 0122 if (m_count > 1) 0123 { 0124 float32 metric1 = cache->metric; 0125 float32 metric2 = GetMetric(); 0126 if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < b2_epsilon) 0127 { 0128 // Reset the simplex. 0129 m_count = 0; 0130 } 0131 } 0132 0133 // If the cache is empty or invalid ... 0134 if (m_count == 0) 0135 { 0136 b2SimplexVertex* v = vertices + 0; 0137 v->indexA = 0; 0138 v->indexB = 0; 0139 b2Vec2 wALocal = proxyA->GetVertex(0); 0140 b2Vec2 wBLocal = proxyB->GetVertex(0); 0141 v->wA = b2Mul(transformA, wALocal); 0142 v->wB = b2Mul(transformB, wBLocal); 0143 v->w = v->wB - v->wA; 0144 v->a = 1.0f; 0145 m_count = 1; 0146 } 0147 } 0148 0149 void WriteCache(b2SimplexCache* cache) const 0150 { 0151 cache->metric = GetMetric(); 0152 cache->count = uint16(m_count); 0153 const b2SimplexVertex* vertices = &m_v1; 0154 for (int32 i = 0; i < m_count; ++i) 0155 { 0156 cache->indexA[i] = uint8(vertices[i].indexA); 0157 cache->indexB[i] = uint8(vertices[i].indexB); 0158 } 0159 } 0160 0161 b2Vec2 GetSearchDirection() const 0162 { 0163 switch (m_count) 0164 { 0165 case 1: 0166 return -m_v1.w; 0167 0168 case 2: 0169 { 0170 b2Vec2 e12 = m_v2.w - m_v1.w; 0171 float32 sgn = b2Cross(e12, -m_v1.w); 0172 if (sgn > 0.0f) 0173 { 0174 // Origin is left of e12. 0175 return b2Cross(1.0f, e12); 0176 } 0177 else 0178 { 0179 // Origin is right of e12. 0180 return b2Cross(e12, 1.0f); 0181 } 0182 } 0183 0184 default: 0185 b2Assert(false); 0186 return b2Vec2_zero; 0187 } 0188 } 0189 0190 b2Vec2 GetClosestPoint() const 0191 { 0192 switch (m_count) 0193 { 0194 case 0: 0195 b2Assert(false); 0196 return b2Vec2_zero; 0197 0198 case 1: 0199 return m_v1.w; 0200 0201 case 2: 0202 return m_v1.a * m_v1.w + m_v2.a * m_v2.w; 0203 0204 case 3: 0205 return b2Vec2_zero; 0206 0207 default: 0208 b2Assert(false); 0209 return b2Vec2_zero; 0210 } 0211 } 0212 0213 void GetWitnessPoints(b2Vec2* pA, b2Vec2* pB) const 0214 { 0215 switch (m_count) 0216 { 0217 case 0: 0218 b2Assert(false); 0219 break; 0220 0221 case 1: 0222 *pA = m_v1.wA; 0223 *pB = m_v1.wB; 0224 break; 0225 0226 case 2: 0227 *pA = m_v1.a * m_v1.wA + m_v2.a * m_v2.wA; 0228 *pB = m_v1.a * m_v1.wB + m_v2.a * m_v2.wB; 0229 break; 0230 0231 case 3: 0232 *pA = m_v1.a * m_v1.wA + m_v2.a * m_v2.wA + m_v3.a * m_v3.wA; 0233 *pB = *pA; 0234 break; 0235 0236 default: 0237 b2Assert(false); 0238 break; 0239 } 0240 } 0241 0242 float32 GetMetric() const 0243 { 0244 switch (m_count) 0245 { 0246 case 0: 0247 b2Assert(false); 0248 return 0.0f; 0249 0250 case 1: 0251 return 0.0f; 0252 0253 case 2: 0254 return b2Distance(m_v1.w, m_v2.w); 0255 0256 case 3: 0257 return b2Cross(m_v2.w - m_v1.w, m_v3.w - m_v1.w); 0258 0259 default: 0260 b2Assert(false); 0261 return 0.0f; 0262 } 0263 } 0264 0265 void Solve2(); 0266 void Solve3(); 0267 0268 b2SimplexVertex m_v1, m_v2, m_v3; 0269 int32 m_count; 0270 }; 0271 0272 0273 // Solve a line segment using barycentric coordinates. 0274 // 0275 // p = a1 * w1 + a2 * w2 0276 // a1 + a2 = 1 0277 // 0278 // The vector from the origin to the closest point on the line is 0279 // perpendicular to the line. 0280 // e12 = w2 - w1 0281 // dot(p, e) = 0 0282 // a1 * dot(w1, e) + a2 * dot(w2, e) = 0 0283 // 0284 // 2-by-2 linear system 0285 // [1 1 ][a1] = [1] 0286 // [w1.e12 w2.e12][a2] = [0] 0287 // 0288 // Define 0289 // d12_1 = dot(w2, e12) 0290 // d12_2 = -dot(w1, e12) 0291 // d12 = d12_1 + d12_2 0292 // 0293 // Solution 0294 // a1 = d12_1 / d12 0295 // a2 = d12_2 / d12 0296 void b2Simplex::Solve2() 0297 { 0298 b2Vec2 w1 = m_v1.w; 0299 b2Vec2 w2 = m_v2.w; 0300 b2Vec2 e12 = w2 - w1; 0301 0302 // w1 region 0303 float32 d12_2 = -b2Dot(w1, e12); 0304 if (d12_2 <= 0.0f) 0305 { 0306 // a2 <= 0, so we clamp it to 0 0307 m_v1.a = 1.0f; 0308 m_count = 1; 0309 return; 0310 } 0311 0312 // w2 region 0313 float32 d12_1 = b2Dot(w2, e12); 0314 if (d12_1 <= 0.0f) 0315 { 0316 // a1 <= 0, so we clamp it to 0 0317 m_v2.a = 1.0f; 0318 m_count = 1; 0319 m_v1 = m_v2; 0320 return; 0321 } 0322 0323 // Must be in e12 region. 0324 float32 inv_d12 = 1.0f / (d12_1 + d12_2); 0325 m_v1.a = d12_1 * inv_d12; 0326 m_v2.a = d12_2 * inv_d12; 0327 m_count = 2; 0328 } 0329 0330 // Possible regions: 0331 // - points[2] 0332 // - edge points[0]-points[2] 0333 // - edge points[1]-points[2] 0334 // - inside the triangle 0335 void b2Simplex::Solve3() 0336 { 0337 b2Vec2 w1 = m_v1.w; 0338 b2Vec2 w2 = m_v2.w; 0339 b2Vec2 w3 = m_v3.w; 0340 0341 // Edge12 0342 // [1 1 ][a1] = [1] 0343 // [w1.e12 w2.e12][a2] = [0] 0344 // a3 = 0 0345 b2Vec2 e12 = w2 - w1; 0346 float32 w1e12 = b2Dot(w1, e12); 0347 float32 w2e12 = b2Dot(w2, e12); 0348 float32 d12_1 = w2e12; 0349 float32 d12_2 = -w1e12; 0350 0351 // Edge13 0352 // [1 1 ][a1] = [1] 0353 // [w1.e13 w3.e13][a3] = [0] 0354 // a2 = 0 0355 b2Vec2 e13 = w3 - w1; 0356 float32 w1e13 = b2Dot(w1, e13); 0357 float32 w3e13 = b2Dot(w3, e13); 0358 float32 d13_1 = w3e13; 0359 float32 d13_2 = -w1e13; 0360 0361 // Edge23 0362 // [1 1 ][a2] = [1] 0363 // [w2.e23 w3.e23][a3] = [0] 0364 // a1 = 0 0365 b2Vec2 e23 = w3 - w2; 0366 float32 w2e23 = b2Dot(w2, e23); 0367 float32 w3e23 = b2Dot(w3, e23); 0368 float32 d23_1 = w3e23; 0369 float32 d23_2 = -w2e23; 0370 0371 // Triangle123 0372 float32 n123 = b2Cross(e12, e13); 0373 0374 float32 d123_1 = n123 * b2Cross(w2, w3); 0375 float32 d123_2 = n123 * b2Cross(w3, w1); 0376 float32 d123_3 = n123 * b2Cross(w1, w2); 0377 0378 // w1 region 0379 if (d12_2 <= 0.0f && d13_2 <= 0.0f) 0380 { 0381 m_v1.a = 1.0f; 0382 m_count = 1; 0383 return; 0384 } 0385 0386 // e12 0387 if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f) 0388 { 0389 float32 inv_d12 = 1.0f / (d12_1 + d12_2); 0390 m_v1.a = d12_1 * inv_d12; 0391 m_v2.a = d12_2 * inv_d12; 0392 m_count = 2; 0393 return; 0394 } 0395 0396 // e13 0397 if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f) 0398 { 0399 float32 inv_d13 = 1.0f / (d13_1 + d13_2); 0400 m_v1.a = d13_1 * inv_d13; 0401 m_v3.a = d13_2 * inv_d13; 0402 m_count = 2; 0403 m_v2 = m_v3; 0404 return; 0405 } 0406 0407 // w2 region 0408 if (d12_1 <= 0.0f && d23_2 <= 0.0f) 0409 { 0410 m_v2.a = 1.0f; 0411 m_count = 1; 0412 m_v1 = m_v2; 0413 return; 0414 } 0415 0416 // w3 region 0417 if (d13_1 <= 0.0f && d23_1 <= 0.0f) 0418 { 0419 m_v3.a = 1.0f; 0420 m_count = 1; 0421 m_v1 = m_v3; 0422 return; 0423 } 0424 0425 // e23 0426 if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f) 0427 { 0428 float32 inv_d23 = 1.0f / (d23_1 + d23_2); 0429 m_v2.a = d23_1 * inv_d23; 0430 m_v3.a = d23_2 * inv_d23; 0431 m_count = 2; 0432 m_v1 = m_v3; 0433 return; 0434 } 0435 0436 // Must be in triangle123 0437 float32 inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3); 0438 m_v1.a = d123_1 * inv_d123; 0439 m_v2.a = d123_2 * inv_d123; 0440 m_v3.a = d123_3 * inv_d123; 0441 m_count = 3; 0442 } 0443 0444 void b2Distance(b2DistanceOutput* output, 0445 b2SimplexCache* cache, 0446 const b2DistanceInput* input) 0447 { 0448 ++b2_gjkCalls; 0449 0450 const b2DistanceProxy* proxyA = &input->proxyA; 0451 const b2DistanceProxy* proxyB = &input->proxyB; 0452 0453 b2Transform transformA = input->transformA; 0454 b2Transform transformB = input->transformB; 0455 0456 // Initialize the simplex. 0457 b2Simplex simplex; 0458 simplex.ReadCache(cache, proxyA, transformA, proxyB, transformB); 0459 0460 // Get simplex vertices as an array. 0461 b2SimplexVertex* vertices = &simplex.m_v1; 0462 const int32 k_maxIters = 20; 0463 0464 // These store the vertices of the last simplex so that we 0465 // can check for duplicates and prevent cycling. 0466 int32 saveA[3], saveB[3]; 0467 int32 saveCount = 0; 0468 0469 float32 distanceSqr1 = b2_maxFloat; 0470 float32 distanceSqr2 = distanceSqr1; 0471 0472 // Main iteration loop. 0473 int32 iter = 0; 0474 while (iter < k_maxIters) 0475 { 0476 // Copy simplex so we can identify duplicates. 0477 saveCount = simplex.m_count; 0478 for (int32 i = 0; i < saveCount; ++i) 0479 { 0480 saveA[i] = vertices[i].indexA; 0481 saveB[i] = vertices[i].indexB; 0482 } 0483 0484 switch (simplex.m_count) 0485 { 0486 case 1: 0487 break; 0488 0489 case 2: 0490 simplex.Solve2(); 0491 break; 0492 0493 case 3: 0494 simplex.Solve3(); 0495 break; 0496 0497 default: 0498 b2Assert(false); 0499 } 0500 0501 // If we have 3 points, then the origin is in the corresponding triangle. 0502 if (simplex.m_count == 3) 0503 { 0504 break; 0505 } 0506 0507 // Compute closest point. 0508 b2Vec2 p = simplex.GetClosestPoint(); 0509 distanceSqr2 = p.LengthSquared(); 0510 0511 // Ensure progress 0512 if (distanceSqr2 >= distanceSqr1) 0513 { 0514 //break; 0515 } 0516 distanceSqr1 = distanceSqr2; 0517 0518 // Get search direction. 0519 b2Vec2 d = simplex.GetSearchDirection(); 0520 0521 // Ensure the search direction is numerically fit. 0522 if (d.LengthSquared() < b2_epsilon * b2_epsilon) 0523 { 0524 // The origin is probably contained by a line segment 0525 // or triangle. Thus the shapes are overlapped. 0526 0527 // We can't return zero here even though there may be overlap. 0528 // In case the simplex is a point, segment, or triangle it is difficult 0529 // to determine if the origin is contained in the CSO or very close to it. 0530 break; 0531 } 0532 0533 // Compute a tentative new simplex vertex using support points. 0534 b2SimplexVertex* vertex = vertices + simplex.m_count; 0535 vertex->indexA = proxyA->GetSupport(b2MulT(transformA.q, -d)); 0536 vertex->wA = b2Mul(transformA, proxyA->GetVertex(vertex->indexA)); 0537 b2Vec2 wBLocal; 0538 vertex->indexB = proxyB->GetSupport(b2MulT(transformB.q, d)); 0539 vertex->wB = b2Mul(transformB, proxyB->GetVertex(vertex->indexB)); 0540 vertex->w = vertex->wB - vertex->wA; 0541 0542 // Iteration count is equated to the number of support point calls. 0543 ++iter; 0544 ++b2_gjkIters; 0545 0546 // Check for duplicate support points. This is the main termination criteria. 0547 bool duplicate = false; 0548 for (int32 i = 0; i < saveCount; ++i) 0549 { 0550 if (vertex->indexA == saveA[i] && vertex->indexB == saveB[i]) 0551 { 0552 duplicate = true; 0553 break; 0554 } 0555 } 0556 0557 // If we found a duplicate support point we must exit to avoid cycling. 0558 if (duplicate) 0559 { 0560 break; 0561 } 0562 0563 // New vertex is ok and needed. 0564 ++simplex.m_count; 0565 } 0566 0567 b2_gjkMaxIters = b2Max(b2_gjkMaxIters, iter); 0568 0569 // Prepare output. 0570 simplex.GetWitnessPoints(&output->pointA, &output->pointB); 0571 output->distance = b2Distance(output->pointA, output->pointB); 0572 output->iterations = iter; 0573 0574 // Cache the simplex. 0575 simplex.WriteCache(cache); 0576 0577 // Apply radii if requested. 0578 if (input->useRadii) 0579 { 0580 float32 rA = proxyA->m_radius; 0581 float32 rB = proxyB->m_radius; 0582 0583 if (output->distance > rA + rB && output->distance > b2_epsilon) 0584 { 0585 // Shapes are still no overlapped. 0586 // Move the witness points to the outer surface. 0587 output->distance -= rA + rB; 0588 b2Vec2 normal = output->pointB - output->pointA; 0589 normal.Normalize(); 0590 output->pointA += rA * normal; 0591 output->pointB -= rB * normal; 0592 } 0593 else 0594 { 0595 // Shapes are overlapped when radii are considered. 0596 // Move the witness points to the middle. 0597 b2Vec2 p = 0.5f * (output->pointA + output->pointB); 0598 output->pointA = p; 0599 output->pointB = p; 0600 output->distance = 0.0f; 0601 } 0602 } 0603 }