// (C) 2006 Jagoon Software. All Rights Reserved.
// 自由に利用していただいて構いませんが、無保証です。

// -------------------------------------
// 衝突判定
// 　行列持ちAABB同士の判定を、OBB の衝突として処理。
// 　戻り値は分離軸の番号(-1 なら衝突)
// -------------------------------------

int CheckCollision(
	const D3DXVECTOR3 *pMin1, const D3DXVECTOR3 *pMax1, const D3DXMATRIX  *pMat1,
	const D3DXVECTOR3 *pMin2, const D3DXVECTOR3 *pMax2, const D3DXMATRIX  *pMat2)
{
	D3DXMATRIX ident; // 単位行列(NULL対策)

	if( pMat1 == NULL || pMat2 == NULL ) {
		D3DXMatrixIdentity( &ident );
		if( pMat1 == NULL ) pMat1 = &ident;
		if( pMat2 == NULL ) pMat2 = &ident;
	}

	D3DXVECTOR3 axis; // 判定軸
	D3DXMATRIX inv1, inv2; // 各軸の逆行列
	D3DXMATRIX AtoB, BtoA; // ローカル座標同士の変換行列

	// OBB の中心と各軸の長さ
	D3DXVECTOR3 obb_center1, obb_center2;
	D3DXVECTOR3 obb_length1, obb_length2;
	obb_center1.x = (pMin1->x + pMax1->x) / 2.0f;
	obb_center1.y = (pMin1->y + pMax1->y) / 2.0f;
	obb_center1.z = (pMin1->z + pMax1->z) / 2.0f;
	obb_center2.x = (pMin2->x + pMax2->x) / 2.0f;
	obb_center2.y = (pMin2->y + pMax2->y) / 2.0f;
	obb_center2.z = (pMin2->z + pMax2->z) / 2.0f;
	D3DXVec3Subtract( &obb_length1, pMax1, &obb_center1 );
	D3DXVec3Subtract( &obb_length2, pMax2, &obb_center2 );

	// A ローカル座標への座標系変換行列を求める
	D3DXMatrixInverse( &inv1, NULL, pMat1 );
	D3DXMatrixMultiply( &BtoA, pMat2, &inv1 );

	// A ローカル座標のB 中央の絶対位置
	D3DXVECTOR3 tB;
	D3DXVec3TransformCoord( &tB, &obb_center2, &BtoA );

	// A ローカル座標のB の長さ
	D3DXVECTOR3 lenB[3];
	for( int i = 0; i < 3; i++ ) {
		lenB[i] = obb_center2;
		if( i == 0 ) lenB[i].x += obb_length2.x;
		if( i == 1 ) lenB[i].y += obb_length2.y;
		if( i == 2 ) lenB[i].z += obb_length2.z;
		D3DXVec3TransformCoord( &lenB[i], &lenB[i], &BtoA );
		D3DXVec3Subtract( &lenB[i], &lenB[i], &tB );
	}

	// 絶対位置から相対位置への変換
	D3DXVec3Subtract( &tB, &tB, &obb_center1 );

	// A の軸 -- これはA の座標系で処理する
	for( int i = 0; i < 3; i++ ) {
		float ra, rb, rc;
#if 1
		ra = obb_length1[i];
		rb = fabs( lenB[0][i] ) + fabs( lenB[1][i] ) + fabs( lenB[2][i] );
		rc = fabs( tB[i] );
#else
		// 不効率だけど、アルゴリズムに忠実に従った方法
		axis.x = (i == 0 ? 1.0f : 0.0f);
		axis.y = (i == 1 ? 1.0f : 0.0f);
		axis.z = (i == 2 ? 1.0f : 0.0f);
		ra = D3DXVec3Dot( &axis, &obb_length1 ); // 正と正の内積は正
		rb = fabs( D3DXVec3Dot( &axis, &lenB[0] ) ) +
			 fabs( D3DXVec3Dot( &axis, &lenB[1] ) ) +
			 fabs( D3DXVec3Dot( &axis, &lenB[2] ) );
		rc = fabs( D3DXVec3Dot( &axis, &tB ) );
#endif
		if( ra + rb < rc ) {
			return i;
		}
	}

	// B ローカル座標への座標系変換行列を求める
	D3DXMatrixInverse( &inv2, NULL, pMat2 );
	D3DXMatrixMultiply( &AtoB, pMat1, &inv2 );

	// B ローカル座標のA 中央の絶対位置
	D3DXVECTOR3 tA;
	D3DXVec3TransformCoord( &tA, &obb_center1, &AtoB );

	// B ローカル座標のA の長さ
	D3DXVECTOR3 lenA[3];
	for( int i = 0; i < 3; i++ ) {
		lenA[i] = obb_center1;
		if( i == 0 ) lenA[i].x += obb_length1.x;
		if( i == 1 ) lenA[i].y += obb_length1.y;
		if( i == 2 ) lenA[i].z += obb_length1.z;
		D3DXVec3TransformCoord( &lenA[i], &lenA[i], &AtoB );
		D3DXVec3Subtract( &lenA[i], &lenA[i], &tA );
	}

	// 絶対位置から相対位置への変換
	D3DXVec3Subtract( &tA, &tA, &obb_center2 );

	// B の軸 -- これはB の座標系で処理する
	for( int i = 0; i < 3; i++ ) {
		float ra, rb, rc;
#if 1
		ra = fabs( lenA[0][i] ) + fabs( lenA[1][i] ) + fabs( lenA[2][i] );
		rb = obb_length2[i];
		rc = fabs( tA[i] );
#else
		// 不効率だけど、アルゴリズムに忠実に従った方法
		axis.x = (i == 0 ? 1.0f : 0.0f);
		axis.y = (i == 1 ? 1.0f : 0.0f);
		axis.z = (i == 2 ? 1.0f : 0.0f);

		ra = fabs( D3DXVec3Dot( &axis, &lenA[0] ) ) +
			 fabs( D3DXVec3Dot( &axis, &lenA[1] ) ) +
			 fabs( D3DXVec3Dot( &axis, &lenA[2] ) );
		rb = D3DXVec3Dot( &axis, &obb_length2 ); // 正と正の内積は正
		rc = fabs( D3DXVec3Dot( &axis, &tA ) );
#endif
		if( ra + rb < rc ) {
			return i+3;
		}
	}

	// A, B の軸に垂直な軸 -- これはA の座標系で処理する
	D3DXVECTOR3 axisA, axisB[3];
	D3DXVECTOR3 zero( 0.0f, 0.0f, 0.0f );
	D3DXVec3TransformCoord( &zero, &zero, &BtoA );

	for( int i = 0; i < 3; i++ ) {
		float ra, rb, rc;
		axisA.x = (i == 0 ? 1.0f : 0.0f);
		axisA.y = (i == 1 ? 1.0f : 0.0f);
		axisA.z = (i == 2 ? 1.0f : 0.0f);
		for( int j = 0; j < 3; j++ ) {
			if( i == 0 ) {
				// 軸ベクトルを求める
				axisB[j].x = (j == 0 ? 1.0f : 0.0f);
				axisB[j].y = (j == 1 ? 1.0f : 0.0f);
				axisB[j].z = (j == 2 ? 1.0f : 0.0f);
				D3DXVec3TransformCoord( &axisB[j], &axisB[j], &BtoA );
				D3DXVec3Subtract( &axisB[j], &axisB[j], &zero );
			}

			// 判定軸はA とB の軸の外積
			D3DXVec3Cross( &axis, &axisA, &axisB[j] );

			ra = fabs( axis[0] * obb_length1[0] ) +
				 fabs( axis[1] * obb_length1[1] ) +
				 fabs( axis[2] * obb_length1[2] );
			rb = fabs( D3DXVec3Dot( &axis, &lenB[0] ) ) +
				 fabs( D3DXVec3Dot( &axis, &lenB[1] ) ) +
				 fabs( D3DXVec3Dot( &axis, &lenB[2] ) );
			rc = fabs( D3DXVec3Dot( &axis, &tB ) );
			if( ra + rb < rc ) {
				return i*3+j+6;
			}
		}
	}

	return -1; // 接触している
}