Bullet 学习笔记之 btCollisionWorld::performDiscreteCollisionDetection

Bullet 物理引擎能够实现多种方式的碰撞检测。其中，对场景中的所有物体进行碰撞检测，是其主要的功能之一。以下将逐步分析一下，该碰撞检测流程

---

1、碰撞检测主要步骤

在 btCollisionWorld 类中，首先向场景中添加碰撞对象，存入 m_collisionObjects 中，同时向 btCollisionObject 中关联 broad phase handler collisionObject->setBroadphaseHandle() 。之后，便可以通过执行 btCollisionWorld::performDiscreteCollisionDetection ，对场景中的所有对象进行碰撞检测。主要分为以下几个步骤：

• （1）更新 AABB
  这个很容易理解，就是更新场景中所有对象的 AABB。（当然，这里面也涉及一些预设的配置信息，这里就不赘述了。）通过函数 btCollisionWorld::updateAabbs() 完成。

• （2）broad phase collision detection
  执行碰撞检测的 broad phase 。这里应该就通过调用 btCollisionWorld::m_broadphasePairCache （即 btBroadphaseInterface 类），对场景中的所有对象，进行初步的相交检测。并将可能发生碰撞的个体对（pairs）存储，以便于后续 narrow phase 阶段，由 btCollisionWorld::m_dispatcher1 （即 btDispatcher 类）进行最终的碰撞检测。该步骤是通过函数 btCollisionWorld::computeOverlappingPairs() 完成的。

• （3）narrow phase collision detection
  执行碰撞检测的 narrow phase 。这里应该是通过调用 btCollisionWorld::m_dispatcher1 （即 btDispatcher 类），对 btCollisionWorld::m_broadphasePairCache 中存放的个体对（pairs）进行最终的碰撞检测。该步骤是通过函数 dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache->getOverlappingPairCache(), dispatchInfo, m_dispatcher1); 完成的。

---

2、Broadphase Collision Detection

这一阶段的工作，主要由 btCollisionWorld::m_broadphasePairCache （即 btBroadphaseInterface 类）类完成。参照 Basic Example 中的例子，分析一下这部分工作都是怎样完成的。

2.1 btBroadphaseInterface 类

在 Basic Example 中，Broadphase 的类为 btDbvtBroadphase ，该类继承自 btBroadphaseInterface 类。先看一下 btBroadphaseInterface 类，如下

///The btBroadphaseInterface class provides an interface to detect aabb-overlapping object pairs.
///Some implementations for this broadphase interface include btAxisSweep3, bt32BitAxisSweep3 and btDbvtBroadphase.
///The actual overlapping pair management, storage, adding and removing of pairs is dealt by the btOverlappingPairCache class.
class btBroadphaseInterface
{
public:
	virtual ~btBroadphaseInterface() {}

	virtual btBroadphaseProxy* createProxy(const btVector3& aabbMin, const btVector3& aabbMax, int shapeType, void* userPtr, int collisionFilterGroup, int collisionFilterMask, btDispatcher* dispatcher) = 0;
	virtual void destroyProxy(btBroadphaseProxy* proxy, btDispatcher* dispatcher) = 0;
	virtual void setAabb(btBroadphaseProxy* proxy, const btVector3& aabbMin, const btVector3& aabbMax, btDispatcher* dispatcher) = 0;
	virtual void getAabb(btBroadphaseProxy* proxy, btVector3& aabbMin, btVector3& aabbMax) const = 0;

	virtual void rayTest(const btVector3& rayFrom, const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin = btVector3(0, 0, 0), const btVector3& aabbMax = btVector3(0, 0, 0)) = 0;

	virtual void aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback) = 0;

	///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb
	virtual void calculateOverlappingPairs(btDispatcher* dispatcher) = 0;

	virtual btOverlappingPairCache* getOverlappingPairCache() = 0;
	virtual const btOverlappingPairCache* getOverlappingPairCache() const = 0;

	///getAabb returns the axis aligned bounding box in the 'global' coordinate frame
	///will add some transform later
	virtual void getBroadphaseAabb(btVector3& aabbMin, btVector3& aabbMax) const = 0;

	///reset broadphase internal structures, to ensure determinism/reproducability
	virtual void resetPool(btDispatcher* dispatcher) { (void)dispatcher; };

	virtual void printStats() = 0;
};

