C++ Fixture::GetBody方法代码示例

inline void Contact::GetWorldManifold(WorldManifold* worldManifold) const
{
	const Body* bodyA = m_fixtureA->GetBody();
	const Body* bodyB = m_fixtureB->GetBody();
	const Shape* shapeA = m_fixtureA->GetShape();
	const Shape* shapeB = m_fixtureB->GetShape();

	worldManifold->Initialize(&m_manifold, bodyA->GetTransform(), shapeA->m_radius, bodyB->GetTransform(), shapeB->m_radius);
}

void Contact::Destroy(Contact* contact, BlockAllocator* allocator)
{
    assert(s_initialized == true);

    Fixture* fixtureA = contact->m_fixtureA;
    Fixture* fixtureB = contact->m_fixtureB;

    if (contact->m_manifold.pointCount > 0 &&
            fixtureA->IsSensor() == false &&
            fixtureB->IsSensor() == false)
    {
        fixtureA->GetBody()->SetAwake(true);
        fixtureB->GetBody()->SetAwake(true);
    }

    Shape::Type typeA = fixtureA->GetType();
    Shape::Type typeB = fixtureB->GetType();

    assert(0 <= typeA && typeB < Shape::e_typeCount);
    assert(0 <= typeA && typeB < Shape::e_typeCount);

    ContactDestroyFcn* destroyFcn = s_registers[typeA][typeB].destroyFcn;
    destroyFcn(contact, allocator);
}

ContactSolver::ContactSolver(ContactSolverDef* def)
{
	m_step = def->step;
	m_allocator = def->allocator;
	m_count = def->count;
	m_positionConstraints = (ContactPositionConstraint*)m_allocator->Allocate(m_count * sizeof(ContactPositionConstraint));
	m_velocityConstraints = (ContactVelocityConstraint*)m_allocator->Allocate(m_count * sizeof(ContactVelocityConstraint));
	m_positions = def->positions;
	m_velocities = def->velocities;
	m_contacts = def->contacts;

	// Initialize position independent portions of the constraints.
	for (int32 i = 0; i < m_count; ++i)
	{
		Contact* contact = m_contacts[i];

		Fixture* fixtureA = contact->m_fixtureA;
		Fixture* fixtureB = contact->m_fixtureB;
		Shape* shapeA = fixtureA->GetShape();
		Shape* shapeB = fixtureB->GetShape();
		float32 radiusA = shapeA->m_radius;
		float32 radiusB = shapeB->m_radius;
		Body* bodyA = fixtureA->GetBody();
		Body* bodyB = fixtureB->GetBody();
		Manifold* manifold = contact->GetManifold();

		int32 pointCount = manifold->pointCount;
		assert(pointCount > 0);

		ContactVelocityConstraint* vc = m_velocityConstraints + i;
		vc->friction = contact->m_friction;
		vc->restitution = contact->m_restitution;
		vc->tangentSpeed = contact->m_tangentSpeed;
		vc->indexA = bodyA->m_islandIndex;
		vc->indexB = bodyB->m_islandIndex;
		vc->invMassA = bodyA->m_invMass;
		vc->invMassB = bodyB->m_invMass;
		vc->invIA = bodyA->m_invI;
		vc->invIB = bodyB->m_invI;
		vc->contactIndex = i;
		vc->pointCount = pointCount;
		vc->K.SetZero();
		vc->normalMass.SetZero();

		ContactPositionConstraint* pc = m_positionConstraints + i;
		pc->indexA = bodyA->m_islandIndex;
		pc->indexB = bodyB->m_islandIndex;
		pc->invMassA = bodyA->m_invMass;
		pc->invMassB = bodyB->m_invMass;
		pc->localCenterA = bodyA->m_sweep.localCenter;
		pc->localCenterB = bodyB->m_sweep.localCenter;
		pc->invIA = bodyA->m_invI;
		pc->invIB = bodyB->m_invI;
		pc->localNormal = manifold->localNormal;
		pc->localPoint = manifold->localPoint;
		pc->pointCount = pointCount;
		pc->radiusA = radiusA;
		pc->radiusB = radiusB;
		pc->type = manifold->type;

		for (int32 j = 0; j < pointCount; ++j)
		{
			ManifoldPoint* cp = manifold->points + j;
			VelocityConstraintPoint* vcp = vc->points + j;
	
			if (m_step.warmStarting)
			{
				vcp->normalImpulse = m_step.dtRatio * cp->normalImpulse;
				vcp->tangentImpulse = m_step.dtRatio * cp->tangentImpulse;
			}
			else
			{
				vcp->normalImpulse = 0.0f;
				vcp->tangentImpulse = 0.0f;
			}

			vcp->rA.SetZero();
			vcp->rB.SetZero();
			vcp->normalMass = 0.0f;
			vcp->tangentMass = 0.0f;
			vcp->velocityBias = 0.0f;

			pc->localPoints[j] = cp->localPoint;
		}
	}
}

