C++ Body::SynchronizeTransform2D方法代码示例

//.........这里部分代码省略.........
		}
	}

#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)
		{
			real32 ratio = maxTranslation / translation.length();
			v *= ratio;
		}

		real32 rotation = h * w;
		if (rotation * rotation > maxRotationSquared)
		{
			real32 ratio = maxRotation / glm::abs(rotation);
			w *= ratio;
		}

		// Integrate
		c += h * v;
		a += h * w;

		m_positions[i].c = c;
		m_positions[i].a = a;
		m_velocities[i].v = v;
		m_velocities[i].w = w;

		// Sync bodies
		Body* body = m_bodies[i];
		body->m_sweep.c = c;
		body->m_sweep.a = a;
		body->m_linearVelocity = v;
		body->m_angularVelocity = w;
		body->SynchronizeTransform2D();
	}

	Report(contactSolver.m_velocityConstraints);
}

//.........这里部分代码省略.........
		glm::vec2 translation = h * v;
		if (glm::dot(translation, translation) > maxTranslationSquared)
		{
			real32 ratio = maxTranslation / translation.length();
			v *= ratio;
		}

		real32 rotation = h * w;
		if (rotation * rotation > maxRotationSquared)
		{
			real32 ratio = maxRotation / glm::abs(rotation);
			w *= ratio;
		}

		// Integrate
		c += h * v;
		a += h * w;

		m_positions[i].c = c;
		m_positions[i].a = a;
		m_velocities[i].v = v;
		m_velocities[i].w = w;
	}

	// Solve position constraints
	captureTimer = Services::getPlatform()->getTime();
	bool positionSolved = false;
	for (s32 i = 0; i < step.positionIterations; ++i)
	{
		bool contactsOkay = contactSolver.SolvePositionConstraints();

		bool jointsOkay = true;
		for (s32 i = 0; i < m_jointCount; ++i)
		{
			bool jointOkay = m_joints[i]->SolvePositionConstraints(solverData);
			jointsOkay = jointsOkay && jointOkay;
		}

		if (contactsOkay && jointsOkay)
		{
			// Exit early if the position errors are small.
			positionSolved = true;
			break;
		}
	}

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

	profile->solvePosition = Services::getPlatform()->getTime()-captureTimer;

	Report(contactSolver.m_velocityConstraints);

	if (allowSleep)
	{
		real32 minSleepTime = FLT_MAX;

		const real32 linTolSqr = linearSleepTolerance * linearSleepTolerance;
		const real32 angTolSqr = angularSleepTolerance * angularSleepTolerance;

		for (s32 i = 0; i < m_bodyCount; ++i)
		{
			Body* b = m_bodies[i];
			if (b->GetType() == staticBody)
			{
				continue;
			}

			if ((b->m_flags & Body::autoSleepFlag) == 0 ||
				b->m_angularVelocity * b->m_angularVelocity > angTolSqr ||
				glm::dot(b->m_linearVelocity, b->m_linearVelocity) > linTolSqr)
			{
				b->m_sleepTime = 0.0f;
				minSleepTime = 0.0f;
			}
			else
			{
				b->m_sleepTime += h;
				minSleepTime = glm::min(minSleepTime, b->m_sleepTime);
			}
		}

		if (minSleepTime >= timeToSleep && positionSolved)
		{
			for (s32 i = 0; i < m_bodyCount; ++i)
			{
				Body* b = m_bodies[i];
				b->SetAwake(false);
			}
		}
	}
}