btBroadphaseInterface 类主要提供接口，包括了 overlapping pairs 的计算、射线检测 ray test 、以及 aabb test，此外还有 overlapping pairs 的存储、读写等管理。有一点不太明白的是，btBroadphaseProxy 是做什么的

2.2 btDbvtBroadphase 类之 m_paircache 及 calculateOverlappingPairs

btDbvtBroadphase 继承自 btBroadphaseInterface ，也几乎没有添加什么新的方法。更多的是多了一堆成员变量，其中，当前比较关心的是成员变量 btOverlappingPairCache* m_paircache; 和成员函数 btDbvtBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher);

首先，在初始化阶段，m_paircache 初始化为 btHashedOverlappingPairCache 对象。之后进行 overlapping pairs 检测。

void btDbvtBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
{
	collide(dispatcher);
#if DBVT_BP_PROFILE
	if (0 == (m_pid % DBVT_BP_PROFILING_RATE))
	{
		printf("fixed(%u) dynamics(%u) pairs(%u)\r\n", m_sets[1].m_leaves, m_sets[0].m_leaves, m_paircache->getNumOverlappingPairs());
		unsigned int total = m_profiling.m_total;
		if (total <= 0) total = 1;
		printf("ddcollide: %u%% (%uus)\r\n", (50 + m_profiling.m_ddcollide * 100) / total, m_profiling.m_ddcollide / DBVT_BP_PROFILING_RATE);
		printf("fdcollide: %u%% (%uus)\r\n", (50 + m_profiling.m_fdcollide * 100) / total, m_profiling.m_fdcollide / DBVT_BP_PROFILING_RATE);
		printf("cleanup:   %u%% (%uus)\r\n", (50 + m_profiling.m_cleanup * 100) / total, m_profiling.m_cleanup / DBVT_BP_PROFILING_RATE);
		printf("total:     %uus\r\n", total / DBVT_BP_PROFILING_RATE);
		const unsigned long sum = m_profiling.m_ddcollide +
								  m_profiling.m_fdcollide +
								  m_profiling.m_cleanup;
		printf("leaked: %u%% (%uus)\r\n", 100 - ((50 + sum * 100) / total), (total - sum) / DBVT_BP_PROFILING_RATE);
		printf("job counts: %u%%\r\n", (m_profiling.m_jobcount * 100) / ((m_sets[0].m_leaves + m_sets[1].m_leaves) * DBVT_BP_PROFILING_RATE));
		clear(m_profiling);
		m_clock.reset();
	}
#endif

	performDeferredRemoval(dispatcher);
}

这部分不太想去细究了。反正就是，得到的结果会存放在 btDbvtBroadphase::m_paircache 中。

2.3 broadphase 碰撞检测结果 btDbvtBroadphase::m_paircache 及 btOverlappingPairCache 类

btOverlappingPairCache 类的作用为管理（添加、删除、存储）broadphase collision detection 得到的 overlapping pair，作者对其表述如下：

//The btOverlappingPairCache provides an interface for overlapping pair management (add, remove, storage), used by the btBroadphaseInterface broadphases.
///The btHashedOverlappingPairCache and btSortedOverlappingPairCache classes are two implementations.
class btOverlappingPairCache : public btOverlappingPairCallback

其中，btOverlappingPairCache 类的主要功能有管理 overlapping pair，以及处理 overlapping pair，比如 btOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback*, btDispatcher* dispatcher)。

在 Basic Example 中，使用的是 btHashedOverlappingPairCache 类（由 btOverlappingPairCache 类派生），关于添加 overlapping pair 就是向 btHashedOverlappingPairCache::m_overlappingPairArray 中添加了一个类型为 btBroadphasePair 的新的对象。

也就是说，broadphase 碰撞检测得到的结果就是一组 btBroadphasePair ，并存放在 btHashedOverlappingPairCache::m_overlappingPairArray 中。

2.4 btBroadphasePair 以及 btBroadphaseProxy

对于 broadphase 碰撞检测阶段，得到的结果就是一组 btBroadphasePair，其定义在文件 btBroadphaseProxy.h 中。其定义可以简化为以下

