Box2D-Lite源码阅读笔记(11)

• 简介
  这次我来看详细的运动处理函数。
• 计算准备工作
  看下面的这个函数里的实现。

void World::Step(float dt)
{
	...
	for (ArbIter arb = arbiters.begin(); arb != arbiters.end(); ++arb)
	{
		arb->second.PreStep(inv_dt);
	}
	...
}

代码如下，注释和后面都会有说明

void Arbiter::PreStep(float inv_dt)
{
	//补偿系数参数
	const float k_allowedPenetration = 0.01f;
	//补偿系数参数
	float k_biasFactor = World::positionCorrection ? 0.2f : 0.0f;
	for (int i = 0; i < numContacts; ++i)
	{
		//获取一个碰撞点
		Contact* c = contacts + i;
		//获取碰撞点到每个物体中心的向量
		Vec2 r1 = c->position - body1->position;
		Vec2 r2 = c->position - body2->position;

		//碰撞方向的冲量
		float rn1 = Dot(r1, c->normal);
		float rn2 = Dot(r2, c->normal);
		//这里的冲量计算公式
		float kNormal = body1->invMass + body2->invMass;
		kNormal += body1->invI * (Dot(r1, r1) - rn1 * rn1) + body2->invI * (Dot(r2, r2) - rn2 * rn2);
		c->massNormal = 1.0f / kNormal;

		//我推测这里是计算摩擦力方向上的冲量
		Vec2 tangent = Cross(c->normal, 1.0f);//旋转90的向量
		float rt1 = Dot(r1, tangent);
		float rt2 = Dot(r2, tangent);
		//这里是冲量计算公式
		float kTangent = body1->invMass + body2->invMass;
		kTangent += body1->invI * (Dot(r1, r1) - rt1 * rt1) + body2->invI * (Dot(r2, r2) - rt2 * rt2);
		c->massTangent = 1.0f /  kTangent;
		//补偿系数计算
		c->bias = -k_biasFactor * inv_dt * Min(0.0f, c->separation + k_allowedPenetration);
		//如果支持冲量累加
		if (World::accumulateImpulses)
		{
			// Apply normal + friction impulse
			Vec2 P = c->Pn * c->normal + c->Pt * tangent;//计算各个分量上的冲量总和
			//将冲量作用在速度和角速度上
			body1->velocity -= body1->invMass * P;
			body1->angularVelocity -= body1->invI * Cross(r1, P);

			body2->velocity += body2->invMass * P;
			body2->angularVelocity += body2->invI * Cross(r2, P);
		}
	}
}

这个里面用到了一个计算总冲量的公式。

$|J|=\frac{-(v_r\cdot \vec{n})(e+1)}{\frac{1}{m_1}+\frac{1}{m_2}+\vec{n}\cdot ((r_1\times \vec{n})/I_1)\times r_1+\vec{n}\cdot ((r_2\times \vec{n})/I_2)\times r_2}$
实际代码中的式子这里只写这一块了。
$r_1\cdot r_1-r_1\cdot \vec{n}\cdot r_1\cdot \vec{n}$
与
$\vec{n}((r_1\times \vec{n})\times r_1)$
其中n为分离轴方向向量，r为碰撞点到物体中心的向量，I为转动惯量，m为物体质量，e+1是补偿系数，没有在这段代码里体现，vr是两个物体的相对速度。
实际上是一回事，经过一系列整理就可以达到，这里就不展开说明了。
实际上预计算就是计算总冲量公式总的分母部分，后面的迭代中，分母是不变的，所以这里先计算好。