Warning, file /education/gcompris/external/qml-box2d/Box2D/Collision/b2TimeOfImpact.cpp was not indexed or was modified since last indexation (in which case cross-reference links may be missing, inaccurate or erroneous).
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/b2Collision.h> 0020 #include <Box2D/Collision/b2Distance.h> 0021 #include <Box2D/Collision/b2TimeOfImpact.h> 0022 #include <Box2D/Collision/Shapes/b2CircleShape.h> 0023 #include <Box2D/Collision/Shapes/b2PolygonShape.h> 0024 #include <Box2D/Common/b2Timer.h> 0025 0026 #include <stdio.h> 0027 0028 float32 b2_toiTime, b2_toiMaxTime; 0029 int32 b2_toiCalls, b2_toiIters, b2_toiMaxIters; 0030 int32 b2_toiRootIters, b2_toiMaxRootIters; 0031 0032 // 0033 struct b2SeparationFunction 0034 { 0035 enum Type 0036 { 0037 e_points, 0038 e_faceA, 0039 e_faceB 0040 }; 0041 0042 // TODO_ERIN might not need to return the separation 0043 0044 float32 Initialize(const b2SimplexCache* cache, 0045 const b2DistanceProxy* proxyA, const b2Sweep& sweepA, 0046 const b2DistanceProxy* proxyB, const b2Sweep& sweepB, 0047 float32 t1) 0048 { 0049 m_proxyA = proxyA; 0050 m_proxyB = proxyB; 0051 int32 count = cache->count; 0052 b2Assert(0 < count && count < 3); 0053 0054 m_sweepA = sweepA; 0055 m_sweepB = sweepB; 0056 0057 b2Transform xfA, xfB; 0058 m_sweepA.GetTransform(&xfA, t1); 0059 m_sweepB.GetTransform(&xfB, t1); 0060 0061 if (count == 1) 0062 { 0063 m_type = e_points; 0064 b2Vec2 localPointA = m_proxyA->GetVertex(cache->indexA[0]); 0065 b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]); 0066 b2Vec2 pointA = b2Mul(xfA, localPointA); 0067 b2Vec2 pointB = b2Mul(xfB, localPointB); 0068 m_axis = pointB - pointA; 0069 float32 s = m_axis.Normalize(); 0070 return s; 0071 } 0072 else if (cache->indexA[0] == cache->indexA[1]) 0073 { 0074 // Two points on B and one on A. 0075 m_type = e_faceB; 0076 b2Vec2 localPointB1 = proxyB->GetVertex(cache->indexB[0]); 0077 b2Vec2 localPointB2 = proxyB->GetVertex(cache->indexB[1]); 0078 0079 m_axis = b2Cross(localPointB2 - localPointB1, 1.0f); 0080 m_axis.Normalize(); 0081 b2Vec2 normal = b2Mul(xfB.q, m_axis); 0082 0083 m_localPoint = 0.5f * (localPointB1 + localPointB2); 0084 b2Vec2 pointB = b2Mul(xfB, m_localPoint); 0085 0086 b2Vec2 localPointA = proxyA->GetVertex(cache->indexA[0]); 0087 b2Vec2 pointA = b2Mul(xfA, localPointA); 0088 0089 float32 s = b2Dot(pointA - pointB, normal); 0090 if (s < 0.0f) 0091 { 0092 m_axis = -m_axis; 0093 s = -s; 0094 } 0095 return s; 0096 } 0097 else 0098 { 0099 // Two points on A and one or two points on B. 0100 m_type = e_faceA; 0101 b2Vec2 localPointA1 = m_proxyA->GetVertex(cache->indexA[0]); 0102 b2Vec2 localPointA2 = m_proxyA->GetVertex(cache->indexA[1]); 0103 0104 m_axis = b2Cross(localPointA2 - localPointA1, 1.0f); 0105 m_axis.Normalize(); 0106 b2Vec2 normal = b2Mul(xfA.q, m_axis); 0107 0108 m_localPoint = 0.5f * (localPointA1 + localPointA2); 0109 b2Vec2 pointA = b2Mul(xfA, m_localPoint); 0110 0111 b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]); 0112 b2Vec2 pointB = b2Mul(xfB, localPointB); 0113 0114 float32 s = b2Dot(pointB - pointA, normal); 0115 if (s < 0.0f) 0116 { 0117 m_axis = -m_axis; 0118 s = -s; 0119 } 0120 return s; 0121 } 0122 } 0123 0124 // 0125 float32 FindMinSeparation(int32* indexA, int32* indexB, float32 t) const 0126 { 0127 b2Transform xfA, xfB; 0128 m_sweepA.GetTransform(&xfA, t); 0129 m_sweepB.GetTransform(&xfB, t); 0130 0131 switch (m_type) 0132 { 0133 case e_points: 0134 { 0135 b2Vec2 axisA = b2MulT(xfA.q, m_axis); 0136 b2Vec2 axisB = b2MulT(xfB.q, -m_axis); 0137 0138 *indexA = m_proxyA->GetSupport(axisA); 0139 *indexB = m_proxyB->GetSupport(axisB); 0140 0141 b2Vec2 localPointA = m_proxyA->GetVertex(*indexA); 0142 b2Vec2 localPointB = m_proxyB->GetVertex(*indexB); 0143 0144 b2Vec2 pointA = b2Mul(xfA, localPointA); 0145 b2Vec2 pointB = b2Mul(xfB, localPointB); 0146 0147 float32 separation = b2Dot(pointB - pointA, m_axis); 0148 return separation; 0149 } 0150 0151 case e_faceA: 0152 { 0153 b2Vec2 normal = b2Mul(xfA.q, m_axis); 0154 b2Vec2 pointA = b2Mul(xfA, m_localPoint); 0155 0156 b2Vec2 axisB = b2MulT(xfB.q, -normal); 0157 0158 *indexA = -1; 0159 *indexB = m_proxyB->GetSupport(axisB); 0160 0161 b2Vec2 localPointB = m_proxyB->GetVertex(*indexB); 0162 b2Vec2 pointB = b2Mul(xfB, localPointB); 0163 0164 float32 separation = b2Dot(pointB - pointA, normal); 0165 return separation; 0166 } 0167 0168 case e_faceB: 0169 { 0170 b2Vec2 normal = b2Mul(xfB.q, m_axis); 0171 b2Vec2 pointB = b2Mul(xfB, m_localPoint); 0172 0173 b2Vec2 axisA = b2MulT(xfA.q, -normal); 0174 0175 *indexB = -1; 0176 *indexA = m_proxyA->GetSupport(axisA); 0177 0178 b2Vec2 localPointA = m_proxyA->GetVertex(*indexA); 0179 b2Vec2 pointA = b2Mul(xfA, localPointA); 0180 0181 float32 separation = b2Dot(pointA - pointB, normal); 0182 return separation; 0183 } 0184 0185 default: 0186 b2Assert(false); 0187 *indexA = -1; 0188 *indexB = -1; 0189 return 0.0f; 0190 } 0191 } 0192 0193 // 0194 float32 Evaluate(int32 indexA, int32 indexB, float32 t) const 0195 { 0196 b2Transform xfA, xfB; 0197 m_sweepA.GetTransform(&xfA, t); 0198 m_sweepB.GetTransform(&xfB, t); 0199 0200 switch (m_type) 0201 { 0202 case e_points: 0203 { 0204 b2Vec2 localPointA = m_proxyA->GetVertex(indexA); 0205 b2Vec2 localPointB = m_proxyB->GetVertex(indexB); 0206 0207 b2Vec2 pointA = b2Mul(xfA, localPointA); 0208 b2Vec2 pointB = b2Mul(xfB, localPointB); 0209 float32 separation = b2Dot(pointB - pointA, m_axis); 0210 0211 return separation; 0212 } 0213 0214 case e_faceA: 0215 { 0216 b2Vec2 normal = b2Mul(xfA.q, m_axis); 0217 b2Vec2 pointA = b2Mul(xfA, m_localPoint); 0218 0219 b2Vec2 localPointB = m_proxyB->GetVertex(indexB); 0220 b2Vec2 pointB = b2Mul(xfB, localPointB); 0221 0222 float32 separation = b2Dot(pointB - pointA, normal); 0223 return separation; 0224 } 0225 0226 case e_faceB: 0227 { 0228 b2Vec2 normal = b2Mul(xfB.q, m_axis); 0229 b2Vec2 pointB = b2Mul(xfB, m_localPoint); 0230 0231 b2Vec2 localPointA = m_proxyA->GetVertex(indexA); 0232 b2Vec2 pointA = b2Mul(xfA, localPointA); 0233 0234 float32 separation = b2Dot(pointA - pointB, normal); 0235 return separation; 0236 } 0237 0238 default: 0239 b2Assert(false); 0240 return 0.0f; 0241 } 0242 } 0243 0244 const b2DistanceProxy* m_proxyA; 0245 const b2DistanceProxy* m_proxyB; 0246 b2Sweep m_sweepA, m_sweepB; 0247 Type m_type; 0248 b2Vec2 m_localPoint; 0249 b2Vec2 m_axis; 0250 }; 0251 0252 // CCD via the local separating axis method. This seeks progression 0253 // by computing the largest time at which separation is maintained. 0254 void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input) 0255 { 0256 b2Timer timer; 0257 0258 ++b2_toiCalls; 0259 0260 output->state = b2TOIOutput::e_unknown; 0261 output->t = input->tMax; 0262 0263 const b2DistanceProxy* proxyA = &input->proxyA; 0264 const b2DistanceProxy* proxyB = &input->proxyB; 0265 0266 b2Sweep sweepA = input->sweepA; 0267 b2Sweep sweepB = input->sweepB; 0268 0269 // Large rotations can make the root finder fail, so we normalize the 0270 // sweep angles. 0271 sweepA.Normalize(); 0272 sweepB.Normalize(); 0273 0274 float32 tMax = input->tMax; 0275 0276 float32 totalRadius = proxyA->m_radius + proxyB->m_radius; 0277 float32 target = b2Max(b2_linearSlop, totalRadius - 3.0f * b2_linearSlop); 0278 float32 tolerance = 0.25f * b2_linearSlop; 0279 b2Assert(target > tolerance); 0280 0281 float32 t1 = 0.0f; 0282 const int32 k_maxIterations = 20; // TODO_ERIN b2Settings 0283 int32 iter = 0; 0284 0285 // Prepare input for distance query. 0286 b2SimplexCache cache; 0287 cache.count = 0; 0288 b2DistanceInput distanceInput; 0289 distanceInput.proxyA = input->proxyA; 0290 distanceInput.proxyB = input->proxyB; 0291 distanceInput.useRadii = false; 0292 0293 // The outer loop progressively attempts to compute new separating axes. 0294 // This loop terminates when an axis is repeated (no progress is made). 0295 for(;;) 0296 { 0297 b2Transform xfA, xfB; 0298 sweepA.GetTransform(&xfA, t1); 0299 sweepB.GetTransform(&xfB, t1); 0300 0301 // Get the distance between shapes. We can also use the results 0302 // to get a separating axis. 0303 distanceInput.transformA = xfA; 0304 distanceInput.transformB = xfB; 0305 b2DistanceOutput distanceOutput; 0306 b2Distance(&distanceOutput, &cache, &distanceInput); 0307 0308 // If the shapes are overlapped, we give up on continuous collision. 0309 if (distanceOutput.distance <= 0.0f) 0310 { 0311 // Failure! 0312 output->state = b2TOIOutput::e_overlapped; 0313 output->t = 0.0f; 0314 break; 0315 } 0316 0317 if (distanceOutput.distance < target + tolerance) 0318 { 0319 // Victory! 0320 output->state = b2TOIOutput::e_touching; 0321 output->t = t1; 0322 break; 0323 } 0324 0325 // Initialize the separating axis. 0326 b2SeparationFunction fcn; 0327 fcn.Initialize(&cache, proxyA, sweepA, proxyB, sweepB, t1); 0328 #if 0 0329 // Dump the curve seen by the root finder 0330 { 0331 const int32 N = 100; 0332 float32 dx = 1.0f / N; 0333 float32 xs[N+1]; 0334 float32 fs[N+1]; 0335 0336 float32 x = 0.0f; 0337 0338 for (int32 i = 0; i <= N; ++i) 0339 { 0340 sweepA.GetTransform(&xfA, x); 0341 sweepB.GetTransform(&xfB, x); 0342 float32 f = fcn.Evaluate(xfA, xfB) - target; 0343 0344 printf("%g %g\n", x, f); 0345 0346 xs[i] = x; 0347 fs[i] = f; 0348 0349 x += dx; 0350 } 0351 } 0352 #endif 0353 0354 // Compute the TOI on the separating axis. We do this by successively 0355 // resolving the deepest point. This loop is bounded by the number of vertices. 0356 bool done = false; 0357 float32 t2 = tMax; 0358 int32 pushBackIter = 0; 0359 for (;;) 0360 { 0361 // Find the deepest point at t2. Store the witness point indices. 0362 int32 indexA, indexB; 0363 float32 s2 = fcn.FindMinSeparation(&indexA, &indexB, t2); 0364 0365 // Is the final configuration separated? 0366 if (s2 > target + tolerance) 0367 { 0368 // Victory! 0369 output->state = b2TOIOutput::e_separated; 0370 output->t = tMax; 0371 done = true; 0372 break; 0373 } 0374 0375 // Has the separation reached tolerance? 0376 if (s2 > target - tolerance) 0377 { 0378 // Advance the sweeps 0379 t1 = t2; 0380 break; 0381 } 0382 0383 // Compute the initial separation of the witness points. 0384 float32 s1 = fcn.Evaluate(indexA, indexB, t1); 0385 0386 // Check for initial overlap. This might happen if the root finder 0387 // runs out of iterations. 0388 if (s1 < target - tolerance) 0389 { 0390 output->state = b2TOIOutput::e_failed; 0391 output->t = t1; 0392 done = true; 0393 break; 0394 } 0395 0396 // Check for touching 0397 if (s1 <= target + tolerance) 0398 { 0399 // Victory! t1 should hold the TOI (could be 0.0). 0400 output->state = b2TOIOutput::e_touching; 0401 output->t = t1; 0402 done = true; 0403 break; 0404 } 0405 0406 // Compute 1D root of: f(x) - target = 0 0407 int32 rootIterCount = 0; 0408 float32 a1 = t1, a2 = t2; 0409 for (;;) 0410 { 0411 // Use a mix of the secant rule and bisection. 0412 float32 t; 0413 if (rootIterCount & 1) 0414 { 0415 // Secant rule to improve convergence. 0416 t = a1 + (target - s1) * (a2 - a1) / (s2 - s1); 0417 } 0418 else 0419 { 0420 // Bisection to guarantee progress. 0421 t = 0.5f * (a1 + a2); 0422 } 0423 0424 ++rootIterCount; 0425 ++b2_toiRootIters; 0426 0427 float32 s = fcn.Evaluate(indexA, indexB, t); 0428 0429 if (b2Abs(s - target) < tolerance) 0430 { 0431 // t2 holds a tentative value for t1 0432 t2 = t; 0433 break; 0434 } 0435 0436 // Ensure we continue to bracket the root. 0437 if (s > target) 0438 { 0439 a1 = t; 0440 s1 = s; 0441 } 0442 else 0443 { 0444 a2 = t; 0445 s2 = s; 0446 } 0447 0448 if (rootIterCount == 50) 0449 { 0450 break; 0451 } 0452 } 0453 0454 b2_toiMaxRootIters = b2Max(b2_toiMaxRootIters, rootIterCount); 0455 0456 ++pushBackIter; 0457 0458 if (pushBackIter == b2_maxPolygonVertices) 0459 { 0460 break; 0461 } 0462 } 0463 0464 ++iter; 0465 ++b2_toiIters; 0466 0467 if (done) 0468 { 0469 break; 0470 } 0471 0472 if (iter == k_maxIterations) 0473 { 0474 // Root finder got stuck. Semi-victory. 0475 output->state = b2TOIOutput::e_failed; 0476 output->t = t1; 0477 break; 0478 } 0479 } 0480 0481 b2_toiMaxIters = b2Max(b2_toiMaxIters, iter); 0482 0483 float32 time = timer.GetMilliseconds(); 0484 b2_toiMaxTime = b2Max(b2_toiMaxTime, time); 0485 b2_toiTime += time; 0486 }