本系列博客是由扭曲45原創,歡迎轉載,轉載時註明出處,http://blog.csdn.net/cg0206/article/details/8441644
TOI全稱Time of Impact,中文的意思是撞擊時間,在Box2d中,我們用b2TimeOfImpact來確定兩個形狀運動時的撞擊時間(TOI)。同時b2TimeOfImpact也主要防止兩個形狀快速移動時可能在一個時間步內彼此穿越對方的情況,也就是我們經常所說的隧道效應。
我們就一起看源碼吧。
1)、b2TimeOfImpact.h文件。
// b2TimeOfImpace的輸入參數 struct b2TOIInput { b2DistanceProxy proxyA; //距離代理A b2DistanceProxy proxyB; //距離代理B b2Sweep sweepA; //掃描A b2Sweep sweepB; //掃描B float32 tMax; //定義掃頻間隔 [0, tMax] }; //b2TimeOfImpact的輸出參數 struct b2TOIOutput { enum State { e_unknown, //未知 e_failed, //失敗 e_overlapped, //重疊 e_touching, //觸碰 e_separated //分離 }; State state; //狀態 float32 t; //掃頻間隔 }; /************************************************************************** * 功能描述:在兩個形狀穿透之前,及時的求出上邊界。用分數表示時間 在[0,tMax]之間。它使用掃頻分離軸和可能丟失一些像非隧道效應碰撞的 中間體,如果你改變時間間隔,你需要重新調用這個函數 注意:使用b2Distance去求在一個撞擊時間內的接觸點和法線 * 參數說明:output:TOI輸出參數指針 input :TOI輸入參數指針 * 返 回 值: (void) **************************************************************************/ void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input);
我們可以看到此處定義了用於保存TOI信息的結構體,分別是b2TOIInput、b2TOIOutput結構體,表示碰撞時間的輸入和輸出參數。對於b2TimeOfImpact函數,則是這篇文章的主角,用於防止兩物體之間的隧道效應,關於此函數的具體情況,等到實現的時候在詳細的和大家聊聊。
2)、b2TimeOfImpact.cpp文件。
我們再來看看b2TimeOfImpact.cpp文件。爲了更好的看源碼,將分成以下三點:
- 全局變量的定義
- b2SeparationFunction結構體的實現
- b2TimeOfImpact函數的實現
1、全局變量的定義
int32 b2_toiCalls, b2_toiIters, b2_toiMaxIters; //調用次數、toi的迭代次數、toi的最大迭代次數(兩層循環中取最大的那個)
int32 b2_toiRootIters, b2_toiMaxRootIters; //根總共迭代次數、在所有根迭代中最大的那次
2、b2SeparationFunction結構體的實現
struct b2SeparationFunction
{
enum Type
{
e_points, //點
e_faceA, //面A
e_faceB //面B
};
/**************************************************************************
* 功能描述:如果不需要就返回間距值
* 參數說明:cache :單純形緩存指針
proxyA:多邊形A的指針
sweepA:掃頻對象的引用
proxyB:多邊形B的指針
sweepB:掃頻對象的引用
t1 :掃頻間隔
* 返 回 值: 間距值
**************************************************************************/
float32 Initialize(const b2SimplexCache* cache,
const b2DistanceProxy* proxyA, const b2Sweep& sweepA,
const b2DistanceProxy* proxyB, const b2Sweep& sweepB,
float32 t1)
{
//賦值代理
m_proxyA = proxyA;
m_proxyB = proxyB;
// 獲取緩存中的頂點數,並驗證
int32 count = cache->count;
b2Assert(0 < count && count < 3);
//賦值掃頻
m_sweepA = sweepA;
m_sweepB = sweepB;
//獲取變換
b2Transform xfA, xfB;
m_sweepA.GetTransform(&xfA, t1);
m_sweepB.GetTransform(&xfB, t1);
//一個頂點
if (count == 1)
{
//賦值,獲得A、B的局部頂點
m_type = e_points;
b2Vec2 localPointA = m_proxyA->GetVertex(cache->indexA[0]);
b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]);
//獲取變換後的A、B點
b2Vec2 pointA = b2Mul(xfA, localPointA);
b2Vec2 pointB = b2Mul(xfB, localPointB);
//獲取從B到的A的向量,返回其長度,並標準化
m_axis = pointB - pointA;
float32 s = m_axis.Normalize();
return s;
}
else if (cache->indexA[0] == cache->indexA[1])
{
// 兩個點在B上和一個在A上
//賦值,獲取B上的兩個局部頂點
m_type = e_faceB;
b2Vec2 localPointB1 = proxyB->GetVertex(cache->indexB[0]);
b2Vec2 localPointB2 = proxyB->GetVertex(cache->indexB[1]);
//獲取B2到B1形成向量的垂直向量,並標準化
m_axis = b2Cross(localPointB2 - localPointB1, 1.0f);
m_axis.Normalize();
//獲取法向量
b2Vec2 normal = b2Mul(xfB.q, m_axis);
// 獲取B1到B2的中間點
m_localPoint = 0.5f * (localPointB1 + localPointB2);
b2Vec2 pointB = b2Mul(xfB, m_localPoint);
// 獲取局部點A,並求得點A
b2Vec2 localPointA = proxyA->GetVertex(cache->indexA[0]);
b2Vec2 pointA = b2Mul(xfA, localPointA);
// 獲取距離
float32 s = b2Dot(pointA - pointB, normal);
// 距離爲負,置反
if (s < 0.0f)
{
m_axis = -m_axis;
s = -s;
}
return s;
}
else
{
// 兩個點在A上和一個或者兩個點在B上
m_type = e_faceA;
b2Vec2 localPointA1 = m_proxyA->GetVertex(cache->indexA[0]);
b2Vec2 localPointA2 = m_proxyA->GetVertex(cache->indexA[1]);
//獲取A2到A1形成向量的垂直向量,並標準化
m_axis = b2Cross(localPointA2 - localPointA1, 1.0f);
m_axis.Normalize();
//獲取法向量
b2Vec2 normal = b2Mul(xfA.q, m_axis);
//獲取A1和A2的中間點
m_localPoint = 0.5f * (localPointA1 + localPointA2);
b2Vec2 pointA = b2Mul(xfA, m_localPoint);
//獲取局部點,並求得點B
b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]);
b2Vec2 pointB = b2Mul(xfB, localPointB);
//獲取距離,並處理
float32 s = b2Dot(pointB - pointA, normal);
if (s < 0.0f)
{
m_axis = -m_axis;
s = -s;
}
return s;
}
}
/**************************************************************************
* 功能描述:尋找最小距離
* 參數說明:indexA :點A的索引
indexB :點B的索引
t :時間值
* 返 回 值: 最小距離
**************************************************************************/
float32 FindMinSeparation(int32* indexA, int32* indexB, float32 t) const
{
//聲明變換A、B,用於獲取在t時間裏獲得竄改變換
b2Transform xfA, xfB;
m_sweepA.GetTransform(&xfA, t);
m_sweepB.GetTransform(&xfB, t);
//處理不同的類型
switch (m_type)
{
case e_points: //點
{
//通過轉置旋轉m_axis獲取單純形支撐點的方向向量
b2Vec2 axisA = b2MulT(xfA.q, m_axis);
b2Vec2 axisB = b2MulT(xfB.q, -m_axis);
//通過方向向量獲取局部頂點的索引
*indexA = m_proxyA->GetSupport(axisA);
*indexB = m_proxyB->GetSupport(axisB);
//通過索引獲取局部頂點
b2Vec2 localPointA = m_proxyA->GetVertex(*indexA);
b2Vec2 localPointB = m_proxyB->GetVertex(*indexB);
//通過變換局部點獲取兩形狀之間的頂點
b2Vec2 pointA = b2Mul(xfA, localPointA);
b2Vec2 pointB = b2Mul(xfB, localPointB);
//求兩形狀的間距,並返回。
float32 separation = b2Dot(pointB - pointA, m_axis);
return separation;
}
case e_faceA: //面A
{
//通過轉置旋轉m_axis獲取單純形支撐點的方向向量
//通過變換局部點獲取當前圖形的點
b2Vec2 normal = b2Mul(xfA.q, m_axis);
b2Vec2 pointA = b2Mul(xfA, m_localPoint);
//通過轉置旋轉m_axis獲取單純形支撐點的方向向量
b2Vec2 axisB = b2MulT(xfB.q, -normal);
//通過索引獲取局部頂點
*indexA = -1;
*indexB = m_proxyB->GetSupport(axisB);
//通過變換局部點獲形狀B的頂點
b2Vec2 localPointB = m_proxyB->GetVertex(*indexB);
b2Vec2 pointB = b2Mul(xfB, localPointB);
//求兩形狀的間距,並返回。
float32 separation = b2Dot(pointB - pointA, normal);
return separation;
}
case e_faceB: //面B
{
//通過轉置旋轉m_axis獲取單純形支撐點的方向向量
//通過變換局部點獲取當前圖形的點
b2Vec2 normal = b2Mul(xfB.q, m_axis);
b2Vec2 pointB = b2Mul(xfB, m_localPoint);
//通過轉置旋轉m_axis獲取單純形支撐點的方向向量
b2Vec2 axisA = b2MulT(xfA.q, -normal);
//通過索引獲取局部頂點
*indexB = -1;
*indexA = m_proxyA->GetSupport(axisA);
//通過變換局部點獲形狀A的頂點
b2Vec2 localPointA = m_proxyA->GetVertex(*indexA);
b2Vec2 pointA = b2Mul(xfA, localPointA);
//求兩形狀的間距,並返回。
float32 separation = b2Dot(pointA - pointB, normal);
return separation;
}
default:
b2Assert(false);
*indexA = -1;
*indexB = -1;
return 0.0f;
}
}
/**************************************************************************
* 功能描述:當前時間步裏兩形狀的距離
* 參數說明:indexA :點A的索引
indexB :點B的索引
t :時間值
* 返 回 值: 當前時間步裏兩形狀的距離
**************************************************************************/
float32 Evaluate(int32 indexA, int32 indexB, float32 t) const
{
b2Transform xfA, xfB;
m_sweepA.GetTransform(&xfA, t);
m_sweepB.GetTransform(&xfB, t);
switch (m_type)
{
case e_points: //點
{
//通過轉置旋轉m_axis獲取頂點的方向向量
b2Vec2 axisA = b2MulT(xfA.q, m_axis);
b2Vec2 axisB = b2MulT(xfB.q, -m_axis);
//通過變換局部點獲形狀A、B的頂點
b2Vec2 localPointA = m_proxyA->GetVertex(indexA);
b2Vec2 localPointB = m_proxyB->GetVertex(indexB);
//獲取當前時間步內的兩形狀上的點
b2Vec2 pointA = b2Mul(xfA, localPointA);
b2Vec2 pointB = b2Mul(xfB, localPointB);
//計算間距,並返回間距
float32 separation = b2Dot(pointB - pointA, m_axis);
return separation;
}
case e_faceA: //面A
{
//旋轉m_axis向量,獲取法向量,同時根據局部點求形狀A上的點
b2Vec2 normal = b2Mul(xfA.q, m_axis);
b2Vec2 pointA = b2Mul(xfA, m_localPoint);
//通過轉置旋轉m_axis獲取單純形支撐點的方向向量
b2Vec2 axisB = b2MulT(xfB.q, -normal);
//通過索引獲取局部頂點,進而通過變換局部點獲取當前時間步內的點
b2Vec2 localPointB = m_proxyB->GetVertex(indexB);
b2Vec2 pointB = b2Mul(xfB, localPointB);
//獲取間距
float32 separation = b2Dot(pointB - pointA, normal);
return separation;
}
case e_faceB: //面B
{
//旋轉m_axis向量,獲取法向量,同時根據局部點求形狀B上的點
b2Vec2 normal = b2Mul(xfB.q, m_axis);
b2Vec2 pointB = b2Mul(xfB, m_localPoint);
//通過轉置旋轉m_axis獲取單純形支撐點的方向向量
b2Vec2 axisA = b2MulT(xfA.q, -normal);
//通過索引獲取局部頂點,進而通過變換局部點獲取當前時間步內的點
b2Vec2 localPointA = m_proxyA->GetVertex(indexA);
b2Vec2 pointA = b2Mul(xfA, localPointA);
//獲取間距
float32 separation = b2Dot(pointA - pointB, normal);
return separation;
}
default:
b2Assert(false);
return 0.0f;
}
}
const b2DistanceProxy* m_proxyA; //代理A
const b2DistanceProxy* m_proxyB; //代理B
b2Sweep m_sweepA, m_sweepB; //掃描A、B
Type m_type; //類型變量
b2Vec2 m_localPoint; //局部點
b2Vec2 m_axis; //方向向量,主要用於變換次向量之後求形狀的頂點
};
關於b2SeparationFunction結構體主要用於查找兩個形狀間距的相關操作。我們主要來說說其內部函數的實現。
關於Initialize函數主要初始化成員變量,並返回兩個形狀之間的距離。
關於FindMinSeparation函數主要是根據不同的單純形類型在時間步內尋找最小距離,並返回其兩個頂點的索引,作爲兩形狀是否碰撞的見證點。
關於Evaluate函數主要是根據不同的單純形類型和FindMinSeparation所查到的見證點獲取當前兩形狀的距離。
3、 b2TimeOfImpact函數的實現
//CCD(continuous collision detection,持續碰撞檢驗)經過局部的分離軸方法。
//這種尋求進展通過計算最大的時間保持分離。
void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input)
{
//調用次數自加
++b2_toiCalls;
//賦值output
output->state = b2TOIOutput::e_unknown;
output->t = input->tMax;
//獲取距離代理
const b2DistanceProxy* proxyA = &input->proxyA;
const b2DistanceProxy* proxyB = &input->proxyB;
//獲取掃頻
b2Sweep sweepA = input->sweepA;
b2Sweep sweepB = input->sweepB;
// 大型旋轉可以使根檢索器失效,所以我們標準化掃頻角度
sweepA.Normalize();
sweepB.Normalize();
//獲取掃頻間隔
float32 tMax = input->tMax;
//獲取兩個形狀半徑之和
float32 totalRadius = proxyA->m_radius + proxyB->m_radius;
float32 target = b2Max(b2_linearSlop, totalRadius - 3.0f * b2_linearSlop);
//允許誤差
float32 tolerance = 0.25f * b2_linearSlop;
//驗證有效值
b2Assert(target > tolerance);
float32 t1 = 0.0f;
//最大迭代次數
const int32 k_maxIterations = 20; // TODO_ERIN b2Settings
//
int32 iter = 0;
// 初始化距離輸入參數
b2SimplexCache cache;
cache.count = 0;
b2DistanceInput distanceInput;
distanceInput.proxyA = input->proxyA;
distanceInput.proxyB = input->proxyB;
distanceInput.useRadii = false;
// 外面的循環逐步嘗試計算新的分離軸
// 當一個軸是重複的(沒有進展),這個循環終止
for(;;)
{
b2Transform xfA, xfB;
sweepA.GetTransform(&xfA, t1);
sweepB.GetTransform(&xfB, t1);
// 獲取形狀之間的距離。我們也可以使用這個結果去獲得一個分離軸
distanceInput.transformA = xfA;
distanceInput.transformB = xfB;
b2DistanceOutput distanceOutput;
b2Distance(&distanceOutput, &cache, &distanceInput);
// 如果形狀重疊,我們放棄連續碰撞
if (distanceOutput.distance <= 0.0f)
{
//失敗!
output->state = b2TOIOutput::e_overlapped;
output->t = 0.0f;
break;
}
if (distanceOutput.distance < target + tolerance)
{
//勝利!
output->state = b2TOIOutput::e_touching;
output->t = t1;
break;
}
// 初始化分離軸
b2SeparationFunction fcn;
fcn.Initialize(&cache, proxyA, sweepA, proxyB, sweepB, t1);
#if 0
// Dump the curve seen by the root finder
{
const int32 N = 100;
float32 dx = 1.0f / N;
float32 xs[N+1];
float32 fs[N+1];
float32 x = 0.0f;
for (int32 i = 0; i <= N; ++i)
{
sweepA.GetTransform(&xfA, x);
sweepB.GetTransform(&xfB, x);
float32 f = fcn.Evaluate(xfA, xfB) - target;
printf("%g %g\n", x, f);
xs[i] = x;
fs[i] = f;
x += dx;
}
}
#endif
//在分離軸上計算TOI(碰撞時間),我們先後解決最深處的點。這個循環是以頂點數爲終止條件的
bool done = false;
float32 t2 = tMax;
int32 pushBackIter = 0;
for (;;)
{
// 在t2上查找最深點,存儲見證點索引
int32 indexA, indexB;
float32 s2 = fcn.FindMinSeparation(&indexA, &indexB, t2);
// 是否是最終的外形分離
if (s2 > target + tolerance)
{
//勝利!
output->state = b2TOIOutput::e_separated;
output->t = tMax;
done = true;
break;
}
//分離值是否達到誤差值
if (s2 > target - tolerance)
{
// 推進掃描
t1 = t2;
break;
}
// 使用見證點計算最初的間距
float32 s1 = fcn.Evaluate(indexA, indexB, t1);
// 檢驗最初重疊。有可能發生根檢索器超出了迭代總的次數的現象。
if (s1 < target - tolerance)
{
output->state = b2TOIOutput::e_failed;
output->t = t1;
done = true;
break;
}
// 檢查觸碰
if (s1 <= target + tolerance)
{
// 勝利!t1必須保留TOI(只有可能是0)
output->state = b2TOIOutput::e_touching;
output->t = t1;
done = true;
break;
}
//計算1D root : f(x) - target = 0
int32 rootIterCount = 0;
float32 a1 = t1, a2 = t2;
for (;;)
{
// 混合使用割線規則和二分法
float32 t;
if (rootIterCount & 1)
{
// 割線規則來提高收斂
t = a1 + (target - s1) * (a2 - a1) / (s2 - s1);
}
else
{
// 二分法保證進度
t = 0.5f * (a1 + a2);
}
float32 s = fcn.Evaluate(indexA, indexB, t);
if (b2Abs(s - target) < tolerance)
{
// 賦值
t2 = t;
break;
}
// 確保我們查找根
if (s > target)
{
a1 = t;
s1 = s;
}
else
{
a2 = t;
s2 = s;
}
//根迭代器
++rootIterCount;
++b2_toiRootIters;
// 循環到達50次後,退出
if (rootIterCount == 50)
{
break;
}
}
b2_toiMaxRootIters = b2Max(b2_toiMaxRootIters, rootIterCount);
//記錄頂點迭代器
++pushBackIter;
//達到頂點的最大次數,退出
if (pushBackIter == b2_maxPolygonVertices)
{
break;
}
}
//根迭代器
++iter;
//toi的迭代次數自增
++b2_toiIters;
if (done)
{
break;
}
if (iter == k_maxIterations)
{
//沒有找到根
output->state = b2TOIOutput::e_failed;
output->t = t1;
break;
}
}
//獲取toi最大迭代器
b2_toiMaxIters = b2Max(b2_toiMaxIters, iter);
}
關於b2TimeOfImpact函數,主要以3重for循環爲主線的,第一層for循環主要是逐步嘗試計算新的分離軸,並當出現一個軸是重複的時,終止循環。第二層for循環主要是在分離軸上計算TOI(碰撞時間),我們先後解決最深處的點。這個循環是以頂點數爲終止條件的。第三層for循環主要使用割線規則和二分法進行求解在t時間內,兩物體碰撞的具體的時間值。這個循環是以找到在誤差允許的範圍內的時間值或者循環50次爲終止條件的。
另外想說一下,在這裏我們每個循環的寫法是for(;;)這樣的,個人感覺不太雅緻,也不能看一眼而不用思索的就知道是死循環的寫法,如改成while(true)或者while(1)更好。
關於兩物體間是否碰撞了?在Box2d中目前我們至少知道3種可以判斷的方法,它們分別是:
- a)、通過兩物體的aabb,判斷是否重疊。
- b)、通過GJK算法算出兩物體間的距離,根據距離判斷是否碰撞
- c)、通過SAT分離軸算法看是否能找出兩物體間的分離軸,如果找得出就沒有碰撞,找不出則碰撞。
Ok,碰撞部分終於學完了,下面我們將繼續學習動力學部分。不早了,各位早安。。。
ps:
以上文章僅是一家之言,若有不妥、錯誤之處,請大家多多指出。同時也希望能與大家多多交流,共同進步。