git.maemo.org Git - blok/blob - Box2D/Source/Collision/b2TimeOfImpact.cpp

   1 /*
   2 * Copyright (c) 2007 Erin Catto http://www.gphysics.com
   3 *
   4 * This software is provided 'as-is', without any express or implied
   5 * warranty.  In no event will the authors be held liable for any damages
   6 * arising from the use of this software.
   7 * Permission is granted to anyone to use this software for any purpose,
   8 * including commercial applications, and to alter it and redistribute it
   9 * freely, subject to the following restrictions:
  10 * 1. The origin of this software must not be misrepresented; you must not
  11 * claim that you wrote the original software. If you use this software
  12 * in a product, an acknowledgment in the product documentation would be
  13 * appreciated but is not required.
  14 * 2. Altered source versions must be plainly marked as such, and must not be
  15 * misrepresented as being the original software.
  16 * 3. This notice may not be removed or altered from any source distribution.
  17 */
  18
  19 #include "b2Collision.h"
  20 #include "Shapes/b2Shape.h"
  21
  22 // This algorithm uses conservative advancement to compute the time of
  23 // impact (TOI) of two shapes.
  24 // Refs: Bullet, Young Kim
  25 float32 b2TimeOfImpact(const b2Shape* shape1, const b2Sweep& sweep1,
  26                                            const b2Shape* shape2, const b2Sweep& sweep2)
  27 {
  28         float32 r1 = shape1->GetSweepRadius();
  29         float32 r2 = shape2->GetSweepRadius();
  30
  31         b2Assert(sweep1.t0 == sweep2.t0);
  32         b2Assert(1.0f - sweep1.t0 > B2_FLT_EPSILON);
  33
  34         float32 t0 = sweep1.t0;
  35         b2Vec2 v1 = sweep1.c - sweep1.c0;
  36         b2Vec2 v2 = sweep2.c - sweep2.c0;
  37         float32 omega1 = sweep1.a - sweep1.a0;
  38         float32 omega2 = sweep2.a - sweep2.a0;
  39
  40         float32 alpha = 0.0f;
  41
  42         b2Vec2 p1, p2;
  43         const int32 k_maxIterations = 20;       // TODO_ERIN b2Settings
  44         int32 iter = 0;
  45         b2Vec2 normal = b2Vec2_zero;
  46         float32 distance = 0.0f;
  47         float32 targetDistance = 0.0f;
  48         for(;;)
  49         {
  50                 float32 t = (1.0f - alpha) * t0 + alpha;
  51                 b2XForm xf1, xf2;
  52                 sweep1.GetXForm(&xf1, t);
  53                 sweep2.GetXForm(&xf2, t);
  54
  55                 // Get the distance between shapes.
  56                 distance = b2Distance(&p1, &p2, shape1, xf1, shape2, xf2);
  57
  58                 if (iter == 0)
  59                 {
  60                         // Compute a reasonable target distance to give some breathing room
  61                         // for conservative advancement.
  62                         if (distance > 2.0f * b2_toiSlop)
  63                         {
  64                                 targetDistance = 1.5f * b2_toiSlop;
  65                         }
  66                         else
  67                         {
  68                                 targetDistance = b2Max(0.05f * b2_toiSlop, distance - 0.5f * b2_toiSlop);
  69                         }
  70                 }
  71
  72                 if (distance - targetDistance < 0.05f * b2_toiSlop || iter == k_maxIterations)
  73                 {
  74                         break;
  75                 }
  76
  77                 normal = p2 - p1;
  78                 normal.Normalize();
  79
  80                 // Compute upper bound on remaining movement.
  81                 float32 approachVelocityBound = b2Dot(normal, v1 - v2) + b2Abs(omega1) * r1 + b2Abs(omega2) * r2;
  82                 if (b2Abs(approachVelocityBound) < B2_FLT_EPSILON)
  83                 {
  84                         alpha = 1.0f;
  85                         break;
  86                 }
  87
  88                 // Get the conservative time increment. Don't advance all the way.
  89                 float32 dAlpha = (distance - targetDistance) / approachVelocityBound;
  90                 //float32 dt = (distance - 0.5f * b2_linearSlop) / approachVelocityBound;
  91                 float32 newAlpha = alpha + dAlpha;
  92
  93                 // The shapes may be moving apart or a safe distance apart.
  94                 if (newAlpha < 0.0f || 1.0f < newAlpha)
  95                 {
  96                         alpha = 1.0f;
  97                         break;
  98                 }
  99
 100                 // Ensure significant advancement.
 101                 if (newAlpha < (1.0f + 100.0f * B2_FLT_EPSILON) * alpha)
 102                 {
 103                         break;
 104                 }
 105
 106                 alpha = newAlpha;
 107
 108                 ++iter;
 109         }
 110
 111         return alpha;
 112 }