void Island::SolveTOI(const PTimeStep& subStep, s32 toiIndexA, s32 toiIndexB)
{
	assert(toiIndexA < m_bodyCount);
	assert(toiIndexB < m_bodyCount);

	// Initialize the body state.
	for (s32 i = 0; i < m_bodyCount; ++i)
	{
		Body* b = m_bodies[i];
		m_positions[i].c = b->m_sweep.c;
		m_positions[i].a = b->m_sweep.a;
		m_velocities[i].v = b->m_linearVelocity;
		m_velocities[i].w = b->m_angularVelocity;
	}

	ContactSolverDef contactSolverDef;
	contactSolverDef.contacts = m_contacts;
	contactSolverDef.count = m_contactCount;
	contactSolverDef.allocator = m_allocator;
	contactSolverDef.step = subStep;
	contactSolverDef.positions = m_positions;
	contactSolverDef.velocities = m_velocities;
	ContactSolver contactSolver(&contactSolverDef);

	// Solve position constraints.
	for (s32 i = 0; i < subStep.positionIterations; ++i)
	{
		bool contactsOkay = contactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB);
		if (contactsOkay)
		{
			break;
		}
	}

#if 0
	// Is the new position really safe?
	for (s32 i = 0; i < m_contactCount; ++i)
	{
		Contact* c = m_contacts[i];
		Fixture* fA = c->GetFixtureA();
		Fixture* fB = c->GetFixtureB();

		Body* bA = fA->GetBody();
		Body* bB = fB->GetBody();

		s32 indexA = c->GetChildIndexA();
		s32 indexB = c->GetChildIndexB();

		DistanceInput input;
		input.proxyA.Set(fA->GetShape(), indexA);
		input.proxyB.Set(fB->GetShape(), indexB);
		input.Transform2DA = bA->GetTransform2D();
		input.Transform2DB = bB->GetTransform2D();
		input.useRadii = false;

		DistanceOutput output;
		SimplexCache cache;
		cache.count = 0;
		Distance(&output, &cache, &input);

		if (output.distance == 0 || cache.count == 3)
		{
			cache.count += 0;
		}
	}
