2D 動的円と動的AABBの衝突判定メモ

前回が線分と動的AABBに続き、今回は動的円と動的AABBによる衝突判定だ!
円の判定が線分だけでは、ちょっと弱すぎだよね。ということでもう一つ追加した。
3Dのほうでも動的AABBと動的球体のはでに作っていたからそんなに移植に時間はかからんと思ったが、調整で時間を食った・・・



そして今回の実験したやつはGIFアニメーションで公開
円とAABBの頂点との衝突応答と、AABB側から動いて円に衝突応答したやつ。 衝突判定・応答を一から作ろうと思っている人は、物凄くめんどくさいから、優秀な衝突判定エンジンを使った方が吉ですよ

  だって、楽でしょw


紅!魔境村より、動的円と動的AABBの判定・応答


とりあえずメモ代わりに貼り付け! 
前回で貼り付けた関数も使用してます。このソースの説明ないから、参考にしようと思っても使えないでしょうなw
//SSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSS
//S [ 構造体名 ] SimpleCollisionResult
//S [ 説明 ]
//S 単純な接触時の情報などがはいる  
//S
//SSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSS
template<class T_VECTOR>
struct SimpleCollisionResult
{
	T_VECTOR			vIntersectionPoint;			// 交差した点
	T_VECTOR			vIntersectionNormal;		// 交差した点の法線
	float				fContactTime;				// 接触したときの時間
	union{
		_U32		uDumy;
		struct{
			_U32	_bResponse			: 1;	// 衝突応答した結果が入っているか?
			_U32	_CollisionType		: 2;	// 最終的に接触したものは?マップ?オブジェクト?
			_U32	_bFoundCollision	: 1;	// 衝突したか?
			_U32	_Ceiling_Ground		: 4;	// 衝突した場合、天井(1)か地面(2)か?
			_U32	_bParallel			: 1;	// 平行か?
			_U32	_bEmbedded			: 1;	// 埋め込み状態か
			_U32	_ContactPlace		: 3;	// 接触場所
			_U32	_ID					:13;	// 任意のID
			_U32	_BVType				: 6;	// 境界ボリュームの種類
		};
	};
public:
	SimpleCollisionResult(){
		this->Init();
	}
	inline void Init(){
		vIntersectionPoint.Init();
		fContactTime = 0.0f;
		uDumy = 0;
	}
};
#define SimpleCollisionResult3D SimpleCollisionResult<SG_VECTOR3>
#define SimpleCollisionResult2D SimpleCollisionResult<SG_VECTOR2>
//-----------------------------------------------------------------------------------
// [ 説明 ] 
//  光線R(t) = rvP + t*rVelがSG_AABB2D aに対して交差しているかどうか、交差している時、
//  交差の距離fTMinおよび交差している点qを返す
// [ 引数 ]
//  rvP		:[in] 基点
//  rVel	:[in] 運動ベクトル
//  rAABB	:[in] AABB
//  pfTMin	:[out] 接触時間
//  pQ		:[out] 接触位置
// [ 戻り値 ]
//   接触していた場合は0以外を返す
//-----------------------------------------------------------------------------------
int IntersectRayAABB(const SG_VECTOR2 &rvP, const SG_VECTOR2 &rVel, const SG_AABB2D &rAABB, float *pfTMin, SG_VECTOR2 *pQ)
{
	int i;
	bool bInside = true;
	SG_VECTOR2 vMaxT;
	vMaxT.x=vMaxT.y=-1.0f;

	// 平面候補を見つける
	for(i=0;i<2;i++)
	{
		if(rvP.v[i] < rAABB.vMin.v[i])
		{
			pQ->v[i]	= rAABB.vMin.v[i];
			bInside		= false;

			// 平面候補へのvMinT.v[i]距離を計算します。
			if(N_CAST(_U32&, rVel.v[i]))
				vMaxT.v[i] = (rAABB.vMin.v[i] - rvP.v[i]) / rVel.v[i];

		}else if(rvP.v[i] > rAABB.vMax.v[i]){
			pQ->v[i]	= rAABB.vMax.v[i];
			bInside		= false;

			// 平面候補へのvMaxT.v[i]距離を計算します。
			if(N_CAST(_U32&, rVel.v[i]))
				vMaxT.v[i] = (rAABB.vMax.v[i] - rvP.v[i]) / rVel.v[i];
		}
	}

	if(bInside){
		//SG_PLANE2D pl;
		*pQ = rvP;
		*pfTMin = 0.0f;
		return 2;
	}

	// 交差点の最終選択のためにmaxTの中で最大
	_U32 WhichPlane = 0;
	if(vMaxT.v[1] > vMaxT.v[WhichPlane])	WhichPlane = 1;

	// AABBの内部で最終候補をチェックします。
	if(N_CAST(_U32&, vMaxT.v[WhichPlane])&0x80000000) return 0;

	for(i=0;i<2;i++)
	{
		if(i!=WhichPlane)
		{
			pQ->v[i] = rvP.v[i] + vMaxT.v[WhichPlane] * rVel.v[i];
#ifdef RAYAABB_EPSILON
			if(pQ->v[i] < rAABB.vMin.v[i] - RAYAABB_EPSILON || pQ->v[i] > rAABB.vMax.v[i] + RAYAABB_EPSILON)	return 1;
#else
			if(pQ->v[i] < rAABB.vMin.v[i] || pQ->v[i] > rAABB.vMax.v[i])	return 0;
#endif
		}
	}
	*pfTMin = vMaxT.v[WhichPlane];
	return 1;
}
//-----------------------------------------------------------------------------------
// [ 説明 ] 
//  動的円と点との衝突判定
// [ 引数 ]
//  rCircle		:[in] 円
//  rVe			:[in] 速度ベクトル
//  rP			:[in] 点
//  pTime		:[in] 時間
// [ 戻り値 ]
//  1の場合辺で衝突あり、0で衝突無し
//-----------------------------------------------------------------------------------
int IntersectMovingCirclePoint(
	const SG_CIRCLE &rCircle,
	const SG_VECTOR2 &rVel,
	const SG_VECTOR2 &rP, 
	float *pTime
)
{
	float fTmpT=1.0f;

	float fA = rVel.Dot();
	float fB = 2.0f * (rCircle.c - rP).Dot(rVel);
	float fC = (rP - rCircle.c).Dot() - rCircle.r*rCircle.r;

	if( !FindLowestRootInInterval(fA, fB, fC, &fTmpT) ){
		return 0;
	}

	*pTime = fTmpT;
	return 1;
}
//-----------------------------------------------------------------------------------
// [ 説明 ] 
//  動的円とAABBとの衝突判定
// [ 引数 ]
//  rS		:[in] 円
//  rVel	:[in] 円の運動ベクトル
//  rB		:[in] AABB
//  rVB		:[in] AABBの運動ベクトル
//  pCR		:[out] SimpleCollisionResult2Dポインタ
// [ 戻り値 ]
//   接触していた場合は0以外を返す
//-----------------------------------------------------------------------------------
int IntersectMovingCircleAABB(const SG_CIRCLE &rS, const SG_VECTOR2 &rVel, const SG_AABB2D &rB, SimpleCollisionResult2D* pCR)
{
	int nR,nAABBRay;
	float fTimeT = FLT_MAX;
	// 球の半径rまでrBを拡張させた結果として得られるSG_AABB2Dを計算
	SG_AABB2D e = rB;
	e.vMin.x -= rS.r; e.vMin.y -= rS.r;
	e.vMax.x += rS.r; e.vMax.y += rS.r;

	// 光線を拡張させたSG_AABB2D eに対して交差。光線がどれもeを外れる場合、交差なしで終了
	// そうでなければ、交差点vPおよび時間tを結果として得る
	SG_VECTOR2 vP,vV;
	nAABBRay = ::IntersectRayAABB(rS.c, rVel, e, &fTimeT, &vP);
	if (nAABBRay==0 || (fTimeT > 1.0f ) )
		return 0;

	int m = 0;

	if (vP.x < rB.vMin.x )
	{
		m |= 0x01;
		vV.x = rB.vMin.x;
	}
	else if (vP.x > rB.vMax.x )
	{
		m |= 0x02;
		vV.x = rB.vMax.x;
	}

	if (vP.y < rB.vMin.y )
	{
		m |= 0x04 | (m?0x10:0);
		vV.y = rB.vMin.y;
	}
	else if (vP.y > rB.vMax.y )
	{
		m |= 0x08 | (m?0x10:0);
		vV.y = rB.vMax.y;
	}
	// vPは頂点領域にある
	if (m & 0x10) {
		pCR->fContactTime = 1.0f;

		nR = ::IntersectMovingCirclePoint(rS, rVel, vV, &pCR->fContactTime);
		if( nR == 0 )return 0; // 交差なし

		pCR->vIntersectionPoint = vV;
		pCR->_bFoundCollision	= TRUE;
		pCR->_ContactPlace		= PURIMITHIBU_CONTACTPLACE_VERTEX;

		// 法線作成
		pCR->vIntersectionNormal = rS.c-pCR->vIntersectionPoint;
		pCR->vIntersectionNormal.Normalize();

		return PURIMITHIBU_CONTACTPLACE_VERTEX;
	}
	else
	{	// 辺領域の中にある

		// 何もしない。拡張された箱との交差する時間tは
		// 正しい交差時間である
		SG_PLANE2D Plane;
		SG_VECTOR2 vMinMax;
		float fMinMax;
		fMinMax = m&0x05 ? -1.0f:1.0f;
		vMinMax = m&0x05 ? rB.vMin:rB.vMax;
		if( m <= 2 ){
			// X
			Plane.vNormal = SG_VECTOR2(fMinMax,0);
			vMinMax.y = 0;
		}else if( m >= 4  ){
			// Y
			Plane.vNormal = SG_VECTOR2(0,fMinMax);
			vMinMax.x = 0;
		}
		Plane.fDistance = Plane.vNormal.Dot(vMinMax);
		pCR->vIntersectionNormal = Plane.vNormal;

		nR = IntersectMovingCirclePlane(rS, rVel, Plane, &pCR->fContactTime,&pCR->vIntersectionPoint,false);
		if( nR == 0 )
			return 0;
		else if (nR == 2)
		{	// 円は平面に埋め込まれている
			pCR->fContactTime		= 0.0f;
			pCR->_bFoundCollision	= TRUE;
			pCR->_bEmbedded			= TRUE;
			pCR->_ContactPlace		= PURIMITHIBU_CONTACTPLACE_EDGE;
			Plane.GetClosestPtPoint(rS.c, &pCR->vIntersectionPoint);
		}
		else
		{	// 辺に接触
			pCR->_bFoundCollision	= TRUE;
			pCR->_ContactPlace		= PURIMITHIBU_CONTACTPLACE_EDGE;
		}
		return PURIMITHIBU_CONTACTPLACE_EDGE;
	}

	return 0;
}
//-----------------------------------------------------------------------------------
// [ 説明 ] 
//  動的円と動的AABBの衝突判定
// [ 引数 ]
//  rS		:[in] 円
//  rVel	:[in] 円の運動ベクトル
//  rB		:[in] AABB
//  rVB		:[in] AABBの運動ベクトル
//  pCR		:[out] SimpleCollisionResult2Dポインタ
// [ 戻り値 ]
//   接触していた場合は0以外を返す
//-----------------------------------------------------------------------------------
int IntersectMovingCircleMoveAABB(
	const SG_CIRCLE &rS, const SG_VECTOR2 &rVel,
	const SG_AABB2D &rAABB, const SG_VECTOR2 &rVB, 
	SimpleCollisionResult2D* pCR)
{
	int nRet;
	SG_VECTOR2 vNew = rVel - rVB;
	nRet = IntersectMovingCircleAABB(rS, vNew, rAABB, pCR);
	if( nRet ){
		pCR->vIntersectionPoint += rVB * pCR->fContactTime;
	}
	return nRet;
}

コメント 

コメントを残す