b2TimeOfImpact.cpp
上传用户:gb3593
上传日期:2022-01-07
资源大小:3028k
文件大小:12k
- /*
- * Copyright (c) 2007-2009 Erin Catto http://www.gphysics.com
- *
- * This software is provided 'as-is', without any express or implied
- * warranty. In no event will the authors be held liable for any damages
- * arising from the use of this software.
- * Permission is granted to anyone to use this software for any purpose,
- * including commercial applications, and to alter it and redistribute it
- * freely, subject to the following restrictions:
- * 1. The origin of this software must not be misrepresented; you must not
- * claim that you wrote the original software. If you use this software
- * in a product, an acknowledgment in the product documentation would be
- * appreciated but is not required.
- * 2. Altered source versions must be plainly marked as such, and must not be
- * misrepresented as being the original software.
- * 3. This notice may not be removed or altered from any source distribution.
- */
- #include <Box2D/Collision/b2Collision.h>
- #include <Box2D/Collision/b2Distance.h>
- #include <Box2D/Collision/b2TimeOfImpact.h>
- #include <Box2D/Collision/Shapes/b2CircleShape.h>
- #include <Box2D/Collision/Shapes/b2PolygonShape.h>
- #include <cstdio>
- int32 b2_toiCalls, b2_toiIters, b2_toiMaxIters;
- int32 b2_toiRootIters, b2_toiMaxRootIters;
- int32 b2_toiMaxOptIters;
- struct b2SeparationFunction
- {
- enum Type
- {
- e_points,
- e_faceA,
- e_faceB
- };
- // TODO_ERIN might not need to return the separation
- float32 Initialize(const b2SimplexCache* cache,
- const b2DistanceProxy* proxyA, const b2Sweep& sweepA,
- const b2DistanceProxy* proxyB, const b2Sweep& sweepB)
- {
- m_proxyA = proxyA;
- m_proxyB = proxyB;
- int32 count = cache->count;
- b2Assert(0 < count && count < 3);
- m_sweepA = sweepA;
- m_sweepB = sweepB;
- b2Transform xfA, xfB;
- m_sweepA.GetTransform(&xfA, 0.0f);
- m_sweepB.GetTransform(&xfB, 0.0f);
- if (count == 1)
- {
- m_type = e_points;
- b2Vec2 localPointA = m_proxyA->GetVertex(cache->indexA[0]);
- b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]);
- b2Vec2 pointA = b2Mul(xfA, localPointA);
- b2Vec2 pointB = b2Mul(xfB, localPointB);
- m_axis = pointB - pointA;
- float32 s = m_axis.Normalize();
- return s;
- }
- else if (cache->indexA[0] == cache->indexA[1])
- {
- // Two points on B and one on A.
- m_type = e_faceB;
- b2Vec2 localPointB1 = proxyB->GetVertex(cache->indexB[0]);
- b2Vec2 localPointB2 = proxyB->GetVertex(cache->indexB[1]);
- m_axis = b2Cross(localPointB2 - localPointB1, 1.0f);
- m_axis.Normalize();
- b2Vec2 normal = b2Mul(xfB.R, m_axis);
- m_localPoint = 0.5f * (localPointB1 + localPointB2);
- b2Vec2 pointB = b2Mul(xfB, m_localPoint);
- b2Vec2 localPointA = proxyA->GetVertex(cache->indexA[0]);
- b2Vec2 pointA = b2Mul(xfA, localPointA);
- float32 s = b2Dot(pointA - pointB, normal);
- if (s < 0.0f)
- {
- m_axis = -m_axis;
- s = -s;
- }
- return s;
- }
- else
- {
- // Two points on A and one or two points on B.
- m_type = e_faceA;
- b2Vec2 localPointA1 = m_proxyA->GetVertex(cache->indexA[0]);
- b2Vec2 localPointA2 = m_proxyA->GetVertex(cache->indexA[1]);
-
- m_axis = b2Cross(localPointA2 - localPointA1, 1.0f);
- m_axis.Normalize();
- b2Vec2 normal = b2Mul(xfA.R, m_axis);
- m_localPoint = 0.5f * (localPointA1 + localPointA2);
- b2Vec2 pointA = b2Mul(xfA, m_localPoint);
- b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]);
- b2Vec2 pointB = b2Mul(xfB, localPointB);
- float32 s = b2Dot(pointB - pointA, normal);
- if (s < 0.0f)
- {
- m_axis = -m_axis;
- s = -s;
- }
- return s;
- }
- }
- float32 FindMinSeparation(int32* indexA, int32* indexB, float32 t) const
- {
- b2Transform xfA, xfB;
- m_sweepA.GetTransform(&xfA, t);
- m_sweepB.GetTransform(&xfB, t);
- switch (m_type)
- {
- case e_points:
- {
- b2Vec2 axisA = b2MulT(xfA.R, m_axis);
- b2Vec2 axisB = b2MulT(xfB.R, -m_axis);
- *indexA = m_proxyA->GetSupport(axisA);
- *indexB = m_proxyB->GetSupport(axisB);
- b2Vec2 localPointA = m_proxyA->GetVertex(*indexA);
- b2Vec2 localPointB = m_proxyB->GetVertex(*indexB);
-
- b2Vec2 pointA = b2Mul(xfA, localPointA);
- b2Vec2 pointB = b2Mul(xfB, localPointB);
- float32 separation = b2Dot(pointB - pointA, m_axis);
- return separation;
- }
- case e_faceA:
- {
- b2Vec2 normal = b2Mul(xfA.R, m_axis);
- b2Vec2 pointA = b2Mul(xfA, m_localPoint);
- b2Vec2 axisB = b2MulT(xfB.R, -normal);
-
- *indexA = -1;
- *indexB = m_proxyB->GetSupport(axisB);
- b2Vec2 localPointB = m_proxyB->GetVertex(*indexB);
- b2Vec2 pointB = b2Mul(xfB, localPointB);
- float32 separation = b2Dot(pointB - pointA, normal);
- return separation;
- }
- case e_faceB:
- {
- b2Vec2 normal = b2Mul(xfB.R, m_axis);
- b2Vec2 pointB = b2Mul(xfB, m_localPoint);
- b2Vec2 axisA = b2MulT(xfA.R, -normal);
- *indexB = -1;
- *indexA = m_proxyA->GetSupport(axisA);
- b2Vec2 localPointA = m_proxyA->GetVertex(*indexA);
- b2Vec2 pointA = b2Mul(xfA, localPointA);
- float32 separation = b2Dot(pointA - pointB, normal);
- return separation;
- }
- default:
- b2Assert(false);
- *indexA = -1;
- *indexB = -1;
- return 0.0f;
- }
- }
- float32 Evaluate(int32 indexA, int32 indexB, float32 t) const
- {
- b2Transform xfA, xfB;
- m_sweepA.GetTransform(&xfA, t);
- m_sweepB.GetTransform(&xfB, t);
- switch (m_type)
- {
- case e_points:
- {
- b2Vec2 axisA = b2MulT(xfA.R, m_axis);
- b2Vec2 axisB = b2MulT(xfB.R, -m_axis);
- b2Vec2 localPointA = m_proxyA->GetVertex(indexA);
- b2Vec2 localPointB = m_proxyB->GetVertex(indexB);
- b2Vec2 pointA = b2Mul(xfA, localPointA);
- b2Vec2 pointB = b2Mul(xfB, localPointB);
- float32 separation = b2Dot(pointB - pointA, m_axis);
- return separation;
- }
- case e_faceA:
- {
- b2Vec2 normal = b2Mul(xfA.R, m_axis);
- b2Vec2 pointA = b2Mul(xfA, m_localPoint);
- b2Vec2 axisB = b2MulT(xfB.R, -normal);
- b2Vec2 localPointB = m_proxyB->GetVertex(indexB);
- b2Vec2 pointB = b2Mul(xfB, localPointB);
- float32 separation = b2Dot(pointB - pointA, normal);
- return separation;
- }
- case e_faceB:
- {
- b2Vec2 normal = b2Mul(xfB.R, m_axis);
- b2Vec2 pointB = b2Mul(xfB, m_localPoint);
- b2Vec2 axisA = b2MulT(xfA.R, -normal);
- b2Vec2 localPointA = m_proxyA->GetVertex(indexA);
- b2Vec2 pointA = b2Mul(xfA, localPointA);
- float32 separation = b2Dot(pointA - pointB, normal);
- return separation;
- }
- default:
- b2Assert(false);
- return 0.0f;
- }
- }
- const b2DistanceProxy* m_proxyA;
- const b2DistanceProxy* m_proxyB;
- b2Sweep m_sweepA, m_sweepB;
- Type m_type;
- b2Vec2 m_localPoint;
- b2Vec2 m_axis;
- };
- // CCD via the local separating axis method. This seeks progression
- // by computing the largest time at which separation is maintained.
- void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input)
- {
- ++b2_toiCalls;
- output->state = b2TOIOutput::e_unknown;
- output->t = input->tMax;
- const b2DistanceProxy* proxyA = &input->proxyA;
- const b2DistanceProxy* proxyB = &input->proxyB;
- b2Sweep sweepA = input->sweepA;
- b2Sweep sweepB = input->sweepB;
- // Large rotations can make the root finder fail, so we normalize the
- // sweep angles.
- sweepA.Normalize();
- sweepB.Normalize();
- float32 tMax = input->tMax;
- float32 totalRadius = proxyA->m_radius + proxyB->m_radius;
- float32 target = b2Max(b2_linearSlop, totalRadius - 3.0f * b2_linearSlop);
- float32 tolerance = 0.25f * b2_linearSlop;
- b2Assert(target > tolerance);
- float32 t1 = 0.0f;
- const int32 k_maxIterations = 20; // TODO_ERIN b2Settings
- int32 iter = 0;
- // Prepare input for distance query.
- b2SimplexCache cache;
- cache.count = 0;
- b2DistanceInput distanceInput;
- distanceInput.proxyA = input->proxyA;
- distanceInput.proxyB = input->proxyB;
- distanceInput.useRadii = false;
- // The outer loop progressively attempts to compute new separating axes.
- // This loop terminates when an axis is repeated (no progress is made).
- for(;;)
- {
- b2Transform xfA, xfB;
- sweepA.GetTransform(&xfA, t1);
- sweepB.GetTransform(&xfB, t1);
- // Get the distance between shapes. We can also use the results
- // to get a separating axis.
- distanceInput.transformA = xfA;
- distanceInput.transformB = xfB;
- b2DistanceOutput distanceOutput;
- b2Distance(&distanceOutput, &cache, &distanceInput);
- // If the shapes are overlapped, we give up on continuous collision.
- if (distanceOutput.distance <= 0.0f)
- {
- // Failure!
- output->state = b2TOIOutput::e_overlapped;
- output->t = 0.0f;
- break;
- }
- if (distanceOutput.distance < target + tolerance)
- {
- // Victory!
- output->state = b2TOIOutput::e_touching;
- output->t = t1;
- break;
- }
- // Initialize the separating axis.
- b2SeparationFunction fcn;
- fcn.Initialize(&cache, proxyA, sweepA, proxyB, sweepB);
- #if 0
- // Dump the curve seen by the root finder
- {
- const int32 N = 100;
- float32 dx = 1.0f / N;
- float32 xs[N+1];
- float32 fs[N+1];
- float32 x = 0.0f;
- for (int32 i = 0; i <= N; ++i)
- {
- sweepA.GetTransform(&xfA, x);
- sweepB.GetTransform(&xfB, x);
- float32 f = fcn.Evaluate(xfA, xfB) - target;
- printf("%g %gn", x, f);
- xs[i] = x;
- fs[i] = f;
- x += dx;
- }
- }
- #endif
- // Compute the TOI on the separating axis. We do this by successively
- // resolving the deepest point. This loop is bounded by the number of vertices.
- bool done = false;
- float32 t2 = tMax;
- int32 pushBackIter = 0;
- for (;;)
- {
- // Find the deepest point at t2. Store the witness point indices.
- int32 indexA, indexB;
- float32 s2 = fcn.FindMinSeparation(&indexA, &indexB, t2);
- // Is the final configuration separated?
- if (s2 > target + tolerance)
- {
- // Victory!
- output->state = b2TOIOutput::e_separated;
- output->t = tMax;
- done = true;
- break;
- }
- // Has the separation reached tolerance?
- if (s2 > target - tolerance)
- {
- // Advance the sweeps
- t1 = t2;
- break;
- }
- // Compute the initial separation of the witness points.
- float32 s1 = fcn.Evaluate(indexA, indexB, t1);
- // Check for initial overlap. This might happen if the root finder
- // runs out of iterations.
- if (s1 < target - tolerance)
- {
- output->state = b2TOIOutput::e_failed;
- output->t = t1;
- done = true;
- break;
- }
- // Check for touching
- if (s1 <= target + tolerance)
- {
- // Victory! t1 should hold the TOI (could be 0.0).
- output->state = b2TOIOutput::e_touching;
- output->t = t1;
- done = true;
- break;
- }
- // Compute 1D root of: f(x) - target = 0
- int32 rootIterCount = 0;
- float32 a1 = t1, a2 = t2;
- for (;;)
- {
- // Use a mix of the secant rule and bisection.
- float32 t;
- if (rootIterCount & 1)
- {
- // Secant rule to improve convergence.
- t = a1 + (target - s1) * (a2 - a1) / (s2 - s1);
- }
- else
- {
- // Bisection to guarantee progress.
- t = 0.5f * (a1 + a2);
- }
- float32 s = fcn.Evaluate(indexA, indexB, t);
- if (b2Abs(s - target) < tolerance)
- {
- // t2 holds a tentative value for t1
- t2 = t;
- break;
- }
- // Ensure we continue to bracket the root.
- if (s > target)
- {
- a1 = t;
- s1 = s;
- }
- else
- {
- a2 = t;
- s2 = s;
- }
- ++rootIterCount;
- ++b2_toiRootIters;
- if (rootIterCount == 50)
- {
- break;
- }
- }
- b2_toiMaxRootIters = b2Max(b2_toiMaxRootIters, rootIterCount);
- ++pushBackIter;
- if (pushBackIter == b2_maxPolygonVertices)
- {
- break;
- }
- }
- ++iter;
- ++b2_toiIters;
- if (done)
- {
- break;
- }
- if (iter == k_maxIterations)
- {
- // Root finder got stuck. Semi-victory.
- output->state = b2TOIOutput::e_failed;
- output->t = t1;
- break;
- }
- }
- b2_toiMaxIters = b2Max(b2_toiMaxIters, iter);
- }