b2TimeOfImpact.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/b2Collision.h>
00020 #include <Box2D/Collision/b2Distance.h>
00021 #include <Box2D/Collision/b2TimeOfImpact.h>
00022 #include <Box2D/Collision/Shapes/b2CircleShape.h>
00023 #include <Box2D/Collision/Shapes/b2PolygonShape.h>
00024 #include <Box2D/Common/b2Timer.h>
00025 
00026 #include <stdio.h>
00027 
00028 float32 b2_toiTime, b2_toiMaxTime;
00029 int32 b2_toiCalls, b2_toiIters, b2_toiMaxIters;
00030 int32 b2_toiRootIters, b2_toiMaxRootIters;
00031 
00032 //
00033 struct b2SeparationFunction
00034 {
00035         enum Type
00036         {
00037                 e_points,
00038                 e_faceA,
00039                 e_faceB
00040         };
00041 
00042         // TODO_ERIN might not need to return the separation
00043 
00044         float32 Initialize(const b2SimplexCache* cache,
00045                 const b2DistanceProxy* proxyA, const b2Sweep& sweepA,
00046                 const b2DistanceProxy* proxyB, const b2Sweep& sweepB,
00047                 float32 t1)
00048         {
00049                 m_proxyA = proxyA;
00050                 m_proxyB = proxyB;
00051                 int32 count = cache->count;
00052                 b2Assert(0 < count && count < 3);
00053 
00054                 m_sweepA = sweepA;
00055                 m_sweepB = sweepB;
00056 
00057                 b2Transform xfA, xfB;
00058                 m_sweepA.GetTransform(&xfA, t1);
00059                 m_sweepB.GetTransform(&xfB, t1);
00060 
00061                 if (count == 1)
00062                 {
00063                         m_type = e_points;
00064                         b2Vec2 localPointA = m_proxyA->GetVertex(cache->indexA[0]);
00065                         b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]);
00066                         b2Vec2 pointA = b2Mul(xfA, localPointA);
00067                         b2Vec2 pointB = b2Mul(xfB, localPointB);
00068                         m_axis = pointB - pointA;
00069                         float32 s = m_axis.Normalize();
00070                         return s;
00071                 }
00072                 else if (cache->indexA[0] == cache->indexA[1])
00073                 {
00074                         // Two points on B and one on A.
00075                         m_type = e_faceB;
00076                         b2Vec2 localPointB1 = proxyB->GetVertex(cache->indexB[0]);
00077                         b2Vec2 localPointB2 = proxyB->GetVertex(cache->indexB[1]);
00078 
00079                         m_axis = b2Cross(localPointB2 - localPointB1, 1.0f);
00080                         m_axis.Normalize();
00081                         b2Vec2 normal = b2Mul(xfB.q, m_axis);
00082 
00083                         m_localPoint = 0.5f * (localPointB1 + localPointB2);
00084                         b2Vec2 pointB = b2Mul(xfB, m_localPoint);
00085 
00086                         b2Vec2 localPointA = proxyA->GetVertex(cache->indexA[0]);
00087                         b2Vec2 pointA = b2Mul(xfA, localPointA);
00088 
00089                         float32 s = b2Dot(pointA - pointB, normal);
00090                         if (s < 0.0f)
00091                         {
00092                                 m_axis = -m_axis;
00093                                 s = -s;
00094                         }
00095                         return s;
00096                 }
00097                 else
00098                 {
00099                         // Two points on A and one or two points on B.
00100                         m_type = e_faceA;
00101                         b2Vec2 localPointA1 = m_proxyA->GetVertex(cache->indexA[0]);
00102                         b2Vec2 localPointA2 = m_proxyA->GetVertex(cache->indexA[1]);
00103                         
00104                         m_axis = b2Cross(localPointA2 - localPointA1, 1.0f);
00105                         m_axis.Normalize();
00106                         b2Vec2 normal = b2Mul(xfA.q, m_axis);
00107 
00108                         m_localPoint = 0.5f * (localPointA1 + localPointA2);
00109                         b2Vec2 pointA = b2Mul(xfA, m_localPoint);
00110 
00111                         b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]);
00112                         b2Vec2 pointB = b2Mul(xfB, localPointB);
00113 
00114                         float32 s = b2Dot(pointB - pointA, normal);
00115                         if (s < 0.0f)
00116                         {
00117                                 m_axis = -m_axis;
00118                                 s = -s;
00119                         }
00120                         return s;
00121                 }
00122         }
00123 
00124         //
00125         float32 FindMinSeparation(int32* indexA, int32* indexB, float32 t) const
00126         {
00127                 b2Transform xfA, xfB;
00128                 m_sweepA.GetTransform(&xfA, t);
00129                 m_sweepB.GetTransform(&xfB, t);
00130 
00131                 switch (m_type)
00132                 {
00133                 case e_points:
00134                         {
00135                                 b2Vec2 axisA = b2MulT(xfA.q,  m_axis);
00136                                 b2Vec2 axisB = b2MulT(xfB.q, -m_axis);
00137 
00138                                 *indexA = m_proxyA->GetSupport(axisA);
00139                                 *indexB = m_proxyB->GetSupport(axisB);
00140 
00141                                 b2Vec2 localPointA = m_proxyA->GetVertex(*indexA);
00142                                 b2Vec2 localPointB = m_proxyB->GetVertex(*indexB);
00143                                 
00144                                 b2Vec2 pointA = b2Mul(xfA, localPointA);
00145                                 b2Vec2 pointB = b2Mul(xfB, localPointB);
00146 
00147                                 float32 separation = b2Dot(pointB - pointA, m_axis);
00148                                 return separation;
00149                         }
00150 
00151                 case e_faceA:
00152                         {
00153                                 b2Vec2 normal = b2Mul(xfA.q, m_axis);
00154                                 b2Vec2 pointA = b2Mul(xfA, m_localPoint);
00155 
00156                                 b2Vec2 axisB = b2MulT(xfB.q, -normal);
00157                                 
00158                                 *indexA = -1;
00159                                 *indexB = m_proxyB->GetSupport(axisB);
00160 
00161                                 b2Vec2 localPointB = m_proxyB->GetVertex(*indexB);
00162                                 b2Vec2 pointB = b2Mul(xfB, localPointB);
00163 
00164                                 float32 separation = b2Dot(pointB - pointA, normal);
00165                                 return separation;
00166                         }
00167 
00168                 case e_faceB:
00169                         {
00170                                 b2Vec2 normal = b2Mul(xfB.q, m_axis);
00171                                 b2Vec2 pointB = b2Mul(xfB, m_localPoint);
00172 
00173                                 b2Vec2 axisA = b2MulT(xfA.q, -normal);
00174 
00175                                 *indexB = -1;
00176                                 *indexA = m_proxyA->GetSupport(axisA);
00177 
00178                                 b2Vec2 localPointA = m_proxyA->GetVertex(*indexA);
00179                                 b2Vec2 pointA = b2Mul(xfA, localPointA);
00180 
00181                                 float32 separation = b2Dot(pointA - pointB, normal);
00182                                 return separation;
00183                         }
00184 
00185                 default:
00186                         b2Assert(false);
00187                         *indexA = -1;
00188                         *indexB = -1;
00189                         return 0.0f;
00190                 }
00191         }
00192 
00193         //
00194         float32 Evaluate(int32 indexA, int32 indexB, float32 t) const
00195         {
00196                 b2Transform xfA, xfB;
00197                 m_sweepA.GetTransform(&xfA, t);
00198                 m_sweepB.GetTransform(&xfB, t);
00199 
00200                 switch (m_type)
00201                 {
00202                 case e_points:
00203                         {
00204                                 b2Vec2 localPointA = m_proxyA->GetVertex(indexA);
00205                                 b2Vec2 localPointB = m_proxyB->GetVertex(indexB);
00206 
00207                                 b2Vec2 pointA = b2Mul(xfA, localPointA);
00208                                 b2Vec2 pointB = b2Mul(xfB, localPointB);
00209                                 float32 separation = b2Dot(pointB - pointA, m_axis);
00210 
00211                                 return separation;
00212                         }
00213 
00214                 case e_faceA:
00215                         {
00216                                 b2Vec2 normal = b2Mul(xfA.q, m_axis);
00217                                 b2Vec2 pointA = b2Mul(xfA, m_localPoint);
00218 
00219                                 b2Vec2 localPointB = m_proxyB->GetVertex(indexB);
00220                                 b2Vec2 pointB = b2Mul(xfB, localPointB);
00221 
00222                                 float32 separation = b2Dot(pointB - pointA, normal);
00223                                 return separation;
00224                         }
00225 
00226                 case e_faceB:
00227                         {
00228                                 b2Vec2 normal = b2Mul(xfB.q, m_axis);
00229                                 b2Vec2 pointB = b2Mul(xfB, m_localPoint);
00230 
00231                                 b2Vec2 localPointA = m_proxyA->GetVertex(indexA);
00232                                 b2Vec2 pointA = b2Mul(xfA, localPointA);
00233 
00234                                 float32 separation = b2Dot(pointA - pointB, normal);
00235                                 return separation;
00236                         }
00237 
00238                 default:
00239                         b2Assert(false);
00240                         return 0.0f;
00241                 }
00242         }
00243 
00244         const b2DistanceProxy* m_proxyA;
00245         const b2DistanceProxy* m_proxyB;
00246         b2Sweep m_sweepA, m_sweepB;
00247         Type m_type;
00248         b2Vec2 m_localPoint;
00249         b2Vec2 m_axis;
00250 };
00251 
00252 // CCD via the local separating axis method. This seeks progression
00253 // by computing the largest time at which separation is maintained.
00254 void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input)
00255 {
00256         b2Timer timer;
00257 
00258         ++b2_toiCalls;
00259 
00260         output->state = b2TOIOutput::e_unknown;
00261         output->t = input->tMax;
00262 
00263         const b2DistanceProxy* proxyA = &input->proxyA;
00264         const b2DistanceProxy* proxyB = &input->proxyB;
00265 
00266         b2Sweep sweepA = input->sweepA;
00267         b2Sweep sweepB = input->sweepB;
00268 
00269         // Large rotations can make the root finder fail, so we normalize the
00270         // sweep angles.
00271         sweepA.Normalize();
00272         sweepB.Normalize();
00273 
00274         float32 tMax = input->tMax;
00275 
00276         float32 totalRadius = proxyA->m_radius + proxyB->m_radius;
00277         float32 target = b2Max(b2_linearSlop, totalRadius - 3.0f * b2_linearSlop);
00278         float32 tolerance = 0.25f * b2_linearSlop;
00279         b2Assert(target > tolerance);
00280 
00281         float32 t1 = 0.0f;
00282         const int32 k_maxIterations = 20;       // TODO_ERIN b2Settings
00283         int32 iter = 0;
00284 
00285         // Prepare input for distance query.
00286         b2SimplexCache cache;
00287         cache.count = 0;
00288         b2DistanceInput distanceInput;
00289         distanceInput.proxyA = input->proxyA;
00290         distanceInput.proxyB = input->proxyB;
00291         distanceInput.useRadii = false;
00292 
00293         // The outer loop progressively attempts to compute new separating axes.
00294         // This loop terminates when an axis is repeated (no progress is made).
00295         for(;;)
00296         {
00297                 b2Transform xfA, xfB;
00298                 sweepA.GetTransform(&xfA, t1);
00299                 sweepB.GetTransform(&xfB, t1);
00300 
00301                 // Get the distance between shapes. We can also use the results
00302                 // to get a separating axis.
00303                 distanceInput.transformA = xfA;
00304                 distanceInput.transformB = xfB;
00305                 b2DistanceOutput distanceOutput;
00306                 b2Distance(&distanceOutput, &cache, &distanceInput);
00307 
00308                 // If the shapes are overlapped, we give up on continuous collision.
00309                 if (distanceOutput.distance <= 0.0f)
00310                 {
00311                         // Failure!
00312                         output->state = b2TOIOutput::e_overlapped;
00313                         output->t = 0.0f;
00314                         break;
00315                 }
00316 
00317                 if (distanceOutput.distance < target + tolerance)
00318                 {
00319                         // Victory!
00320                         output->state = b2TOIOutput::e_touching;
00321                         output->t = t1;
00322                         break;
00323                 }
00324 
00325                 // Initialize the separating axis.
00326                 b2SeparationFunction fcn;
00327                 fcn.Initialize(&cache, proxyA, sweepA, proxyB, sweepB, t1);
00328 #if 0
00329                 // Dump the curve seen by the root finder
00330                 {
00331                         const int32 N = 100;
00332                         float32 dx = 1.0f / N;
00333                         float32 xs[N+1];
00334                         float32 fs[N+1];
00335 
00336                         float32 x = 0.0f;
00337 
00338                         for (int32 i = 0; i <= N; ++i)
00339                         {
00340                                 sweepA.GetTransform(&xfA, x);
00341                                 sweepB.GetTransform(&xfB, x);
00342                                 float32 f = fcn.Evaluate(xfA, xfB) - target;
00343 
00344                                 printf("%g %g\n", x, f);
00345 
00346                                 xs[i] = x;
00347                                 fs[i] = f;
00348 
00349                                 x += dx;
00350                         }
00351                 }
00352 #endif
00353 
00354                 // Compute the TOI on the separating axis. We do this by successively
00355                 // resolving the deepest point. This loop is bounded by the number of vertices.
00356                 bool done = false;
00357                 float32 t2 = tMax;
00358                 int32 pushBackIter = 0;
00359                 for (;;)
00360                 {
00361                         // Find the deepest point at t2. Store the witness point indices.
00362                         int32 indexA, indexB;
00363                         float32 s2 = fcn.FindMinSeparation(&indexA, &indexB, t2);
00364 
00365                         // Is the final configuration separated?
00366                         if (s2 > target + tolerance)
00367                         {
00368                                 // Victory!
00369                                 output->state = b2TOIOutput::e_separated;
00370                                 output->t = tMax;
00371                                 done = true;
00372                                 break;
00373                         }
00374 
00375                         // Has the separation reached tolerance?
00376                         if (s2 > target - tolerance)
00377                         {
00378                                 // Advance the sweeps
00379                                 t1 = t2;
00380                                 break;
00381                         }
00382 
00383                         // Compute the initial separation of the witness points.
00384                         float32 s1 = fcn.Evaluate(indexA, indexB, t1);
00385 
00386                         // Check for initial overlap. This might happen if the root finder
00387                         // runs out of iterations.
00388                         if (s1 < target - tolerance)
00389                         {
00390                                 output->state = b2TOIOutput::e_failed;
00391                                 output->t = t1;
00392                                 done = true;
00393                                 break;
00394                         }
00395 
00396                         // Check for touching
00397                         if (s1 <= target + tolerance)
00398                         {
00399                                 // Victory! t1 should hold the TOI (could be 0.0).
00400                                 output->state = b2TOIOutput::e_touching;
00401                                 output->t = t1;
00402                                 done = true;
00403                                 break;
00404                         }
00405 
00406                         // Compute 1D root of: f(x) - target = 0
00407                         int32 rootIterCount = 0;
00408                         float32 a1 = t1, a2 = t2;
00409                         for (;;)
00410                         {
00411                                 // Use a mix of the secant rule and bisection.
00412                                 float32 t;
00413                                 if (rootIterCount & 1)
00414                                 {
00415                                         // Secant rule to improve convergence.
00416                                         t = a1 + (target - s1) * (a2 - a1) / (s2 - s1);
00417                                 }
00418                                 else
00419                                 {
00420                                         // Bisection to guarantee progress.
00421                                         t = 0.5f * (a1 + a2);
00422                                 }
00423 
00424                                 ++rootIterCount;
00425                                 ++b2_toiRootIters;
00426 
00427                                 float32 s = fcn.Evaluate(indexA, indexB, t);
00428 
00429                                 if (b2Abs(s - target) < tolerance)
00430                                 {
00431                                         // t2 holds a tentative value for t1
00432                                         t2 = t;
00433                                         break;
00434                                 }
00435 
00436                                 // Ensure we continue to bracket the root.
00437                                 if (s > target)
00438                                 {
00439                                         a1 = t;
00440                                         s1 = s;
00441                                 }
00442                                 else
00443                                 {
00444                                         a2 = t;
00445                                         s2 = s;
00446                                 }
00447                                 
00448                                 if (rootIterCount == 50)
00449                                 {
00450                                         break;
00451                                 }
00452                         }
00453 
00454                         b2_toiMaxRootIters = b2Max(b2_toiMaxRootIters, rootIterCount);
00455 
00456                         ++pushBackIter;
00457 
00458                         if (pushBackIter == b2_maxPolygonVertices)
00459                         {
00460                                 break;
00461                         }
00462                 }
00463 
00464                 ++iter;
00465                 ++b2_toiIters;
00466 
00467                 if (done)
00468                 {
00469                         break;
00470                 }
00471 
00472                 if (iter == k_maxIterations)
00473                 {
00474                         // Root finder got stuck. Semi-victory.
00475                         output->state = b2TOIOutput::e_failed;
00476                         output->t = t1;
00477                         break;
00478                 }
00479         }
00480 
00481         b2_toiMaxIters = b2Max(b2_toiMaxIters, iter);
00482 
00483         float32 time = timer.GetMilliseconds();
00484         b2_toiMaxTime = b2Max(b2_toiMaxTime, time);
00485         b2_toiTime += time;
00486 }


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