b2Distance.cpp
Go to the documentation of this file.
00001 /*
00002 * Copyright (c) 2007-2009 Erin Catto http://www.box2d.org
00003 *
00004 * This software is provided 'as-is', without any express or implied
00005 * warranty.  In no event will the authors be held liable for any damages
00006 * arising from the use of this software.
00007 * Permission is granted to anyone to use this software for any purpose,
00008 * including commercial applications, and to alter it and redistribute it
00009 * freely, subject to the following restrictions:
00010 * 1. The origin of this software must not be misrepresented; you must not
00011 * claim that you wrote the original software. If you use this software
00012 * in a product, an acknowledgment in the product documentation would be
00013 * appreciated but is not required.
00014 * 2. Altered source versions must be plainly marked as such, and must not be
00015 * misrepresented as being the original software.
00016 * 3. This notice may not be removed or altered from any source distribution.
00017 */
00018 
00019 #include <Box2D/Collision/b2Distance.h>
00020 #include <Box2D/Collision/Shapes/b2CircleShape.h>
00021 #include <Box2D/Collision/Shapes/b2EdgeShape.h>
00022 #include <Box2D/Collision/Shapes/b2ChainShape.h>
00023 #include <Box2D/Collision/Shapes/b2PolygonShape.h>
00024 
00025 // GJK using Voronoi regions (Christer Ericson) and Barycentric coordinates.
00026 int32 b2_gjkCalls, b2_gjkIters, b2_gjkMaxIters;
00027 
00028 void b2DistanceProxy::Set(const b2Shape* shape, int32 index)
00029 {
00030         switch (shape->GetType())
00031         {
00032         case b2Shape::e_circle:
00033                 {
00034                         const b2CircleShape* circle = static_cast<const b2CircleShape*>(shape);
00035                         m_vertices = &circle->m_p;
00036                         m_count = 1;
00037                         m_radius = circle->m_radius;
00038                 }
00039                 break;
00040 
00041         case b2Shape::e_polygon:
00042                 {
00043                         const b2PolygonShape* polygon = static_cast<const b2PolygonShape*>(shape);
00044                         m_vertices = polygon->m_vertices;
00045                         m_count = polygon->m_count;
00046                         m_radius = polygon->m_radius;
00047                 }
00048                 break;
00049 
00050         case b2Shape::e_chain:
00051                 {
00052                         const b2ChainShape* chain = static_cast<const b2ChainShape*>(shape);
00053                         b2Assert(0 <= index && index < chain->m_count);
00054 
00055                         m_buffer[0] = chain->m_vertices[index];
00056                         if (index + 1 < chain->m_count)
00057                         {
00058                                 m_buffer[1] = chain->m_vertices[index + 1];
00059                         }
00060                         else
00061                         {
00062                                 m_buffer[1] = chain->m_vertices[0];
00063                         }
00064 
00065                         m_vertices = m_buffer;
00066                         m_count = 2;
00067                         m_radius = chain->m_radius;
00068                 }
00069                 break;
00070 
00071         case b2Shape::e_edge:
00072                 {
00073                         const b2EdgeShape* edge = static_cast<const b2EdgeShape*>(shape);
00074                         m_vertices = &edge->m_vertex1;
00075                         m_count = 2;
00076                         m_radius = edge->m_radius;
00077                 }
00078                 break;
00079 
00080         default:
00081                 b2Assert(false);
00082         }
00083 }
00084 
00085 
00086 struct b2SimplexVertex
00087 {
00088         b2Vec2 wA;              // support point in proxyA
00089         b2Vec2 wB;              // support point in proxyB
00090         b2Vec2 w;               // wB - wA
00091         float32 a;              // barycentric coordinate for closest point
00092         int32 indexA;   // wA index
00093         int32 indexB;   // wB index
00094 };
00095 
00096 struct b2Simplex
00097 {
00098         void ReadCache( const b2SimplexCache* cache,
00099                                         const b2DistanceProxy* proxyA, const b2Transform& transformA,
00100                                         const b2DistanceProxy* proxyB, const b2Transform& transformB)
00101         {
00102                 b2Assert(cache->count <= 3);
00103                 
00104                 // Copy data from cache.
00105                 m_count = cache->count;
00106                 b2SimplexVertex* vertices = &m_v1;
00107                 for (int32 i = 0; i < m_count; ++i)
00108                 {
00109                         b2SimplexVertex* v = vertices + i;
00110                         v->indexA = cache->indexA[i];
00111                         v->indexB = cache->indexB[i];
00112                         b2Vec2 wALocal = proxyA->GetVertex(v->indexA);
00113                         b2Vec2 wBLocal = proxyB->GetVertex(v->indexB);
00114                         v->wA = b2Mul(transformA, wALocal);
00115                         v->wB = b2Mul(transformB, wBLocal);
00116                         v->w = v->wB - v->wA;
00117                         v->a = 0.0f;
00118                 }
00119 
00120                 // Compute the new simplex metric, if it is substantially different than
00121                 // old metric then flush the simplex.
00122                 if (m_count > 1)
00123                 {
00124                         float32 metric1 = cache->metric;
00125                         float32 metric2 = GetMetric();
00126                         if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < b2_epsilon)
00127                         {
00128                                 // Reset the simplex.
00129                                 m_count = 0;
00130                         }
00131                 }
00132 
00133                 // If the cache is empty or invalid ...
00134                 if (m_count == 0)
00135                 {
00136                         b2SimplexVertex* v = vertices + 0;
00137                         v->indexA = 0;
00138                         v->indexB = 0;
00139                         b2Vec2 wALocal = proxyA->GetVertex(0);
00140                         b2Vec2 wBLocal = proxyB->GetVertex(0);
00141                         v->wA = b2Mul(transformA, wALocal);
00142                         v->wB = b2Mul(transformB, wBLocal);
00143                         v->w = v->wB - v->wA;
00144                         v->a = 1.0f;
00145                         m_count = 1;
00146                 }
00147         }
00148 
00149         void WriteCache(b2SimplexCache* cache) const
00150         {
00151                 cache->metric = GetMetric();
00152                 cache->count = uint16(m_count);
00153                 const b2SimplexVertex* vertices = &m_v1;
00154                 for (int32 i = 0; i < m_count; ++i)
00155                 {
00156                         cache->indexA[i] = uint8(vertices[i].indexA);
00157                         cache->indexB[i] = uint8(vertices[i].indexB);
00158                 }
00159         }
00160 
00161         b2Vec2 GetSearchDirection() const
00162         {
00163                 switch (m_count)
00164                 {
00165                 case 1:
00166                         return -m_v1.w;
00167 
00168                 case 2:
00169                         {
00170                                 b2Vec2 e12 = m_v2.w - m_v1.w;
00171                                 float32 sgn = b2Cross(e12, -m_v1.w);
00172                                 if (sgn > 0.0f)
00173                                 {
00174                                         // Origin is left of e12.
00175                                         return b2Cross(1.0f, e12);
00176                                 }
00177                                 else
00178                                 {
00179                                         // Origin is right of e12.
00180                                         return b2Cross(e12, 1.0f);
00181                                 }
00182                         }
00183 
00184                 default:
00185                         b2Assert(false);
00186                         return b2Vec2_zero;
00187                 }
00188         }
00189 
00190         b2Vec2 GetClosestPoint() const
00191         {
00192                 switch (m_count)
00193                 {
00194                 case 0:
00195                         b2Assert(false);
00196                         return b2Vec2_zero;
00197 
00198                 case 1:
00199                         return m_v1.w;
00200 
00201                 case 2:
00202                         return m_v1.a * m_v1.w + m_v2.a * m_v2.w;
00203 
00204                 case 3:
00205                         return b2Vec2_zero;
00206 
00207                 default:
00208                         b2Assert(false);
00209                         return b2Vec2_zero;
00210                 }
00211         }
00212 
00213         void GetWitnessPoints(b2Vec2* pA, b2Vec2* pB) const
00214         {
00215                 switch (m_count)
00216                 {
00217                 case 0:
00218                         b2Assert(false);
00219                         break;
00220 
00221                 case 1:
00222                         *pA = m_v1.wA;
00223                         *pB = m_v1.wB;
00224                         break;
00225 
00226                 case 2:
00227                         *pA = m_v1.a * m_v1.wA + m_v2.a * m_v2.wA;
00228                         *pB = m_v1.a * m_v1.wB + m_v2.a * m_v2.wB;
00229                         break;
00230 
00231                 case 3:
00232                         *pA = m_v1.a * m_v1.wA + m_v2.a * m_v2.wA + m_v3.a * m_v3.wA;
00233                         *pB = *pA;
00234                         break;
00235 
00236                 default:
00237                         b2Assert(false);
00238                         break;
00239                 }
00240         }
00241 
00242         float32 GetMetric() const
00243         {
00244                 switch (m_count)
00245                 {
00246                 case 0:
00247                         b2Assert(false);
00248                         return 0.0f;
00249 
00250                 case 1:
00251                         return 0.0f;
00252 
00253                 case 2:
00254                         return b2Distance(m_v1.w, m_v2.w);
00255 
00256                 case 3:
00257                         return b2Cross(m_v2.w - m_v1.w, m_v3.w - m_v1.w);
00258 
00259                 default:
00260                         b2Assert(false);
00261                         return 0.0f;
00262                 }
00263         }
00264 
00265         void Solve2();
00266         void Solve3();
00267 
00268         b2SimplexVertex m_v1, m_v2, m_v3;
00269         int32 m_count;
00270 };
00271 
00272 
00273 // Solve a line segment using barycentric coordinates.
00274 //
00275 // p = a1 * w1 + a2 * w2
00276 // a1 + a2 = 1
00277 //
00278 // The vector from the origin to the closest point on the line is
00279 // perpendicular to the line.
00280 // e12 = w2 - w1
00281 // dot(p, e) = 0
00282 // a1 * dot(w1, e) + a2 * dot(w2, e) = 0
00283 //
00284 // 2-by-2 linear system
00285 // [1      1     ][a1] = [1]
00286 // [w1.e12 w2.e12][a2] = [0]
00287 //
00288 // Define
00289 // d12_1 =  dot(w2, e12)
00290 // d12_2 = -dot(w1, e12)
00291 // d12 = d12_1 + d12_2
00292 //
00293 // Solution
00294 // a1 = d12_1 / d12
00295 // a2 = d12_2 / d12
00296 void b2Simplex::Solve2()
00297 {
00298         b2Vec2 w1 = m_v1.w;
00299         b2Vec2 w2 = m_v2.w;
00300         b2Vec2 e12 = w2 - w1;
00301 
00302         // w1 region
00303         float32 d12_2 = -b2Dot(w1, e12);
00304         if (d12_2 <= 0.0f)
00305         {
00306                 // a2 <= 0, so we clamp it to 0
00307                 m_v1.a = 1.0f;
00308                 m_count = 1;
00309                 return;
00310         }
00311 
00312         // w2 region
00313         float32 d12_1 = b2Dot(w2, e12);
00314         if (d12_1 <= 0.0f)
00315         {
00316                 // a1 <= 0, so we clamp it to 0
00317                 m_v2.a = 1.0f;
00318                 m_count = 1;
00319                 m_v1 = m_v2;
00320                 return;
00321         }
00322 
00323         // Must be in e12 region.
00324         float32 inv_d12 = 1.0f / (d12_1 + d12_2);
00325         m_v1.a = d12_1 * inv_d12;
00326         m_v2.a = d12_2 * inv_d12;
00327         m_count = 2;
00328 }
00329 
00330 // Possible regions:
00331 // - points[2]
00332 // - edge points[0]-points[2]
00333 // - edge points[1]-points[2]
00334 // - inside the triangle
00335 void b2Simplex::Solve3()
00336 {
00337         b2Vec2 w1 = m_v1.w;
00338         b2Vec2 w2 = m_v2.w;
00339         b2Vec2 w3 = m_v3.w;
00340 
00341         // Edge12
00342         // [1      1     ][a1] = [1]
00343         // [w1.e12 w2.e12][a2] = [0]
00344         // a3 = 0
00345         b2Vec2 e12 = w2 - w1;
00346         float32 w1e12 = b2Dot(w1, e12);
00347         float32 w2e12 = b2Dot(w2, e12);
00348         float32 d12_1 = w2e12;
00349         float32 d12_2 = -w1e12;
00350 
00351         // Edge13
00352         // [1      1     ][a1] = [1]
00353         // [w1.e13 w3.e13][a3] = [0]
00354         // a2 = 0
00355         b2Vec2 e13 = w3 - w1;
00356         float32 w1e13 = b2Dot(w1, e13);
00357         float32 w3e13 = b2Dot(w3, e13);
00358         float32 d13_1 = w3e13;
00359         float32 d13_2 = -w1e13;
00360 
00361         // Edge23
00362         // [1      1     ][a2] = [1]
00363         // [w2.e23 w3.e23][a3] = [0]
00364         // a1 = 0
00365         b2Vec2 e23 = w3 - w2;
00366         float32 w2e23 = b2Dot(w2, e23);
00367         float32 w3e23 = b2Dot(w3, e23);
00368         float32 d23_1 = w3e23;
00369         float32 d23_2 = -w2e23;
00370         
00371         // Triangle123
00372         float32 n123 = b2Cross(e12, e13);
00373 
00374         float32 d123_1 = n123 * b2Cross(w2, w3);
00375         float32 d123_2 = n123 * b2Cross(w3, w1);
00376         float32 d123_3 = n123 * b2Cross(w1, w2);
00377 
00378         // w1 region
00379         if (d12_2 <= 0.0f && d13_2 <= 0.0f)
00380         {
00381                 m_v1.a = 1.0f;
00382                 m_count = 1;
00383                 return;
00384         }
00385 
00386         // e12
00387         if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f)
00388         {
00389                 float32 inv_d12 = 1.0f / (d12_1 + d12_2);
00390                 m_v1.a = d12_1 * inv_d12;
00391                 m_v2.a = d12_2 * inv_d12;
00392                 m_count = 2;
00393                 return;
00394         }
00395 
00396         // e13
00397         if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f)
00398         {
00399                 float32 inv_d13 = 1.0f / (d13_1 + d13_2);
00400                 m_v1.a = d13_1 * inv_d13;
00401                 m_v3.a = d13_2 * inv_d13;
00402                 m_count = 2;
00403                 m_v2 = m_v3;
00404                 return;
00405         }
00406 
00407         // w2 region
00408         if (d12_1 <= 0.0f && d23_2 <= 0.0f)
00409         {
00410                 m_v2.a = 1.0f;
00411                 m_count = 1;
00412                 m_v1 = m_v2;
00413                 return;
00414         }
00415 
00416         // w3 region
00417         if (d13_1 <= 0.0f && d23_1 <= 0.0f)
00418         {
00419                 m_v3.a = 1.0f;
00420                 m_count = 1;
00421                 m_v1 = m_v3;
00422                 return;
00423         }
00424 
00425         // e23
00426         if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f)
00427         {
00428                 float32 inv_d23 = 1.0f / (d23_1 + d23_2);
00429                 m_v2.a = d23_1 * inv_d23;
00430                 m_v3.a = d23_2 * inv_d23;
00431                 m_count = 2;
00432                 m_v1 = m_v3;
00433                 return;
00434         }
00435 
00436         // Must be in triangle123
00437         float32 inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3);
00438         m_v1.a = d123_1 * inv_d123;
00439         m_v2.a = d123_2 * inv_d123;
00440         m_v3.a = d123_3 * inv_d123;
00441         m_count = 3;
00442 }
00443 
00444 void b2Distance(b2DistanceOutput* output,
00445                                 b2SimplexCache* cache,
00446                                 const b2DistanceInput* input)
00447 {
00448         ++b2_gjkCalls;
00449 
00450         const b2DistanceProxy* proxyA = &input->proxyA;
00451         const b2DistanceProxy* proxyB = &input->proxyB;
00452 
00453         b2Transform transformA = input->transformA;
00454         b2Transform transformB = input->transformB;
00455 
00456         // Initialize the simplex.
00457         b2Simplex simplex;
00458         simplex.ReadCache(cache, proxyA, transformA, proxyB, transformB);
00459 
00460         // Get simplex vertices as an array.
00461         b2SimplexVertex* vertices = &simplex.m_v1;
00462         const int32 k_maxIters = 20;
00463 
00464         // These store the vertices of the last simplex so that we
00465         // can check for duplicates and prevent cycling.
00466         int32 saveA[3], saveB[3];
00467         int32 saveCount = 0;
00468 
00469         float32 distanceSqr1 = b2_maxFloat;
00470         float32 distanceSqr2 = distanceSqr1;
00471 
00472         // Main iteration loop.
00473         int32 iter = 0;
00474         while (iter < k_maxIters)
00475         {
00476                 // Copy simplex so we can identify duplicates.
00477                 saveCount = simplex.m_count;
00478                 for (int32 i = 0; i < saveCount; ++i)
00479                 {
00480                         saveA[i] = vertices[i].indexA;
00481                         saveB[i] = vertices[i].indexB;
00482                 }
00483 
00484                 switch (simplex.m_count)
00485                 {
00486                 case 1:
00487                         break;
00488 
00489                 case 2:
00490                         simplex.Solve2();
00491                         break;
00492 
00493                 case 3:
00494                         simplex.Solve3();
00495                         break;
00496 
00497                 default:
00498                         b2Assert(false);
00499                 }
00500 
00501                 // If we have 3 points, then the origin is in the corresponding triangle.
00502                 if (simplex.m_count == 3)
00503                 {
00504                         break;
00505                 }
00506 
00507                 // Compute closest point.
00508                 b2Vec2 p = simplex.GetClosestPoint();
00509                 distanceSqr2 = p.LengthSquared();
00510 
00511                 // Ensure progress
00512                 if (distanceSqr2 >= distanceSqr1)
00513                 {
00514                         //break;
00515                 }
00516                 distanceSqr1 = distanceSqr2;
00517 
00518                 // Get search direction.
00519                 b2Vec2 d = simplex.GetSearchDirection();
00520 
00521                 // Ensure the search direction is numerically fit.
00522                 if (d.LengthSquared() < b2_epsilon * b2_epsilon)
00523                 {
00524                         // The origin is probably contained by a line segment
00525                         // or triangle. Thus the shapes are overlapped.
00526 
00527                         // We can't return zero here even though there may be overlap.
00528                         // In case the simplex is a point, segment, or triangle it is difficult
00529                         // to determine if the origin is contained in the CSO or very close to it.
00530                         break;
00531                 }
00532 
00533                 // Compute a tentative new simplex vertex using support points.
00534                 b2SimplexVertex* vertex = vertices + simplex.m_count;
00535                 vertex->indexA = proxyA->GetSupport(b2MulT(transformA.q, -d));
00536                 vertex->wA = b2Mul(transformA, proxyA->GetVertex(vertex->indexA));
00537                 b2Vec2 wBLocal;
00538                 vertex->indexB = proxyB->GetSupport(b2MulT(transformB.q, d));
00539                 vertex->wB = b2Mul(transformB, proxyB->GetVertex(vertex->indexB));
00540                 vertex->w = vertex->wB - vertex->wA;
00541 
00542                 // Iteration count is equated to the number of support point calls.
00543                 ++iter;
00544                 ++b2_gjkIters;
00545 
00546                 // Check for duplicate support points. This is the main termination criteria.
00547                 bool duplicate = false;
00548                 for (int32 i = 0; i < saveCount; ++i)
00549                 {
00550                         if (vertex->indexA == saveA[i] && vertex->indexB == saveB[i])
00551                         {
00552                                 duplicate = true;
00553                                 break;
00554                         }
00555                 }
00556 
00557                 // If we found a duplicate support point we must exit to avoid cycling.
00558                 if (duplicate)
00559                 {
00560                         break;
00561                 }
00562 
00563                 // New vertex is ok and needed.
00564                 ++simplex.m_count;
00565         }
00566 
00567         b2_gjkMaxIters = b2Max(b2_gjkMaxIters, iter);
00568 
00569         // Prepare output.
00570         simplex.GetWitnessPoints(&output->pointA, &output->pointB);
00571         output->distance = b2Distance(output->pointA, output->pointB);
00572         output->iterations = iter;
00573 
00574         // Cache the simplex.
00575         simplex.WriteCache(cache);
00576 
00577         // Apply radii if requested.
00578         if (input->useRadii)
00579         {
00580                 float32 rA = proxyA->m_radius;
00581                 float32 rB = proxyB->m_radius;
00582 
00583                 if (output->distance > rA + rB && output->distance > b2_epsilon)
00584                 {
00585                         // Shapes are still no overlapped.
00586                         // Move the witness points to the outer surface.
00587                         output->distance -= rA + rB;
00588                         b2Vec2 normal = output->pointB - output->pointA;
00589                         normal.Normalize();
00590                         output->pointA += rA * normal;
00591                         output->pointB -= rB * normal;
00592                 }
00593                 else
00594                 {
00595                         // Shapes are overlapped when radii are considered.
00596                         // Move the witness points to the middle.
00597                         b2Vec2 p = 0.5f * (output->pointA + output->pointB);
00598                         output->pointA = p;
00599                         output->pointB = p;
00600                         output->distance = 0.0f;
00601                 }
00602         }
00603 }


mvsim
Author(s):
autogenerated on Thu Jun 6 2019 22:08:34