#endif

	// Leap of faith to new safe state.
	m_bodies[toiIndexA]->m_sweep.c0 = m_positions[toiIndexA].c;
	m_bodies[toiIndexA]->m_sweep.a0 = m_positions[toiIndexA].a;
	m_bodies[toiIndexB]->m_sweep.c0 = m_positions[toiIndexB].c;
	m_bodies[toiIndexB]->m_sweep.a0 = m_positions[toiIndexB].a;

	// No warm starting is needed for TOI events because warm
	// starting impulses were applied in the discrete solver.
	contactSolver.InitializeVelocityConstraints();

	// Solve velocity constraints.
	for (s32 i = 0; i < subStep.velocityIterations; ++i)
	{
		contactSolver.SolveVelocityConstraints();
	}

	// Don't store the TOI contact forces for warm starting
	// because they can be quite large.

	real32 h = subStep.delta;

	// Integrate positions
	for (s32 i = 0; i < m_bodyCount; ++i)
	{
		glm::vec2 c = m_positions[i].c;
		real32 a = m_positions[i].a;
		glm::vec2 v = m_velocities[i].v;
		real32 w = m_velocities[i].w;

		// Check for large velocities
		glm::vec2 translation = h * v;
		if (glm::dot(translation, translation) > maxTranslationSquared)
		{
//.........这里部分代码省略.........

// This is the top level collision call for the time step. Here
// all the narrow phase collision is processed for the world
// contact list.
void ContactManager::Collide()
{
	// Update awake contacts.
	Contact* c = m_contactList;
	while (c)
	{
		Fixture* fixtureA = c->GetFixtureA();
		Fixture* fixtureB = c->GetFixtureB();
		s32 indexA = c->GetChildIndexA();
		s32 indexB = c->GetChildIndexB();
		Body* bodyA = fixtureA->GetBody();
		Body* bodyB = fixtureB->GetBody();

		// Is this contact flagged for filtering?
		if (c->m_flags & Contact::filterFlag)
		{
			// Should these bodies collide?
			if (bodyB->ShouldCollide(bodyA) == false)
			{
				Contact* cNuke = c;
				c = cNuke->GetNext();
				Destroy(cNuke);
				continue;
			}

			// Check user filtering.
			if (m_contactFilter && m_contactFilter->ShouldCollide(fixtureA, fixtureB) == false)
			{
				Contact* cNuke = c;
				c = cNuke->GetNext();
				Destroy(cNuke);
				continue;
			}

			// Clear the filtering flag.
			c->m_flags &= ~Contact::filterFlag;
		}

		bool activeA = bodyA->IsAwake() && bodyA->m_type != staticBody;
		bool activeB = bodyB->IsAwake() && bodyB->m_type != staticBody;

		// At least one body must be awake and it must be dynamic or kinematic.
		if (activeA == false && activeB == false)
		{
			c = c->GetNext();
			continue;
		}

		s32 proxyIdA = fixtureA->m_proxies[indexA].proxyId;
		s32 proxyIdB = fixtureB->m_proxies[indexB].proxyId;
		bool overlap = m_broadPhase.TestOverlap(proxyIdA, proxyIdB);

		// Here we destroy contacts that cease to overlap in the broad-phase.
		if (overlap == false)
		{
			Contact* cNuke = c;
			c = cNuke->GetNext();
			Destroy(cNuke);
			continue;
		}

		// The contact persists.
		c->Update(m_contactListener);
		c = c->GetNext();
	}
}

void ContactManager::Destroy(Contact* c)
{
	Fixture* fixtureA = c->GetFixtureA();
	Fixture* fixtureB = c->GetFixtureB();
	Body* bodyA = fixtureA->GetBody();
	Body* bodyB = fixtureB->GetBody();

	if (m_contactListener && c->IsTouching())
	{
		m_contactListener->EndContact(c);
	}

	// Remove from the world.
	if (c->m_prev)
	{
		c->m_prev->m_next = c->m_next;
	}

	if (c->m_next)
	{
		c->m_next->m_prev = c->m_prev;
	}

	if (c == m_contactList)
	{
		m_contactList = c->m_next;
	}

	// Remove from body 1
	if (c->m_nodeA.prev)
	{
		c->m_nodeA.prev->next = c->m_nodeA.next;
	}

	if (c->m_nodeA.next)
	{
		c->m_nodeA.next->prev = c->m_nodeA.prev;
	}

	if (&c->m_nodeA == bodyA->m_contactList)
	{
		bodyA->m_contactList = c->m_nodeA.next;
	}

	// Remove from body 2
	if (c->m_nodeB.prev)
	{
		c->m_nodeB.prev->next = c->m_nodeB.next;
	}

	if (c->m_nodeB.next)
	{
		c->m_nodeB.next->prev = c->m_nodeB.prev;
	}

	if (&c->m_nodeB == bodyB->m_contactList)
	{
		bodyB->m_contactList = c->m_nodeB.next;
	}

	// Call the factory.
	Contact::Destroy(c, m_allocator);
	--m_contactCount;
}

void ContactManager::AddPair(void* proxyUserDataA, void* proxyUserDataB)
{
	FixtureProxy* proxyA = (FixtureProxy*)proxyUserDataA;
	FixtureProxy* proxyB = (FixtureProxy*)proxyUserDataB;

	Fixture* fixtureA = proxyA->fixture;
	Fixture* fixtureB = proxyB->fixture;

	s32 indexA = proxyA->childIndex;
	s32 indexB = proxyB->childIndex;

	Body* bodyA = fixtureA->GetBody();
	Body* bodyB = fixtureB->GetBody();

	// Are the fixtures on the same body?
	if (bodyA == bodyB)
	{
		return;
	}

	// TODO_ERIN use a hash table to remove a potential bottleneck when both
	// bodies have a lot of contacts.
	// Does a contact already exist?
	ContactEdge* edge = bodyB->GetContactList();
	while (edge)
	{
		if (edge->other == bodyA)
		{
			Fixture* fA = edge->contact->GetFixtureA();
			Fixture* fB = edge->contact->GetFixtureB();
			s32 iA = edge->contact->GetChildIndexA();
			s32 iB = edge->contact->GetChildIndexB();

			if (fA == fixtureA && fB == fixtureB && iA == indexA && iB == indexB)
			{
				// A contact already exists.
				return;
			}

			if (fA == fixtureB && fB == fixtureA && iA == indexB && iB == indexA)
			{
				// A contact already exists.
				return;
			}
		}

		edge = edge->next;
	}

	// Does a joint override collision? Is at least one body dynamic?
	if (bodyB->ShouldCollide(bodyA) == false)
	{
		return;
	}

	// Check user filtering.
	if (m_contactFilter && m_contactFilter->ShouldCollide(fixtureA, fixtureB) == false)
	{
		return;
	}

	// Call the factory.
	Contact* c = Contact::Create(fixtureA, indexA, fixtureB, indexB, m_allocator);
	if (c == NULL)
	{
		return;
	}

	// Contact creation may swap fixtures.
	fixtureA = c->GetFixtureA();
	fixtureB = c->GetFixtureB();
	indexA = c->GetChildIndexA();
	indexB = c->GetChildIndexB();
	bodyA = fixtureA->GetBody();
	bodyB = fixtureB->GetBody();

	// Insert into the world.
	c->m_prev = NULL;
	c->m_next = m_contactList;
	if (m_contactList != NULL)
	{
		m_contactList->m_prev = c;
	}
	m_contactList = c;

	// Connect to island graph.

	// Connect to body A
	c->m_nodeA.contact = c;
	c->m_nodeA.other = bodyB;

	c->m_nodeA.prev = NULL;
	c->m_nodeA.next = bodyA->m_contactList;
	if (bodyA->m_contactList != NULL)
	{
		bodyA->m_contactList->prev = &c->m_nodeA;
	}
	bodyA->m_contactList = &c->m_nodeA;

	// Connect to body B
//.........这里部分代码省略.........