///The btBroadphasePair class contains a pair of aabb-overlapping objects.
///A btDispatcher can search a btCollisionAlgorithm that performs exact/narrowphase collision detection on the actual collision shapes.
ATTRIBUTE_ALIGNED16(struct)
btBroadphasePair
{
        btBroadphaseProxy* m_pProxy0;
	btBroadphaseProxy* m_pProxy1;

	mutable btCollisionAlgorithm* m_algorithm;

        ...
}

也就是说，在每个 overlapping pair 中，包括了可能相交（准确地说只是 AABB 发生了相交）的两个对象的 proxy （即 btBroadphaseProxy 类），以及要对这两个对像进行进一步精确碰撞检测所需要执行的算法。

btBroadphaseProxy 类应该是发生相交的两个对象的代理，包括了一些信息、以及指向该对象的指针。其定义简化为

///The btBroadphaseProxy is the main class that can be used with the Bullet broadphases.
///It stores collision shape type information, collision filter information and a client object, typically a btCollisionObject or btRigidBody.
ATTRIBUTE_ALIGNED16(struct)
btBroadphaseProxy
{
	//Usually the client btCollisionObject or Rigidbody class
	void* m_clientObject;
	int m_collisionFilterGroup;
	int m_collisionFilterMask;

	int m_uniqueId;  //m_uniqueId is introduced for paircache. could get rid of this, by calculating the address offset etc.

	btVector3 m_aabbMin;
	btVector3 m_aabbMax;
}

2.5 broadphase 碰撞检测总结

