b2TimeOfImpact.cpp 3.2 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112
  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. #include "b2Collision.h"
  19. #include "Shapes/b2Shape.h"
  20. // This algorithm uses conservative advancement to compute the time of
  21. // impact (TOI) of two shapes.
  22. // Refs: Bullet, Young Kim
  23. float32 b2TimeOfImpact(const b2Shape* shape1, const b2Sweep& sweep1,
  24. const b2Shape* shape2, const b2Sweep& sweep2)
  25. {
  26. float32 r1 = shape1->GetSweepRadius();
  27. float32 r2 = shape2->GetSweepRadius();
  28. b2Assert(sweep1.t0 == sweep2.t0);
  29. b2Assert(1.0f - sweep1.t0 > B2_FLT_EPSILON);
  30. float32 t0 = sweep1.t0;
  31. b2Vec2 v1 = sweep1.c - sweep1.c0;
  32. b2Vec2 v2 = sweep2.c - sweep2.c0;
  33. float32 omega1 = sweep1.a - sweep1.a0;
  34. float32 omega2 = sweep2.a - sweep2.a0;
  35. float32 alpha = 0.0f;
  36. b2Vec2 p1, p2;
  37. const int32 k_maxIterations = 20; // TODO_ERIN b2Settings
  38. int32 iter = 0;
  39. b2Vec2 normal = b2Vec2_zero;
  40. float32 distance = 0.0f;
  41. float32 targetDistance = 0.0f;
  42. for(;;)
  43. {
  44. float32 t = (1.0f - alpha) * t0 + alpha;
  45. b2XForm xf1, xf2;
  46. sweep1.GetXForm(&xf1, t);
  47. sweep2.GetXForm(&xf2, t);
  48. // Get the distance between shapes.
  49. distance = b2Distance(&p1, &p2, shape1, xf1, shape2, xf2);
  50. if (iter == 0)
  51. {
  52. // Compute a reasonable target distance to give some breathing room
  53. // for conservative advancement.
  54. if (distance > 2.0f * b2_toiSlop)
  55. {
  56. targetDistance = 1.5f * b2_toiSlop;
  57. }
  58. else
  59. {
  60. targetDistance = b2Max(0.05f * b2_toiSlop, distance - 0.5f * b2_toiSlop);
  61. }
  62. }
  63. if (distance - targetDistance < 0.05f * b2_toiSlop || iter == k_maxIterations)
  64. {
  65. break;
  66. }
  67. normal = p2 - p1;
  68. normal.Normalize();
  69. // Compute upper bound on remaining movement.
  70. float32 approachVelocityBound = b2Dot(normal, v1 - v2) + b2Abs(omega1) * r1 + b2Abs(omega2) * r2;
  71. if (b2Abs(approachVelocityBound) < B2_FLT_EPSILON)
  72. {
  73. alpha = 1.0f;
  74. break;
  75. }
  76. // Get the conservative time increment. Don't advance all the way.
  77. float32 dAlpha = (distance - targetDistance) / approachVelocityBound;
  78. //float32 dt = (distance - 0.5f * b2_linearSlop) / approachVelocityBound;
  79. float32 newAlpha = alpha + dAlpha;
  80. // The shapes may be moving apart or a safe distance apart.
  81. if (newAlpha < 0.0f || 1.0f < newAlpha)
  82. {
  83. alpha = 1.0f;
  84. break;
  85. }
  86. // Ensure significant advancement.
  87. if (newAlpha < (1.0f + 100.0f * B2_FLT_EPSILON) * alpha)
  88. {
  89. break;
  90. }
  91. alpha = newAlpha;
  92. ++iter;
  93. }
  94. return alpha;
  95. }