我的 OBB SAT 工作正常,甚至刚体求解器也适用于 AABB。它不适用于 OBB。 BoxA 不断通过 BoxB.
int Arbiter::OBBOBBIntersection(Contact *c, RigidBody *A, RigidBody *B)
{
XMVECTOR mtv = XMVectorSet(0, 0, 0, 0);
float mtd = -FLT_MAX;
int NumContacts = 0;
char str[512];
int Code, l, m;
for(l=0; l<A->GetOBB()->SupportFaceCount(); l++)
{
if(!TestAxis(A->GetOBB()->SupportFaceDirection(l), mtv, mtd, l + 1, Code))
return 0;
}
for(m=0; m<B->GetOBB()->SupportFaceCount(); m++)
{
if(!TestAxis(B->GetOBB()->SupportFaceDirection(m), mtv, mtd, l + m + 1, Code))
return 0;
}
for (int i=0; i<A->GetOBB()->SupportEdgeCount(); i++)
{
XMVECTOR iVec = A->GetOBB()->SupportEdgeDirection(i);
for (int j=0; j<B->GetOBB()->SupportEdgeCount(); j++)
{
XMVECTOR jVec = B->GetOBB()->SupportEdgeDirection(j);
XMVECTOR crossedVec = /*XMVector3Normalize*/(XMVector3Cross(iVec, jVec));
if(!TestAxis(crossedVec, mtv, mtd, l + m + 3 * i + j + 1, Code))
return 0;
}
}
sprintf_s(str, "[TEST AXIS] MTV: %.10f %.10f %.10f MTD: %.10f Code %d", mtv.m128_f32[0], mtv.m128_f32[1], mtv.m128_f32[2], mtd, Code);
string tmp2(str);
RELog::GetInstance()->WriteMessage(tmp2);
if (Code > 6)
{
XMVECTOR normal = -mtv;
float sign;
XMVECTOR pa = A->GetPosition();
for (int j=0; j<3; j++)
{
sign = (XMVector3Dot(normal, A->GetOBB()->SupportEdgeDirection(j)).m128_f32[0] > 0) ? 1.0f : -1.0f;
for (int i=0; i<3; i++)
{
pa.m128_f32[i] += sign * XMLoadFloat3(&A->GetOBB()->GetRadius()).m128_f32[j] * A->GetOBB()->SupportEdgeDirection(j).m128_f32[i];
}
}
normal = mtv;
XMVECTOR pb = B->GetPosition();
for (int j=0; j<3; j++)
{
sign = (XMVector3Dot(normal, B->GetOBB()->SupportEdgeDirection(j)).m128_f32[0] > 0) ? 1.0f : -1.0f;
for (int i=0; i<3; i++)
{
pb.m128_f32[i] += sign * XMLoadFloat3(&B->GetOBB()->GetRadius()).m128_f32[j] * B->GetOBB()->SupportEdgeDirection(j).m128_f32[i];
}
}
XMVECTOR ua = A->GetOBB()->SupportEdgeDirection(((Code)-7)/3);
XMVECTOR ub = B->GetOBB()->SupportEdgeDirection(((Code)-7)%3);
float alpha, beta;
LineClosestApproach(pa, ua, pb, ub, alpha, beta);
pa += ua*alpha;
pb += ub*beta;
c->normal = -1000*mtv;
c->position = (pa+pb)/2;
c->separation = 1000*mtd;
c->contactID.key = 2;
sprintf_s(str, "[EDGE COLLISION] [CONTACTS] ID: 1 --> Normal: %.10f %.10f %.10f, Position: %.10f %.10f %.10f, Separation: %.10f", contacts->normal.m128_f32[0], contacts->normal.m128_f32[1], contacts->normal.m128_f32[2], contacts->position.m128_f32[0], contacts->position.m128_f32[1], contacts->position.m128_f32[2], contacts->separation);
string tmp(str);
RELog::GetInstance()->WriteMessage(tmp);
}
else
{ // Face Colision
XMVECTOR verts_oriented[8], plane_center_verts_oriented[6];
XMVECTOR normal;
if (Code <= 3)
{
normal = XMVector3Normalize(-mtv);
XMFLOAT3 b_vertices[] = { XMFLOAT3(B->GetOBB()->GetCenter().x-B->GetOBB()->GetRadius().x, B->GetOBB()->GetCenter().y-B->GetOBB()->GetRadius().y, B->GetOBB()->GetCenter().z-B->GetOBB()->GetRadius().z),
XMFLOAT3(B->GetOBB()->GetCenter().x-B->GetOBB()->GetRadius().x, B->GetOBB()->GetCenter().y+B->GetOBB()->GetRadius().y, B->GetOBB()->GetCenter().z-B->GetOBB()->GetRadius().z),
XMFLOAT3(B->GetOBB()->GetCenter().x+B->GetOBB()->GetRadius().x, B->GetOBB()->GetCenter().y-B->GetOBB()->GetRadius().y, B->GetOBB()->GetCenter().z-B->GetOBB()->GetRadius().z),
XMFLOAT3(B->GetOBB()->GetCenter().x+B->GetOBB()->GetRadius().x, B->GetOBB()->GetCenter().y+B->GetOBB()->GetRadius().y, B->GetOBB()->GetCenter().z-B->GetOBB()->GetRadius().z),
XMFLOAT3(B->GetOBB()->GetCenter().x+B->GetOBB()->GetRadius().x, B->GetOBB()->GetCenter().y-B->GetOBB()->GetRadius().y, B->GetOBB()->GetCenter().z+B->GetOBB()->GetRadius().z),
XMFLOAT3(B->GetOBB()->GetCenter().x+B->GetOBB()->GetRadius().x, B->GetOBB()->GetCenter().y+B->GetOBB()->GetRadius().y, B->GetOBB()->GetCenter().z+B->GetOBB()->GetRadius().z),
XMFLOAT3(B->GetOBB()->GetCenter().x-B->GetOBB()->GetRadius().x, B->GetOBB()->GetCenter().y-B->GetOBB()->GetRadius().y, B->GetOBB()->GetCenter().z+B->GetOBB()->GetRadius().z),
XMFLOAT3(B->GetOBB()->GetCenter().x-B->GetOBB()->GetRadius().x, B->GetOBB()->GetCenter().y+B->GetOBB()->GetRadius().y, B->GetOBB()->GetCenter().z+B->GetOBB()->GetRadius().z)
};
for(int i=0;i<8;i++)
verts_oriented[i] = XMVector3Transform(XMLoadFloat3(&b_vertices[i]), B->GetOrientation());
XMFLOAT3 center_vertices[] = { XMFLOAT3(A->GetOBB()->GetCenter().x+A->GetOBB()->GetRadius().x, A->GetOBB()->GetCenter().y, A->GetOBB()->GetCenter().z),
XMFLOAT3(A->GetOBB()->GetCenter().x-A->GetOBB()->GetRadius().x, A->GetOBB()->GetCenter().y, A->GetOBB()->GetCenter().z),
XMFLOAT3(A->GetOBB()->GetCenter().x, A->GetOBB()->GetCenter().y+A->GetOBB()->GetRadius().y, A->GetOBB()->GetCenter().z),
XMFLOAT3(A->GetOBB()->GetCenter().x, A->GetOBB()->GetCenter().y-A->GetOBB()->GetRadius().y, A->GetOBB()->GetCenter().z),
XMFLOAT3(A->GetOBB()->GetCenter().x, A->GetOBB()->GetCenter().y, A->GetOBB()->GetCenter().z+A->GetOBB()->GetRadius().z),
XMFLOAT3(A->GetOBB()->GetCenter().x, A->GetOBB()->GetCenter().y, A->GetOBB()->GetCenter().z-A->GetOBB()->GetRadius().z)
};
for(int i=0;i<6;i++)
plane_center_verts_oriented[i] = XMVector3Transform(XMLoadFloat3(¢er_vertices[i]), A->GetOrientation());
}
else
{
normal = XMVector3Normalize(mtv);
XMFLOAT3 b_vertices[] = { XMFLOAT3(A->GetOBB()->GetCenter().x-A->GetOBB()->GetRadius().x, A->GetOBB()->GetCenter().y-A->GetOBB()->GetRadius().y, A->GetOBB()->GetCenter().z-A->GetOBB()->GetRadius().z),
XMFLOAT3(A->GetOBB()->GetCenter().x-A->GetOBB()->GetRadius().x, A->GetOBB()->GetCenter().y+A->GetOBB()->GetRadius().y, A->GetOBB()->GetCenter().z-A->GetOBB()->GetRadius().z),
XMFLOAT3(A->GetOBB()->GetCenter().x+A->GetOBB()->GetRadius().x, A->GetOBB()->GetCenter().y-A->GetOBB()->GetRadius().y, A->GetOBB()->GetCenter().z-A->GetOBB()->GetRadius().z),
XMFLOAT3(A->GetOBB()->GetCenter().x+A->GetOBB()->GetRadius().x, A->GetOBB()->GetCenter().y+A->GetOBB()->GetRadius().y, A->GetOBB()->GetCenter().z-A->GetOBB()->GetRadius().z),
XMFLOAT3(A->GetOBB()->GetCenter().x+A->GetOBB()->GetRadius().x, A->GetOBB()->GetCenter().y-A->GetOBB()->GetRadius().y, A->GetOBB()->GetCenter().z+A->GetOBB()->GetRadius().z),
XMFLOAT3(A->GetOBB()->GetCenter().x+A->GetOBB()->GetRadius().x, A->GetOBB()->GetCenter().y+A->GetOBB()->GetRadius().y, A->GetOBB()->GetCenter().z+A->GetOBB()->GetRadius().z),
XMFLOAT3(A->GetOBB()->GetCenter().x-A->GetOBB()->GetRadius().x, A->GetOBB()->GetCenter().y-A->GetOBB()->GetRadius().y, A->GetOBB()->GetCenter().z+A->GetOBB()->GetRadius().z),
XMFLOAT3(A->GetOBB()->GetCenter().x-A->GetOBB()->GetRadius().x, A->GetOBB()->GetCenter().y+A->GetOBB()->GetRadius().y, A->GetOBB()->GetCenter().z+A->GetOBB()->GetRadius().z)
};
for(int i=0;i<8;i++)
verts_oriented[i] = XMVector3Transform(XMLoadFloat3(&b_vertices[i]), A->GetOrientation());
XMFLOAT3 center_vertices[] = { XMFLOAT3(B->GetOBB()->GetCenter().x+B->GetOBB()->GetRadius().x, B->GetOBB()->GetCenter().y, B->GetOBB()->GetCenter().z),
XMFLOAT3(B->GetOBB()->GetCenter().x-B->GetOBB()->GetRadius().x, B->GetOBB()->GetCenter().y, B->GetOBB()->GetCenter().z),
XMFLOAT3(B->GetOBB()->GetCenter().x, B->GetOBB()->GetCenter().y+B->GetOBB()->GetRadius().y, B->GetOBB()->GetCenter().z),
XMFLOAT3(B->GetOBB()->GetCenter().x, B->GetOBB()->GetCenter().y-B->GetOBB()->GetRadius().y, B->GetOBB()->GetCenter().z),
XMFLOAT3(B->GetOBB()->GetCenter().x, B->GetOBB()->GetCenter().y, B->GetOBB()->GetCenter().z+B->GetOBB()->GetRadius().z),
XMFLOAT3(B->GetOBB()->GetCenter().x, B->GetOBB()->GetCenter().y, B->GetOBB()->GetCenter().z-B->GetOBB()->GetRadius().z)
};
for(int i=0;i<6;i++)
plane_center_verts_oriented[i] = XMVector3Transform(XMLoadFloat3(¢er_vertices[i]), B->GetOrientation());
}
vector<Plane> ReferenceFace;
if (Code == 1)
{
ReferenceFace.push_back(Plane(XMFLOAT3(A->GetOBB()->mAxisZ.x, A->GetOBB()->mAxisZ.y, A->GetOBB()->mAxisZ.z), XMVector3Dot(XMLoadFloat3(&A->GetOBB()->mAxisZ), XMLoadFloat3(&XMFLOAT3(plane_center_verts_oriented[4].m128_f32[0], plane_center_verts_oriented[4].m128_f32[1], plane_center_verts_oriented[4].m128_f32[2]))).m128_f32[0] ).Normalized());
ReferenceFace.push_back(Plane(XMFLOAT3(A->GetOBB()->mAxisY.x, A->GetOBB()->mAxisY.y, A->GetOBB()->mAxisY.z), XMVector3Dot(XMLoadFloat3(&A->GetOBB()->mAxisY), XMLoadFloat3(&XMFLOAT3(plane_center_verts_oriented[2].m128_f32[0], plane_center_verts_oriented[2].m128_f32[1], plane_center_verts_oriented[2].m128_f32[2]))).m128_f32[0] ).Normalized());
ReferenceFace.push_back(Plane(XMFLOAT3(-A->GetOBB()->mAxisZ.x, -A->GetOBB()->mAxisZ.y, -A->GetOBB()->mAxisZ.z), XMVector3Dot(-XMLoadFloat3(&A->GetOBB()->mAxisZ), XMLoadFloat3(&XMFLOAT3(plane_center_verts_oriented[5].m128_f32[0], plane_center_verts_oriented[5].m128_f32[1], plane_center_verts_oriented[5].m128_f32[2]))).m128_f32[0] ).Normalized());
ReferenceFace.push_back(Plane(XMFLOAT3(-A->GetOBB()->mAxisY.x, -A->GetOBB()->mAxisY.y, -A->GetOBB()->mAxisY.z), XMVector3Dot(-XMLoadFloat3(&A->GetOBB()->mAxisY), XMLoadFloat3(&XMFLOAT3(plane_center_verts_oriented[3].m128_f32[0], plane_center_verts_oriented[3].m128_f32[1], plane_center_verts_oriented[3].m128_f32[2]))).m128_f32[0] ).Normalized());
}
else
if (Code == 2)
{
ReferenceFace.push_back(Plane(XMFLOAT3(A->GetOBB()->mAxisX.x, A->GetOBB()->mAxisX.y, A->GetOBB()->mAxisX.z), XMVector3Dot(XMLoadFloat3(&A->GetOBB()->mAxisX), XMLoadFloat3(&XMFLOAT3(plane_center_verts_oriented[0].m128_f32[0], plane_center_verts_oriented[0].m128_f32[1], plane_center_verts_oriented[0].m128_f32[2]))).m128_f32[0] ).Normalized());
ReferenceFace.push_back(Plane(XMFLOAT3(A->GetOBB()->mAxisZ.x, A->GetOBB()->mAxisZ.y, A->GetOBB()->mAxisZ.z), XMVector3Dot(XMLoadFloat3(&A->GetOBB()->mAxisZ), XMLoadFloat3(&XMFLOAT3(plane_center_verts_oriented[4].m128_f32[0], plane_center_verts_oriented[4].m128_f32[1], plane_center_verts_oriented[4].m128_f32[2]))).m128_f32[0] ).Normalized());
ReferenceFace.push_back(Plane(XMFLOAT3(-A->GetOBB()->mAxisX.x, -A->GetOBB()->mAxisX.y, -A->GetOBB()->mAxisX.z), XMVector3Dot(-XMLoadFloat3(&A->GetOBB()->mAxisX), XMLoadFloat3(&XMFLOAT3(plane_center_verts_oriented[1].m128_f32[0], plane_center_verts_oriented[1].m128_f32[1], plane_center_verts_oriented[1].m128_f32[2]))).m128_f32[0] ).Normalized());
ReferenceFace.push_back(Plane(XMFLOAT3(-A->GetOBB()->mAxisZ.x, -A->GetOBB()->mAxisZ.y, -A->GetOBB()->mAxisZ.z), XMVector3Dot(-XMLoadFloat3(&A->GetOBB()->mAxisZ), XMLoadFloat3(&XMFLOAT3(plane_center_verts_oriented[5].m128_f32[0], plane_center_verts_oriented[5].m128_f32[1], plane_center_verts_oriented[5].m128_f32[2]))).m128_f32[0] ).Normalized());
}
else
if (Code == 3)
{
ReferenceFace.push_back(Plane(XMFLOAT3(A->GetOBB()->mAxisX.x, A->GetOBB()->mAxisX.y, A->GetOBB()->mAxisX.z), XMVector3Dot(XMLoadFloat3(&A->GetOBB()->mAxisX), XMLoadFloat3(&XMFLOAT3(plane_center_verts_oriented[0].m128_f32[0], plane_center_verts_oriented[0].m128_f32[1], plane_center_verts_oriented[0].m128_f32[2]))).m128_f32[0] ).Normalized());
ReferenceFace.push_back(Plane(XMFLOAT3(A->GetOBB()->mAxisY.x, A->GetOBB()->mAxisY.y, A->GetOBB()->mAxisY.z), XMVector3Dot(XMLoadFloat3(&A->GetOBB()->mAxisY), XMLoadFloat3(&XMFLOAT3(plane_center_verts_oriented[2].m128_f32[0], plane_center_verts_oriented[2].m128_f32[1], plane_center_verts_oriented[2].m128_f32[2]))).m128_f32[0] ).Normalized());
ReferenceFace.push_back(Plane(XMFLOAT3(-A->GetOBB()->mAxisX.x, -A->GetOBB()->mAxisX.y, -A->GetOBB()->mAxisX.z), XMVector3Dot(-XMLoadFloat3(&A->GetOBB()->mAxisX), XMLoadFloat3(&XMFLOAT3(plane_center_verts_oriented[1].m128_f32[0], plane_center_verts_oriented[1].m128_f32[1], plane_center_verts_oriented[1].m128_f32[2]))).m128_f32[0] ).Normalized());
ReferenceFace.push_back(Plane(XMFLOAT3(-A->GetOBB()->mAxisY.x, -A->GetOBB()->mAxisY.y, -A->GetOBB()->mAxisY.z), XMVector3Dot(-XMLoadFloat3(&A->GetOBB()->mAxisY), XMLoadFloat3(&XMFLOAT3(plane_center_verts_oriented[3].m128_f32[0], plane_center_verts_oriented[3].m128_f32[1], plane_center_verts_oriented[3].m128_f32[2]))).m128_f32[0] ).Normalized());
}
else
if (Code == 4)
{
ReferenceFace.push_back(Plane(XMFLOAT3(B->GetOBB()->mAxisZ.x, B->GetOBB()->mAxisZ.y, B->GetOBB()->mAxisZ.z), XMVector3Dot(XMLoadFloat3(&B->GetOBB()->mAxisZ), XMLoadFloat3(&XMFLOAT3(plane_center_verts_oriented[4].m128_f32[0], plane_center_verts_oriented[4].m128_f32[1], plane_center_verts_oriented[4].m128_f32[2]))).m128_f32[0] ).Normalized());
ReferenceFace.push_back(Plane(XMFLOAT3(B->GetOBB()->mAxisY.x, B->GetOBB()->mAxisY.y, B->GetOBB()->mAxisY.z), XMVector3Dot(XMLoadFloat3(&B->GetOBB()->mAxisY), XMLoadFloat3(&XMFLOAT3(plane_center_verts_oriented[2].m128_f32[0], plane_center_verts_oriented[2].m128_f32[1], plane_center_verts_oriented[2].m128_f32[2]))).m128_f32[0] ).Normalized());
ReferenceFace.push_back(Plane(XMFLOAT3(-B->GetOBB()->mAxisZ.x, -B->GetOBB()->mAxisZ.y, -B->GetOBB()->mAxisZ.z), XMVector3Dot(-XMLoadFloat3(&B->GetOBB()->mAxisZ), XMLoadFloat3(&XMFLOAT3(plane_center_verts_oriented[5].m128_f32[0], plane_center_verts_oriented[5].m128_f32[1], plane_center_verts_oriented[5].m128_f32[2]))).m128_f32[0] ).Normalized());
ReferenceFace.push_back(Plane(XMFLOAT3(-B->GetOBB()->mAxisY.x, -B->GetOBB()->mAxisY.y, -B->GetOBB()->mAxisY.z), XMVector3Dot(-XMLoadFloat3(&B->GetOBB()->mAxisY), XMLoadFloat3(&XMFLOAT3(plane_center_verts_oriented[3].m128_f32[0], plane_center_verts_oriented[3].m128_f32[1], plane_center_verts_oriented[3].m128_f32[2]))).m128_f32[0] ).Normalized());
}
else
if (Code == 5)
{
ReferenceFace.push_back(Plane(XMFLOAT3(B->GetOBB()->mAxisX.x, B->GetOBB()->mAxisX.y, B->GetOBB()->mAxisX.z), XMVector3Dot(XMLoadFloat3(&B->GetOBB()->mAxisX), XMLoadFloat3(&XMFLOAT3(plane_center_verts_oriented[0].m128_f32[0], plane_center_verts_oriented[0].m128_f32[1], plane_center_verts_oriented[0].m128_f32[2]))).m128_f32[0] ).Normalized());
ReferenceFace.push_back(Plane(XMFLOAT3(B->GetOBB()->mAxisZ.x, B->GetOBB()->mAxisZ.y, B->GetOBB()->mAxisZ.z), XMVector3Dot(XMLoadFloat3(&B->GetOBB()->mAxisZ), XMLoadFloat3(&XMFLOAT3(plane_center_verts_oriented[4].m128_f32[0], plane_center_verts_oriented[4].m128_f32[1], plane_center_verts_oriented[4].m128_f32[2]))).m128_f32[0] ).Normalized());
ReferenceFace.push_back(Plane(XMFLOAT3(-B->GetOBB()->mAxisX.x, -B->GetOBB()->mAxisX.y, -B->GetOBB()->mAxisX.z), XMVector3Dot(-XMLoadFloat3(&B->GetOBB()->mAxisX), XMLoadFloat3(&XMFLOAT3(plane_center_verts_oriented[1].m128_f32[0], plane_center_verts_oriented[1].m128_f32[1], plane_center_verts_oriented[1].m128_f32[2]))).m128_f32[0] ).Normalized());
ReferenceFace.push_back(Plane(XMFLOAT3(-B->GetOBB()->mAxisZ.x, -B->GetOBB()->mAxisZ.y, -B->GetOBB()->mAxisZ.z), XMVector3Dot(-XMLoadFloat3(&B->GetOBB()->mAxisZ), XMLoadFloat3(&XMFLOAT3(plane_center_verts_oriented[5].m128_f32[0], plane_center_verts_oriented[5].m128_f32[1], plane_center_verts_oriented[5].m128_f32[2]))).m128_f32[0] ).Normalized());
}
else
if (Code == 6)
{
ReferenceFace.push_back(Plane(XMFLOAT3(B->GetOBB()->mAxisX.x, B->GetOBB()->mAxisX.y, B->GetOBB()->mAxisX.z), XMVector3Dot(XMLoadFloat3(&B->GetOBB()->mAxisX), XMLoadFloat3(&XMFLOAT3(plane_center_verts_oriented[0].m128_f32[0], plane_center_verts_oriented[0].m128_f32[1], plane_center_verts_oriented[0].m128_f32[2]))).m128_f32[0] ).Normalized());
ReferenceFace.push_back(Plane(XMFLOAT3(B->GetOBB()->mAxisY.x, B->GetOBB()->mAxisY.y, B->GetOBB()->mAxisY.z), XMVector3Dot(XMLoadFloat3(&B->GetOBB()->mAxisY), XMLoadFloat3(&XMFLOAT3(plane_center_verts_oriented[2].m128_f32[0], plane_center_verts_oriented[2].m128_f32[1], plane_center_verts_oriented[2].m128_f32[2]))).m128_f32[0] ).Normalized());
ReferenceFace.push_back(Plane(XMFLOAT3(-B->GetOBB()->mAxisX.x, -B->GetOBB()->mAxisX.y, -B->GetOBB()->mAxisX.z), XMVector3Dot(-XMLoadFloat3(&B->GetOBB()->mAxisX), XMLoadFloat3(&XMFLOAT3(plane_center_verts_oriented[1].m128_f32[0], plane_center_verts_oriented[1].m128_f32[1], plane_center_verts_oriented[1].m128_f32[2]))).m128_f32[0] ).Normalized());
ReferenceFace.push_back(Plane(XMFLOAT3(-B->GetOBB()->mAxisY.x, -B->GetOBB()->mAxisY.y, -B->GetOBB()->mAxisY.z), XMVector3Dot(-XMLoadFloat3(&B->GetOBB()->mAxisY), XMLoadFloat3(&XMFLOAT3(plane_center_verts_oriented[3].m128_f32[0], plane_center_verts_oriented[3].m128_f32[1], plane_center_verts_oriented[3].m128_f32[2]))).m128_f32[0] ).Normalized());
}
int IncidentFaceCode = -1;
if (Code <= 3)
{
vector<XMVECTOR> b_supportFaceNormals;
for (int i=0; i< B->GetOBB()->SupportFaceCount(); i++)
{
XMVECTOR tmp = B->GetOBB()->SupportFaceDirection(i);
b_supportFaceNormals.push_back(tmp);
b_supportFaceNormals.push_back(-tmp);
}
float dmin = FLT_MAX;
for (int i=0; i<b_supportFaceNormals.size(); i++)
{
float compVal = XMVector3Dot(normal, b_supportFaceNormals[i]).m128_f32[0];
if (dmin > compVal)
{
dmin = compVal;
IncidentFaceCode = i + 1;
}
}
}
else
{
vector<XMVECTOR> a_supportFaceNormals;
for (int i=0; i< A->GetOBB()->SupportFaceCount(); i++)
{
XMVECTOR tmp = A->GetOBB()->SupportFaceDirection(i);
a_supportFaceNormals.push_back(tmp);
a_supportFaceNormals.push_back(-tmp);
}
float dmin = FLT_MAX;
for (int i=0; i<a_supportFaceNormals.size(); i++)
{
float compVal = XMVector3Dot(normal, a_supportFaceNormals[i]).m128_f32[0];
if (dmin > compVal)
{
dmin = compVal;
IncidentFaceCode = i + 1;
}
}
}
XMFLOAT3 verts[8];
for (int i=0;i<8;i++)
{
XMStoreFloat3(&verts[i], verts_oriented[i]);
}
std::vector<XMFLOAT3> IncidentFace;
if (IncidentFaceCode == 6)
{
IncidentFace.push_back(verts[2]);
IncidentFace.push_back(verts[3]);
IncidentFace.push_back(verts[1]);
IncidentFace.push_back(verts[0]);
}
else
if (IncidentFaceCode == 5)
{
IncidentFace.push_back(verts[6]);
IncidentFace.push_back(verts[7]);
IncidentFace.push_back(verts[5]);
IncidentFace.push_back(verts[4]);
}
else
if (IncidentFaceCode == 1)
{
IncidentFace.push_back(verts[4]);
IncidentFace.push_back(verts[5]);
IncidentFace.push_back(verts[3]);
IncidentFace.push_back(verts[2]);
}
else
if (IncidentFaceCode == 2)
{
IncidentFace.push_back(verts[0]);
IncidentFace.push_back(verts[1]);
IncidentFace.push_back(verts[7]);
IncidentFace.push_back(verts[6]);
}
else
if (IncidentFaceCode == 3)
{
IncidentFace.push_back(verts[3]);
IncidentFace.push_back(verts[5]);
IncidentFace.push_back(verts[7]);
IncidentFace.push_back(verts[1]);
}
else
if (IncidentFaceCode == 4)
{
IncidentFace.push_back(verts[4]);
IncidentFace.push_back(verts[2]);
IncidentFace.push_back(verts[0]);
IncidentFace.push_back(verts[6]);
}
if (!IncidentFace.empty() && !ReferenceFace.empty())
{
vector<XMFLOAT3> ContactPoints;
ContactPoints = SHClipPolygon(IncidentFace, ReferenceFace[0]);
if (!ContactPoints.empty())
{
NumContacts++;//1
ContactPoints = SHClipPolygon(ContactPoints, ReferenceFace[1]);
if (!ContactPoints.empty())
{
NumContacts++;//2
ContactPoints = SHClipPolygon(ContactPoints, ReferenceFace[2]);
if (!ContactPoints.empty())
{
NumContacts++;//3
ContactPoints = SHClipPolygon(ContactPoints, ReferenceFace[3]);
if (!ContactPoints.empty())
NumContacts++;//4
}
}
}
if (ContactPoints.empty())
return 0;
for (int i=0;i<ContactPoints.size();i++)
{
c[i].normal = normal;
XMVECTOR TMP = XMLoadFloat3(&ContactPoints[i]);
TMP -= normal*mtd;
XMStoreFloat3(&ContactPoints[i], TMP);
c[i].position = XMLoadFloat3(&ContactPoints[i]);
c[i].separation = mtd;
c[i].contactID.key = 1;
sprintf_s(str, "[FACE COLLISION] [CONTACTS] ID: %d --> Normal: %.10f %.10f %.10f, Position: %.10f %.10f %.10f, Separation: %.10f", i, contacts[i].normal.m128_f32[0], contacts[i].normal.m128_f32[1], contacts[i].normal.m128_f32[2], contacts[i].position.m128_f32[0], contacts[i].position.m128_f32[1], contacts[i].position.m128_f32[2], contacts[i].separation);
string tmp(str);
RELog::GetInstance()->WriteMessage(tmp);
}
}
}
return NumContacts;
}
如果我不为盒子 A(OBB) 使用方向 --> extent(1,1,1) center(1, 20, 1) AxisX, AxisY, AxisZ to i,j,k 我的刚体解算器工作(这是 Box2D Lite 的扩展),给定 Box B(OBB) --> extent(100,1,100) center (0,0,0) AxisX, AxisY, AxisZ to i,j,k。我非常努力地想弄清楚(使 sim 参数尽可能简单)我找不到问题所在。如果我为盒子 A 使用方向,盒子 A 会一直穿过盒子 B.