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 等等內容。