b2TOISolver.cpp
上传用户:gb3593
上传日期:2022-01-07
资源大小:3028k
文件大小:6k
- /*
- * Copyright (c) 2006-2010 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/Dynamics/Contacts/b2TOISolver.h>
- #include <Box2D/Dynamics/Contacts/b2Contact.h>
- #include <Box2D/Dynamics/b2Body.h>
- #include <Box2D/Dynamics/b2Fixture.h>
- #include <Box2D/Common/b2StackAllocator.h>
- struct b2TOIConstraint
- {
- b2Vec2 localPoints[b2_maxManifoldPoints];
- b2Vec2 localNormal;
- b2Vec2 localPoint;
- b2Manifold::Type type;
- float32 radius;
- int32 pointCount;
- b2Body* bodyA;
- b2Body* bodyB;
- };
- b2TOISolver::b2TOISolver(b2StackAllocator* allocator)
- {
- m_allocator = allocator;
- m_constraints = NULL;
- m_count = NULL;
- m_toiBody = NULL;
- }
- b2TOISolver::~b2TOISolver()
- {
- Clear();
- }
- void b2TOISolver::Clear()
- {
- if (m_allocator && m_constraints)
- {
- m_allocator->Free(m_constraints);
- m_constraints = NULL;
- }
- }
- void b2TOISolver::Initialize(b2Contact** contacts, int32 count, b2Body* toiBody)
- {
- Clear();
- m_count = count;
- m_toiBody = toiBody;
- m_constraints = (b2TOIConstraint*) m_allocator->Allocate(m_count * sizeof(b2TOIConstraint));
- for (int32 i = 0; i < m_count; ++i)
- {
- b2Contact* contact = contacts[i];
- b2Fixture* fixtureA = contact->GetFixtureA();
- b2Fixture* fixtureB = contact->GetFixtureB();
- b2Shape* shapeA = fixtureA->GetShape();
- b2Shape* shapeB = fixtureB->GetShape();
- float32 radiusA = shapeA->m_radius;
- float32 radiusB = shapeB->m_radius;
- b2Body* bodyA = fixtureA->GetBody();
- b2Body* bodyB = fixtureB->GetBody();
- b2Manifold* manifold = contact->GetManifold();
- b2Assert(manifold->pointCount > 0);
- b2TOIConstraint* constraint = m_constraints + i;
- constraint->bodyA = bodyA;
- constraint->bodyB = bodyB;
- constraint->localNormal = manifold->localNormal;
- constraint->localPoint = manifold->localPoint;
- constraint->type = manifold->type;
- constraint->pointCount = manifold->pointCount;
- constraint->radius = radiusA + radiusB;
- for (int32 j = 0; j < constraint->pointCount; ++j)
- {
- b2ManifoldPoint* cp = manifold->points + j;
- constraint->localPoints[j] = cp->localPoint;
- }
- }
- }
- struct b2TOISolverManifold
- {
- void Initialize(b2TOIConstraint* cc, int32 index)
- {
- b2Assert(cc->pointCount > 0);
- switch (cc->type)
- {
- case b2Manifold::e_circles:
- {
- b2Vec2 pointA = cc->bodyA->GetWorldPoint(cc->localPoint);
- b2Vec2 pointB = cc->bodyB->GetWorldPoint(cc->localPoints[0]);
- if (b2DistanceSquared(pointA, pointB) > b2_epsilon * b2_epsilon)
- {
- normal = pointB - pointA;
- normal.Normalize();
- }
- else
- {
- normal.Set(1.0f, 0.0f);
- }
- point = 0.5f * (pointA + pointB);
- separation = b2Dot(pointB - pointA, normal) - cc->radius;
- }
- break;
- case b2Manifold::e_faceA:
- {
- normal = cc->bodyA->GetWorldVector(cc->localNormal);
- b2Vec2 planePoint = cc->bodyA->GetWorldPoint(cc->localPoint);
- b2Vec2 clipPoint = cc->bodyB->GetWorldPoint(cc->localPoints[index]);
- separation = b2Dot(clipPoint - planePoint, normal) - cc->radius;
- point = clipPoint;
- }
- break;
- case b2Manifold::e_faceB:
- {
- normal = cc->bodyB->GetWorldVector(cc->localNormal);
- b2Vec2 planePoint = cc->bodyB->GetWorldPoint(cc->localPoint);
- b2Vec2 clipPoint = cc->bodyA->GetWorldPoint(cc->localPoints[index]);
- separation = b2Dot(clipPoint - planePoint, normal) - cc->radius;
- point = clipPoint;
- // Ensure normal points from A to B
- normal = -normal;
- }
- break;
- }
- }
- b2Vec2 normal;
- b2Vec2 point;
- float32 separation;
- };
- // Push out the toi body to provide clearance for further simulation.
- bool b2TOISolver::Solve(float32 baumgarte)
- {
- float32 minSeparation = 0.0f;
- for (int32 i = 0; i < m_count; ++i)
- {
- b2TOIConstraint* c = m_constraints + i;
- b2Body* bodyA = c->bodyA;
- b2Body* bodyB = c->bodyB;
- float32 massA = bodyA->m_mass;
- float32 massB = bodyB->m_mass;
- // Only the TOI body should move.
- if (bodyA == m_toiBody)
- {
- massB = 0.0f;
- }
- else
- {
- massA = 0.0f;
- }
- float32 invMassA = massA * bodyA->m_invMass;
- float32 invIA = massA * bodyA->m_invI;
- float32 invMassB = massB * bodyB->m_invMass;
- float32 invIB = massB * bodyB->m_invI;
- // Solve normal constraints
- for (int32 j = 0; j < c->pointCount; ++j)
- {
- b2TOISolverManifold psm;
- psm.Initialize(c, j);
- b2Vec2 normal = psm.normal;
- b2Vec2 point = psm.point;
- float32 separation = psm.separation;
- b2Vec2 rA = point - bodyA->m_sweep.c;
- b2Vec2 rB = point - bodyB->m_sweep.c;
- // Track max constraint error.
- minSeparation = b2Min(minSeparation, separation);
- // Prevent large corrections and allow slop.
- float32 C = b2Clamp(baumgarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f);
- // Compute the effective mass.
- float32 rnA = b2Cross(rA, normal);
- float32 rnB = b2Cross(rB, normal);
- float32 K = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB;
- // Compute normal impulse
- float32 impulse = K > 0.0f ? - C / K : 0.0f;
- b2Vec2 P = impulse * normal;
- bodyA->m_sweep.c -= invMassA * P;
- bodyA->m_sweep.a -= invIA * b2Cross(rA, P);
- bodyA->SynchronizeTransform();
- bodyB->m_sweep.c += invMassB * P;
- bodyB->m_sweep.a += invIB * b2Cross(rB, P);
- bodyB->SynchronizeTransform();
- }
- }
- // We can't expect minSpeparation >= -b2_linearSlop because we don't
- // push the separation above -b2_linearSlop.
- return minSeparation >= -1.5f * b2_linearSlop;
- }