在 broadphase of collision detection，通过调用 ``btBroadphaseInterface的派生类（比如btDbvtBroadphase）中的成员方法 calculateOverlappingPairs(btDispatcher* dispatcher)，对场景中所有物体进行初步碰撞检测（应该是检测 AABB 是否相交），检测得到的结果为一组btBroadphasePair，并存放在 m_paircache中（可由 btBroadphaseInterface::getXXX() 访问）。在每个btBroadphasePair` 中，存放了可能发生碰撞的两个对象的 proxy 和 narrowphase 进行碰撞检测的算法（函数指针）。而对象的 proxy 存放的则是该对象的一些信息（如碰撞标志位、类型）以及指向该对象的指针。

（此外，关于一些检测方式（比如 ray test 等）涉及到的 callback，就先不去细究了）

（感慨一下，整个碰撞检测引擎一层叠一层，挺复杂的，不过也挺精巧的。）

---

3、Narrow Phase Collision Detection

在 narrowphase of collision detection 阶段，是由 btDispatcher 的派生类（如 btCollisionDispatcher）来完成，调用了函数 dispatchAllCollisionPairs(btOverlappingPairCache* pairCache, const btDispatcherInfo& dispatchInfo, btDispatcher* dispatcher);

3.1 btDispatcher 类

在 narrow phase of collision detection 阶段，其主要接口由 btDispatcher 类定义，如下：

///The btDispatcher interface class can be used in combination with broadphase to dispatch calculations for overlapping pairs.
///For example for pairwise collision detection, calculating contact points stored in btPersistentManifold or user callbacks (game logic).
class btDispatcher
{
public:
	virtual ~btDispatcher();

	virtual btCollisionAlgorithm* findAlgorithm(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, btPersistentManifold* sharedManifold, ebtDispatcherQueryType queryType) = 0;

	virtual btPersistentManifold* getNewManifold(const btCollisionObject* b0, const btCollisionObject* b1) = 0;

	virtual void releaseManifold(btPersistentManifold* manifold) = 0;

	virtual void clearManifold(btPersistentManifold* manifold) = 0;

	virtual bool needsCollision(const btCollisionObject* body0, const btCollisionObject* body1) = 0;

	virtual bool needsResponse(const btCollisionObject* body0, const btCollisionObject* body1) = 0;

	virtual void dispatchAllCollisionPairs(btOverlappingPairCache* pairCache, const btDispatcherInfo& dispatchInfo, btDispatcher* dispatcher) = 0;

	virtual int getNumManifolds() const = 0;

	virtual btPersistentManifold* getManifoldByIndexInternal(int index) = 0;

	virtual btPersistentManifold** getInternalManifoldPointer() = 0;

	virtual btPoolAllocator* getInternalManifoldPool() = 0;

	virtual const btPoolAllocator* getInternalManifoldPool() const = 0;

	virtual void* allocateCollisionAlgorithm(int size) = 0;

	virtual void freeCollisionAlgorithm(void* ptr) = 0;
};

主要包含了对 overlapping pairs 的处理，（即，通过相应的碰撞检测算法，对 overlapping pairs 进行最终的碰撞检测）；以及对碰撞结果 btPersistentManifold 的管理。

3.2 btCollisionDispatcher 类之 m_manifoldsPtr 及 dispatchAllCollisionPairs

btCollisionDispatcher 类继承自 btDispatcher ，同样也不涉及太多新的方法。（新的方法主要是涉及碰撞检测算法矩阵相关的，这里就不再赘述了。）成员变量中，当前比较关心的是成员变量 btAlignedObjectArray<btPersistentManifold*> m_manifoldsPtr; 和成员函数 btCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache, const btDispatcherInfo& dispatchInfo, btDispatcher* dispatcher);

其中，碰撞检测得到的结果存放在 btCollisionDispatcher->m_manifoldsPtr 中；执行碰撞检测则依靠 btCollisionDispatcher::dispatchAllCollisionPairs(..)函数完成。

在碰撞检测过程中，最终执行的代码为：

void btCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache, const btDispatcherInfo& dispatchInfo, btDispatcher* dispatcher)
{
	//m_blockedForChanges = true;

	btCollisionPairCallback collisionCallback(dispatchInfo, this);

	{
		BT_PROFILE("processAllOverlappingPairs");
		pairCache->processAllOverlappingPairs(&collisionCallback, dispatcher, dispatchInfo);
	}

	//m_blockedForChanges = false;
}

（哈哈，终究还是由 pairCache 中的函数执行了。）

在 Basic Example 中，broad phase pairCache 选用的是 btHashedOverlappingPairCache 类，查找其中的 btHashedOverlappingPairCache::processAllOverlappingPairs(..) 函数发现，其执行的是如下操作

for (i = 0; i < m_overlappingPairArray.size();)
{
	btBroadphasePair* pair = &m_overlappingPairArray[i];
	if (callback->processOverlap(*pair))
	{
		removeOverlappingPair(pair->m_pProxy0, pair->m_pProxy1, dispatcher);
	}
	else
	{
		i++;
	}
}

也就是说，执行的是 callback->processOverlap(*pair) （又转回来了）

btCollisionPairCallback::processOverlap(btBroadphasePair& pair) 函数的内容是 (*m_dispatcher->getNearCallback())(pair, *m_dispatcher, m_dispatchInfo); 也就是说，最后执行的是 dispatcher 中的 m_nearCallback 函数。（这是一个函数指针，应该是需要根据不同的 pair 类型，从碰撞检测矩阵中调用函数）

下面，进一步看一下 m_nearCallback 函数指针是什么，在哪里进行了设置。

注：在 btCollisionDispatcher 类的构造函数中，将 m_nearCallback 设定为 defaultNearCallback ，也就是，最终的 narrow phase of collision detection 执行的是，对每一对 overlapping pairs 执行函数 btCollisionDispatcher::defaultNearCallback(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo)

也就是说，对于每一对 AABB 相交的 overlapping pair，它的 narrow phase 阶段，也是通过其自身（btBroadphasePair 类）的 m_algorithm 函数实现的。这个过程包括以下几个主要步骤：

// 从碰撞检测算法矩阵中选取对应的碰撞检测算法（函数指针）
collisionPair.m_algorithm = dispatcher.findAlgorithm(&obj0Wrap, &obj1Wrap, 0, BT_CONTACT_POINT_ALGORITHMS);

//discrete collision detection query
collisionPair.m_algorithm->processCollision(&obj0Wrap, &obj1Wrap, dispatchInfo, &contactPointResult);

//continuous collision detection query, time of impact (toi)
btScalar toi = collisionPair.m_algorithm->calculateTimeOfImpact(colObj0, colObj1, dispatchInfo, &contactPointResult);
if (dispatchInfo.m_timeOfImpact > toi) dispatchInfo.m_timeOfImpact = toi;

这里还有一点不明白的是，碰撞得到的结果，是如何存放到 btCollisionDispatcher->m_manifoldsPtr 中的。
解释：在所有 Algorithm 类中，有存放 m_manifoldPtr 指针。

3.3 narrow phase 碰撞检测结果 btCollisionDispatcher->m_manifoldsPtr 以及 btPersistentManifold 类 和 btManifoldPoint 类

在 narrow phase 阶段得到的碰撞检测结果，统一存放在 btCollisionDispatcher->m_manifoldsPtr 中，碰撞结果的存储类型为 btPersistentManifold 类。该类的定义（注解）为：

///btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping in the broadphase.
///Those contact points are created by the collision narrow phase.
///The cache can be empty, or hold 1,2,3 or 4 points. Some collision algorithms (GJK) might only add one point at a time.
///updates/refreshes old contact points, and throw them away if necessary (distance becomes too large)
///reduces the cache to 4 points, when more then 4 points are added, using following rules:
///the contact point with deepest penetration is always kept, and it tries to maximuze the area covered by the points
///note that some pairs of objects might have more then one contact manifold.

//ATTRIBUTE_ALIGNED128( class) btPersistentManifold : public btTypedObject
ATTRIBUTE_ALIGNED16(class)
btPersistentManifold : public btTypedObject
{
        btManifoldPoint m_pointCache[MANIFOLD_CACHE_SIZE];

	/// this two body pointers can point to the physics rigidbody class.
	const btCollisionObject* m_body0;
	const btCollisionObject* m_body1;

	int m_cachedPoints;

	btScalar m_contactBreakingThreshold;
	btScalar m_contactProcessingThreshold;
}

也就是说，当两个物体发生 overlapping 时，就会生成并保持有一个 btPersistentManifold 类，用来记录两个物体之间（可能）发生的碰撞信息，比如，两个物体的指针，碰撞点等。在 btPersistentManifold 类中，（根据真实检测到的碰撞结果）可以有 0～4 个碰撞/接触点。

对于两个物体的碰撞/接触点，则通过 btManifoldPoint 类存储。btManifoldPoint 类的定义为：

/// ManifoldContactPoint collects and maintains persistent contactpoints.
/// used to improve stability and performance of rigidbody dynamics response.
class btManifoldPoint
{
        btVector3 m_localPointA;
	btVector3 m_localPointB;
	btVector3 m_positionWorldOnB;
	///m_positionWorldOnA is redundant information, see getPositionWorldOnA(), but for clarity
	btVector3 m_positionWorldOnA;
	btVector3 m_normalWorldOnB;

	btScalar m_distance1;
	btScalar m_combinedFriction;
	btScalar m_combinedRollingFriction;   //torsional friction orthogonal to contact normal, useful to make spheres stop rolling forever
	btScalar m_combinedSpinningFriction;  //torsional friction around contact normal, useful for grasping objects
	btScalar m_combinedRestitution;

	//BP mod, store contact triangles.
	int m_partId0;
	int m_partId1;
	int m_index0;
	int m_index1;

	mutable void* m_userPersistentData;
	//bool			m_lateralFrictionInitialized;
	int m_contactPointFlags;

	btScalar m_appliedImpulse;
	btScalar m_prevRHS;
	btScalar m_appliedImpulseLateral1;
	btScalar m_appliedImpulseLateral2;
	btScalar m_contactMotion1;
	btScalar m_contactMotion2;

        btScalar m_frictionCFM;

	int m_lifeTime;  //lifetime of the contactpoint in frames

	btVector3 m_lateralFrictionDir1;
	btVector3 m_lateralFrictionDir2;
}

所记录的内容也非常的丰富。包含了两个物体之间发生碰撞的点的位置、摩擦、等等大量的数据。

3.4 narrow phase 总结

在 narrow phase of collision detection，碰撞检测的算法，由存放在 overlapping pair cache 中的算法执行，最终的结果存储在了 btCollisionDispatcher->m_manifoldsPtr 中。包含了两个物体/对象的指针，以及可能的四个接触点（0～4个），类型为 btManifoldPoint

那么，到现在为止，Bullet 引擎中，关于碰撞检测的主体框架，结果的存储位置等等，也算弄清楚一些了。后面仍需进一步弄清楚的，有刚体运动、约束求解、软体形变等等。

在 碰撞检测环节，同样有一些需要进一步弄明白的，比如，碰撞检测标志位（mask）、callback 函数的执行等，以及 raytest 等等内容。