mirror of https://github.com/AxioDL/metaforce.git
Implement builtin collider tests
This commit is contained in:
Jack Andersen
parent
ff15bfbec3
commit
7063f2412d
|
|
@ -1,5 +1,7 @@
|
|||
#include "CCollidableAABox.hpp"
|
||||
#include "CollisionUtil.hpp"
|
||||
#include "CCollidableSphere.hpp"
|
||||
#include "CCollisionInfo.hpp"
|
||||
|
||||
namespace urde
|
||||
{
|
||||
|
|
@ -56,13 +58,46 @@ void CCollidableAABox::SetStaticTableIndex(u32 index)
|
|||
sTableIndex = index;
|
||||
}
|
||||
|
||||
bool CCollidableAABox::CollideMovingAABox(const CInternalCollisionStructure &, const zeus::CVector3f &, double &, CCollisionInfo &)
|
||||
bool CCollidableAABox::CollideMovingAABox(const CInternalCollisionStructure& collision, const zeus::CVector3f& dir,
|
||||
double& dOut, CCollisionInfo& infoOut)
|
||||
{
|
||||
const CCollidableAABox& p0 = static_cast<const CCollidableAABox&>(collision.GetLeft().GetPrim());
|
||||
const CCollidableAABox& p1 = static_cast<const CCollidableAABox&>(collision.GetRight().GetPrim());
|
||||
|
||||
zeus::CAABox b0 = p0.Transform(collision.GetLeft().GetTransform());
|
||||
zeus::CAABox b1 = p1.Transform(collision.GetRight().GetTransform());
|
||||
|
||||
double d;
|
||||
zeus::CVector3f point, normal;
|
||||
if (CollisionUtil::AABox_AABox_Moving(b0, b1, dir, d, point, normal) && d > 0.0 && d < dOut)
|
||||
{
|
||||
dOut = d;
|
||||
infoOut = CCollisionInfo(point, p0.GetMaterial(), p1.GetMaterial(), normal, -normal);
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool CCollidableAABox::CollideMovingSphere(const CInternalCollisionStructure &, const zeus::CVector3f &, double &, CCollisionInfo &)
|
||||
bool CCollidableAABox::CollideMovingSphere(const CInternalCollisionStructure& collision, const zeus::CVector3f& dir,
|
||||
double& dOut, CCollisionInfo& infoOut)
|
||||
{
|
||||
const CCollidableAABox& p0 = static_cast<const CCollidableAABox&>(collision.GetLeft().GetPrim());
|
||||
const CCollidableSphere& p1 = static_cast<const CCollidableSphere&>(collision.GetRight().GetPrim());
|
||||
|
||||
zeus::CAABox b0 = p0.CalculateAABox(collision.GetLeft().GetTransform());
|
||||
zeus::CSphere s1 = p1.Transform(collision.GetRight().GetTransform());
|
||||
|
||||
double d = dOut;
|
||||
zeus::CVector3f point, normal;
|
||||
if (CollisionUtil::MovingSphereAABox(s1, b0, -dir, d, point, normal) && d < dOut)
|
||||
{
|
||||
point = s1.position - s1.radius * normal;
|
||||
dOut = d;
|
||||
infoOut = CCollisionInfo(point, p0.GetMaterial(), p1.GetMaterial(), -normal);
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -1,4 +1,7 @@
|
|||
#include "CCollidableSphere.hpp"
|
||||
#include "CCollisionInfoList.hpp"
|
||||
#include "CCollidableAABox.hpp"
|
||||
#include "CollisionUtil.hpp"
|
||||
|
||||
namespace urde
|
||||
{
|
||||
|
|
@ -8,16 +11,195 @@ u32 CCollidableSphere::sTableIndex = -1;
|
|||
namespace Collide
|
||||
{
|
||||
|
||||
bool Sphere_AABox(const CInternalCollisionStructure&, CCollisionInfoList&) { return false; }
|
||||
bool Sphere_AABox(const CInternalCollisionStructure& collision, CCollisionInfoList& list)
|
||||
{
|
||||
const CCollidableSphere& p0 = static_cast<const CCollidableSphere&>(collision.GetLeft().GetPrim());
|
||||
const CCollidableAABox& p1 = static_cast<const CCollidableAABox&>(collision.GetRight().GetPrim());
|
||||
|
||||
bool Sphere_AABox_Bool(const CInternalCollisionStructure&)
|
||||
zeus::CSphere s0 = p0.Transform(collision.GetLeft().GetTransform());
|
||||
zeus::CAABox b1 = p1.Transform(collision.GetRight().GetTransform());
|
||||
|
||||
float distSq = 0.f;
|
||||
int flags = 0;
|
||||
for (int i=0 ; i<3 ; ++i)
|
||||
{
|
||||
if (s0.position[i] < b1.min[i])
|
||||
{
|
||||
if (s0.position[i] + s0.radius >= b1.min[i])
|
||||
{
|
||||
float dist = s0.position[i] - b1.min[i];
|
||||
distSq += dist * dist;
|
||||
flags |= 1 << (2 * i);
|
||||
}
|
||||
else
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else if (s0.position[i] > b1.max[i])
|
||||
{
|
||||
if (s0.position[i] - s0.radius <= b1.max[i])
|
||||
{
|
||||
float dist = s0.position[i] - b1.max[i];
|
||||
distSq += dist * dist;
|
||||
flags |= 1 << (2 * i + 1);
|
||||
}
|
||||
else
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool Sphere_Sphere(const CInternalCollisionStructure&, CCollisionInfoList&) { return false; }
|
||||
if (!flags)
|
||||
{
|
||||
zeus::CVector3f normal = (s0.position - b1.center()).normalized();
|
||||
zeus::CVector3f point = s0.position + normal * s0.radius;
|
||||
CCollisionInfo info(point, p0.GetMaterial(), p1.GetMaterial(), normal);
|
||||
list.Add(info, false);
|
||||
return true;
|
||||
}
|
||||
|
||||
if (distSq > s0.radius * s0.radius)
|
||||
return false;
|
||||
|
||||
zeus::CVector3f point;
|
||||
switch (flags)
|
||||
{
|
||||
case 0x1a:
|
||||
point = zeus::CVector3f(b1.max.x, b1.max.y, b1.min.z);
|
||||
break;
|
||||
case 0x19:
|
||||
point = zeus::CVector3f(b1.min.x, b1.max.y, b1.min.z);
|
||||
break;
|
||||
case 0x16:
|
||||
point = zeus::CVector3f(b1.max.x, b1.min.y, b1.min.z);
|
||||
break;
|
||||
case 0x15:
|
||||
point = zeus::CVector3f(b1.min.x, b1.min.y, b1.min.z);
|
||||
break;
|
||||
case 0x2a:
|
||||
point = zeus::CVector3f(b1.max.x, b1.max.y, b1.max.z);
|
||||
break;
|
||||
case 0x29:
|
||||
point = zeus::CVector3f(b1.min.x, b1.max.y, b1.max.z);
|
||||
break;
|
||||
case 0x26:
|
||||
point = zeus::CVector3f(b1.max.x, b1.min.y, b1.max.z);
|
||||
break;
|
||||
case 0x25:
|
||||
point = zeus::CVector3f(b1.min.x, b1.min.y, b1.max.z);
|
||||
break;
|
||||
case 0x11:
|
||||
point = zeus::CVector3f(b1.min.x, s0.position.y, b1.min.z);
|
||||
break;
|
||||
case 0x12:
|
||||
point = zeus::CVector3f(b1.max.x, s0.position.y, b1.min.z);
|
||||
break;
|
||||
case 0x14:
|
||||
point = zeus::CVector3f(s0.position.x, b1.min.y, b1.min.z);
|
||||
break;
|
||||
case 0x18:
|
||||
point = zeus::CVector3f(s0.position.x, b1.max.y, b1.min.z);
|
||||
break;
|
||||
case 0x5:
|
||||
point = zeus::CVector3f(b1.min.x, b1.min.y, s0.position.z);
|
||||
break;
|
||||
case 0x6:
|
||||
point = zeus::CVector3f(b1.max.x, b1.min.y, s0.position.z);
|
||||
break;
|
||||
case 0x9:
|
||||
point = zeus::CVector3f(b1.min.x, b1.max.y, s0.position.z);
|
||||
break;
|
||||
case 0xa:
|
||||
point = zeus::CVector3f(b1.max.x, b1.max.y, s0.position.z);
|
||||
break;
|
||||
case 0x21:
|
||||
point = zeus::CVector3f(b1.min.x, s0.position.y, b1.max.z);
|
||||
break;
|
||||
case 0x22:
|
||||
point = zeus::CVector3f(b1.max.x, s0.position.y, b1.max.z);
|
||||
break;
|
||||
case 0x24:
|
||||
point = zeus::CVector3f(s0.position.x, b1.min.y, b1.max.z);
|
||||
break;
|
||||
case 0x28:
|
||||
point = zeus::CVector3f(s0.position.x, b1.max.y, b1.max.z);
|
||||
break;
|
||||
case 0x1:
|
||||
point = zeus::CVector3f(b1.min.x, s0.position.y, s0.position.z);
|
||||
break;
|
||||
case 0x2:
|
||||
point = zeus::CVector3f(b1.max.x, s0.position.y, s0.position.z);
|
||||
break;
|
||||
case 0x4:
|
||||
point = zeus::CVector3f(s0.position.x, b1.min.y, s0.position.z);
|
||||
break;
|
||||
case 0x8:
|
||||
point = zeus::CVector3f(s0.position.x, b1.max.y, s0.position.z);
|
||||
break;
|
||||
case 0x10:
|
||||
point = zeus::CVector3f(s0.position.x, s0.position.y, b1.min.z);
|
||||
break;
|
||||
case 0x20:
|
||||
point = zeus::CVector3f(s0.position.x, s0.position.y, b1.max.z);
|
||||
break;
|
||||
default: break;
|
||||
}
|
||||
|
||||
CCollisionInfo info(point, p0.GetMaterial(), p1.GetMaterial(), (s0.position - point).normalized());
|
||||
list.Add(info, false);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Sphere_AABox_Bool(const CInternalCollisionStructure& collision)
|
||||
{
|
||||
const CCollidableSphere& p0 = static_cast<const CCollidableSphere&>(collision.GetLeft().GetPrim());
|
||||
const CCollidableAABox& p1 = static_cast<const CCollidableAABox&>(collision.GetRight().GetPrim());
|
||||
|
||||
zeus::CSphere s0 = p0.Transform(collision.GetLeft().GetTransform());
|
||||
zeus::CAABox b1 = p1.Transform(collision.GetRight().GetTransform());
|
||||
|
||||
return CCollidableSphere::Sphere_AABox_Bool(s0, b1);
|
||||
}
|
||||
|
||||
bool Sphere_Sphere(const CInternalCollisionStructure& collision, CCollisionInfoList& list)
|
||||
{
|
||||
const CCollidableSphere& p0 = static_cast<const CCollidableSphere&>(collision.GetLeft().GetPrim());
|
||||
const CCollidableSphere& p1 = static_cast<const CCollidableSphere&>(collision.GetRight().GetPrim());
|
||||
|
||||
zeus::CSphere s0 = p0.Transform(collision.GetLeft().GetTransform());
|
||||
zeus::CSphere s1 = p1.Transform(collision.GetRight().GetTransform());
|
||||
|
||||
float radiusSum = s0.radius + s1.radius;
|
||||
zeus::CVector3f delta = s0.position - s1.position;
|
||||
float deltaMagSq = delta.magSquared();
|
||||
if (deltaMagSq <= radiusSum * radiusSum)
|
||||
{
|
||||
zeus::CVector3f deltaNorm = delta.canBeNormalized() ?
|
||||
(1.f / std::sqrt(deltaMagSq)) * delta : zeus::CVector3f::skRight;
|
||||
zeus::CVector3f collisionPoint = deltaNorm * s1.radius + s1.position;
|
||||
CCollisionInfo info(collisionPoint, p0.GetMaterial(), p1.GetMaterial(), deltaNorm);
|
||||
list.Add(info, false);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool Sphere_Sphere_Bool(const CInternalCollisionStructure& collision)
|
||||
{
|
||||
const CCollidableSphere& p0 = static_cast<const CCollidableSphere&>(collision.GetLeft().GetPrim());
|
||||
const CCollidableSphere& p1 = static_cast<const CCollidableSphere&>(collision.GetRight().GetPrim());
|
||||
|
||||
zeus::CSphere s0 = p0.Transform(collision.GetLeft().GetTransform());
|
||||
zeus::CSphere s1 = p1.Transform(collision.GetRight().GetTransform());
|
||||
|
||||
float radiusSum = s0.radius + s1.radius;
|
||||
return (s0.position - s1.position).magSquared() <= radiusSum * radiusSum;
|
||||
}
|
||||
|
||||
bool Sphere_Sphere_Bool(const CInternalCollisionStructure&) { return false; }
|
||||
}
|
||||
|
||||
CCollidableSphere::CCollidableSphere(const zeus::CSphere& sphere, const CMaterialList& list)
|
||||
|
|
@ -51,15 +233,46 @@ const CCollisionPrimitive::Type& CCollidableSphere::GetType() { return sType; }
|
|||
|
||||
void CCollidableSphere::SetStaticTableIndex(u32 index) { sTableIndex = index; }
|
||||
|
||||
bool CCollidableSphere::CollideMovingAABox(const CInternalCollisionStructure&, const zeus::CVector3f&, double&,
|
||||
CCollisionInfo&)
|
||||
bool CCollidableSphere::CollideMovingAABox(const CInternalCollisionStructure& collision, const zeus::CVector3f& dir,
|
||||
double& dOut, CCollisionInfo& infoOut)
|
||||
{
|
||||
const CCollidableSphere& p0 = static_cast<const CCollidableSphere&>(collision.GetLeft().GetPrim());
|
||||
const CCollidableAABox& p1 = static_cast<const CCollidableAABox&>(collision.GetRight().GetPrim());
|
||||
|
||||
zeus::CSphere s0 = p0.Transform(collision.GetLeft().GetTransform());
|
||||
zeus::CAABox b1 = p1.CalculateAABox(collision.GetRight().GetTransform());
|
||||
|
||||
double d = dOut;
|
||||
zeus::CVector3f point, normal;
|
||||
if (CollisionUtil::MovingSphereAABox(s0, b1, dir, d, point, normal) && d < dOut)
|
||||
{
|
||||
dOut = d;
|
||||
infoOut = CCollisionInfo(point, p0.GetMaterial(), p1.GetMaterial(), normal);
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool CCollidableSphere::CollideMovingSphere(const CInternalCollisionStructure&, const zeus::CVector3f&, double&,
|
||||
CCollisionInfo&)
|
||||
bool CCollidableSphere::CollideMovingSphere(const CInternalCollisionStructure& collision, const zeus::CVector3f& dir,
|
||||
double& dOut, CCollisionInfo& infoOut)
|
||||
{
|
||||
const CCollidableSphere& p0 = static_cast<const CCollidableSphere&>(collision.GetLeft().GetPrim());
|
||||
const CCollidableSphere& p1 = static_cast<const CCollidableSphere&>(collision.GetRight().GetPrim());
|
||||
|
||||
zeus::CSphere s0 = p0.Transform(collision.GetLeft().GetTransform());
|
||||
zeus::CSphere s1 = p1.Transform(collision.GetRight().GetTransform());
|
||||
|
||||
double d = dOut;
|
||||
if (CollisionUtil::RaySphereIntersection_Double(zeus::CSphere(s1.position, s0.radius + s1.radius),
|
||||
s0.position, dir, d) && d >= 0.0 && d < dOut)
|
||||
{
|
||||
dOut = d;
|
||||
zeus::CVector3f normal = (s0.position + float(d) * dir - s1.position).normalized();
|
||||
infoOut = CCollisionInfo(s1.position + s1.radius * normal, p0.GetMaterial(), p1.GetMaterial(), normal);
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -960,7 +960,7 @@ bool CMetroidAreaCollider::MovingSphereCollisionCheck_Cached(const COctreeLeafCa
|
|||
}
|
||||
|
||||
bool intersects = (sphere.position - surf.GetVert(0)).dot(surfNormal) <= sphere.radius;
|
||||
bool x49c[] = {true, true, true};
|
||||
bool testVert[] = {true, true, true};
|
||||
const u16* edgeIndices = node.GetOwner().GetTriangleEdgeIndices(triIdx);
|
||||
for (int k=0 ; k<3 ; ++k)
|
||||
{
|
||||
|
|
@ -1002,23 +1002,23 @@ bool CMetroidAreaCollider::MovingSphereCollisionCheck_Cached(const COctreeLeafCa
|
|||
dOut = mag;
|
||||
triRet = true;
|
||||
ret = true;
|
||||
x49c[k] = false;
|
||||
x49c[nextIdx] = false;
|
||||
testVert[k] = false;
|
||||
testVert[nextIdx] = false;
|
||||
}
|
||||
else if (t < -sphere.radius && dirDotEdge <= 0.f)
|
||||
{
|
||||
x49c[k] = false;
|
||||
testVert[k] = false;
|
||||
}
|
||||
else if (t > edgeVecMag + sphere.radius && dirDotEdge >= 0.0)
|
||||
{
|
||||
x49c[nextIdx] = false;
|
||||
testVert[nextIdx] = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
x49c[k] = false;
|
||||
x49c[nextIdx] = false;
|
||||
testVert[k] = false;
|
||||
testVert[nextIdx] = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -1029,7 +1029,7 @@ bool CMetroidAreaCollider::MovingSphereCollisionCheck_Cached(const COctreeLeafCa
|
|||
for (int k=0 ; k<3 ; ++k)
|
||||
{
|
||||
u16 vertIdx = vertIndices[k];
|
||||
if (x49c[k])
|
||||
if (testVert[k])
|
||||
{
|
||||
if (g_DupPrimitiveCheckCount != g_DupVertexList[vertIdx])
|
||||
{
|
||||
|
|
|
|||
|
|
@ -328,16 +328,65 @@ bool AABoxAABoxIntersection(const zeus::CAABox& aabb0, const CMaterialList& list
|
|||
const zeus::CAABox& aabb1, const CMaterialList& list1,
|
||||
CCollisionInfoList& infoList)
|
||||
{
|
||||
zeus::CAABox boolAABB = aabb0.booleanIntersection(aabb1);
|
||||
if (boolAABB.invalid())
|
||||
zeus::CVector3f maxOfMin(std::max(aabb0.min.x, aabb1.min.x),
|
||||
std::max(aabb0.min.y, aabb1.min.y),
|
||||
std::max(aabb0.min.z, aabb1.min.z));
|
||||
zeus::CVector3f minOfMax(std::min(aabb0.max.x, aabb1.max.x),
|
||||
std::min(aabb0.max.y, aabb1.max.y),
|
||||
std::min(aabb0.max.z, aabb1.max.z));
|
||||
|
||||
if (maxOfMin.x >= minOfMax.x || maxOfMin.y >= minOfMax.y || maxOfMin.z >= minOfMax.z)
|
||||
return false;
|
||||
|
||||
/* TODO: Finish */
|
||||
zeus::CAABox boolAABB(maxOfMin, minOfMax);
|
||||
|
||||
if (!infoList.GetCount())
|
||||
int ineqFlags[] =
|
||||
{
|
||||
infoList.Add(CCollisionInfo(boolAABB, list0, list1, AABBNormalTable[4], -AABBNormalTable[4]), false);
|
||||
infoList.Add(CCollisionInfo(boolAABB, list0, list1, AABBNormalTable[5], -AABBNormalTable[5]), false);
|
||||
(aabb0.min.x <= aabb1.min.x ? 1 << 0 : 0) |
|
||||
(aabb0.min.x <= aabb1.max.x ? 1 << 1 : 0) |
|
||||
(aabb0.max.x <= aabb1.min.x ? 1 << 2 : 0) |
|
||||
(aabb0.max.x <= aabb1.max.x ? 1 << 3 : 0),
|
||||
(aabb0.min.y <= aabb1.min.y ? 1 << 0 : 0) |
|
||||
(aabb0.min.y <= aabb1.max.y ? 1 << 1 : 0) |
|
||||
(aabb0.max.y <= aabb1.min.y ? 1 << 2 : 0) |
|
||||
(aabb0.max.y <= aabb1.max.y ? 1 << 3 : 0),
|
||||
(aabb0.min.z <= aabb1.min.z ? 1 << 0 : 0) |
|
||||
(aabb0.min.z <= aabb1.max.z ? 1 << 1 : 0) |
|
||||
(aabb0.max.z <= aabb1.min.z ? 1 << 2 : 0) |
|
||||
(aabb0.max.z <= aabb1.max.z ? 1 << 3 : 0),
|
||||
};
|
||||
|
||||
for (int i=0 ; i<3 ; ++i)
|
||||
{
|
||||
switch (ineqFlags[i])
|
||||
{
|
||||
case 0x2: // aabb0.min <= aabb1.max
|
||||
{
|
||||
CCollisionInfo info(boolAABB, list0, list1, AABBNormalTable[i*2+1], -AABBNormalTable[i*2+1]);
|
||||
infoList.Add(info, false);
|
||||
break;
|
||||
}
|
||||
case 0xB: // aabb0.min <= aabb1.min && aabb0.max <= aabb1.min && aabb0.max <= aabb1.max
|
||||
{
|
||||
CCollisionInfo info(boolAABB, list0, list1, AABBNormalTable[i*2], -AABBNormalTable[i*2]);
|
||||
infoList.Add(info, false);
|
||||
break;
|
||||
}
|
||||
default: break;
|
||||
}
|
||||
}
|
||||
|
||||
if (infoList.GetCount())
|
||||
return true;
|
||||
|
||||
{
|
||||
CCollisionInfo info(boolAABB, list0, list1, AABBNormalTable[4], -AABBNormalTable[4]);
|
||||
infoList.Add(info, false);
|
||||
}
|
||||
|
||||
{
|
||||
CCollisionInfo info(boolAABB, list0, list1, AABBNormalTable[5], -AABBNormalTable[5]);
|
||||
infoList.Add(info, false);
|
||||
}
|
||||
|
||||
return true;
|
||||
|
|
@ -776,5 +825,354 @@ bool TriSphereIntersection(const zeus::CSphere& sphere,
|
|||
return true;
|
||||
}
|
||||
|
||||
bool BoxLineTest(const zeus::CAABox& aabb, const zeus::CVector3f& point, const zeus::CVector3f& dir,
|
||||
float& tMin, float& tMax, int& axis, bool& sign)
|
||||
{
|
||||
tMin = -999999.f;
|
||||
tMax = 999999.f;
|
||||
|
||||
for (int i=0 ; i<3 ; ++i)
|
||||
{
|
||||
if (dir[i] == 0.f)
|
||||
if (point[i] < aabb.min[i] || point[i] > aabb.max[i])
|
||||
return false;
|
||||
|
||||
float dirRecip = 1.f / dir[i];
|
||||
float tmpMin, tmpMax;
|
||||
if (dir[i] < 0.f)
|
||||
{
|
||||
tmpMin = (aabb.max[i] - point[i]) * dirRecip;
|
||||
tmpMax = (aabb.min[i] - point[i]) * dirRecip;
|
||||
}
|
||||
else
|
||||
{
|
||||
tmpMin = (aabb.min[i] - point[i]) * dirRecip;
|
||||
tmpMax = (aabb.max[i] - point[i]) * dirRecip;
|
||||
}
|
||||
|
||||
if (tmpMin > tMin)
|
||||
{
|
||||
sign = dir[i] < 0.f;
|
||||
axis = i;
|
||||
tMin = tmpMin;
|
||||
}
|
||||
|
||||
if (tmpMax < tMax)
|
||||
tMax = tmpMax;
|
||||
}
|
||||
|
||||
return tMin <= tMax;
|
||||
}
|
||||
|
||||
bool LineCircleIntersection2d(const zeus::CVector3f& point, const zeus::CVector3f& dir, const zeus::CSphere& sphere,
|
||||
int axis1, int axis2, float& d)
|
||||
{
|
||||
zeus::CVector3f delta = sphere.position - point;
|
||||
zeus::CVector2f deltaVec(delta[axis1], delta[axis2]);
|
||||
zeus::CVector2f dirVec(dir[axis1], dir[axis2]);
|
||||
|
||||
float dirVecMag = dirVec.magnitude();
|
||||
if (dirVecMag < FLT_EPSILON)
|
||||
return false;
|
||||
|
||||
float deltaVecDot = deltaVec.dot(dirVec / dirVecMag);
|
||||
float deltaVecMagSq = deltaVec.magSquared();
|
||||
|
||||
float sphereRadSq = sphere.radius * sphere.radius;
|
||||
if (deltaVecDot < 0.f && deltaVecMagSq > sphereRadSq)
|
||||
return false;
|
||||
|
||||
float tSq = sphereRadSq - (deltaVecMagSq - deltaVecDot * deltaVecDot);
|
||||
if (tSq < 0.f)
|
||||
return false;
|
||||
|
||||
float t = std::sqrt(tSq);
|
||||
|
||||
d = (deltaVecMagSq > sphereRadSq) ? deltaVecDot - t : deltaVecDot + t;
|
||||
d /= dirVecMag;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool MovingSphereAABox(const zeus::CSphere& sphere, const zeus::CAABox& aabb, const zeus::CVector3f& dir,
|
||||
double& dOut, zeus::CVector3f& point, zeus::CVector3f& normal)
|
||||
{
|
||||
zeus::CAABox expAABB(aabb.min - sphere.radius, aabb.max + sphere.radius);
|
||||
float tMin, tMax;
|
||||
int axis;
|
||||
bool sign;
|
||||
if (!BoxLineTest(expAABB, sphere.position, dir, tMin, tMax, axis, sign))
|
||||
return false;
|
||||
|
||||
point = sphere.position + tMin * dir;
|
||||
|
||||
int nextAxis1 = (axis+1) % 3; // r0
|
||||
int nextAxis2 = (axis+2) % 3; // r5
|
||||
|
||||
bool inMin1 = point[nextAxis1] >= aabb.min[nextAxis1]; // r6
|
||||
bool inMax1 = point[nextAxis1] <= aabb.max[nextAxis1]; // r8
|
||||
bool inBounds1 = inMin1 && inMax1; // r9
|
||||
bool inMin2 = point[nextAxis2] >= aabb.min[nextAxis2]; // r7
|
||||
bool inMax2 = point[nextAxis2] <= aabb.max[nextAxis2]; // r4
|
||||
bool inBounds2 = inMin2 && inMax2; // r8
|
||||
|
||||
if (inBounds1 && inBounds2)
|
||||
{
|
||||
if (tMin < 0.f || tMin > dOut)
|
||||
return false;
|
||||
normal[axis] = sign ? 1.f : -1.f;
|
||||
dOut = tMin;
|
||||
point -= normal * sphere.radius;
|
||||
return true;
|
||||
}
|
||||
else if (!inBounds1 && !inBounds2)
|
||||
{
|
||||
int pointFlags = (1 << axis) * sign | (1 << nextAxis1) * inMin1 | (1 << nextAxis2) * inMin2;
|
||||
zeus::CVector3f aabbPoint = aabb.getPoint(pointFlags);
|
||||
float d;
|
||||
if (CollisionUtil::RaySphereIntersection(zeus::CSphere(aabbPoint, sphere.radius),
|
||||
sphere.position, dir, dOut, d, point))
|
||||
{
|
||||
int useAxis = -1;
|
||||
for (int i=0 ; i<3 ; ++i)
|
||||
{
|
||||
if ((pointFlags & (1 << i)) ? aabbPoint[i] > point[i] : aabbPoint[i] < point[i])
|
||||
{
|
||||
useAxis = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (useAxis == -1)
|
||||
{
|
||||
normal = (point - aabbPoint).normalized();
|
||||
point -= sphere.radius * normal;
|
||||
return true;
|
||||
}
|
||||
|
||||
int useAxisNext1 = (useAxis+1) % 3;
|
||||
int useAxisNext2 = (useAxis+2) % 3;
|
||||
|
||||
float d;
|
||||
if (CollisionUtil::LineCircleIntersection2d(sphere.position, dir, zeus::CSphere(aabbPoint, sphere.radius),
|
||||
useAxisNext1, useAxisNext2, d) && d > 0.f && d < dOut)
|
||||
{
|
||||
if (point[useAxis] > aabb.max[useAxis])
|
||||
{
|
||||
int useAxisBit = 1 << useAxis;
|
||||
if (pointFlags & useAxisBit)
|
||||
return false;
|
||||
|
||||
zeus::CVector3f aabbPoint1 = aabb.getPoint(pointFlags | useAxisBit);
|
||||
if (CollisionUtil::RaySphereIntersection(zeus::CSphere(aabbPoint1, sphere.radius),
|
||||
sphere.position, dir, dOut, d, point))
|
||||
{
|
||||
dOut = d;
|
||||
normal = (point - aabbPoint1).normalized();
|
||||
point -= normal * sphere.radius;
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else if (point[useAxis] < aabb.min[useAxis])
|
||||
{
|
||||
int useAxisBit = 1 << useAxis;
|
||||
if (!(pointFlags & useAxisBit))
|
||||
return false;
|
||||
|
||||
zeus::CVector3f aabbPoint1 = aabb.getPoint(pointFlags ^ useAxisBit);
|
||||
if (CollisionUtil::RaySphereIntersection(zeus::CSphere(aabbPoint1, sphere.radius),
|
||||
sphere.position, dir, dOut, d, point))
|
||||
{
|
||||
dOut = d;
|
||||
normal = (point - aabbPoint1).normalized();
|
||||
point -= normal * sphere.radius;
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
normal = point - aabbPoint;
|
||||
normal.normalize();
|
||||
point -= normal * sphere.radius;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
int reverseCount = 0;
|
||||
float dMin = 1.0e10f;
|
||||
int minAxis = 0;
|
||||
for (int i=0 ; i<3 ; ++i)
|
||||
{
|
||||
if (std::fabs(dir[i]) > FLT_EPSILON)
|
||||
{
|
||||
bool pointMax = pointFlags & (1 << i);
|
||||
if (pointMax != dir[i] > 0.f)
|
||||
{
|
||||
++reverseCount;
|
||||
float d = 1.f / dir[i] * ((pointMax ? aabb.max[i] : aabb.min[i]) - sphere.position[i]);
|
||||
if (d < 0.f)
|
||||
return false;
|
||||
if (d < dMin)
|
||||
{
|
||||
dMin = d;
|
||||
minAxis = i;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (reverseCount < 2)
|
||||
return false;
|
||||
|
||||
int useAxisNext1 = (minAxis+1) % 3;
|
||||
int useAxisNext2 = (minAxis+2) % 3;
|
||||
float d;
|
||||
if (CollisionUtil::LineCircleIntersection2d(sphere.position, dir, zeus::CSphere(aabbPoint, sphere.radius),
|
||||
useAxisNext1, useAxisNext2, d) && d > 0.f && d < dOut)
|
||||
{
|
||||
point = sphere.position + d * dir;
|
||||
if (point[minAxis] > aabb.max[minAxis])
|
||||
return false;
|
||||
if (point[minAxis] < aabb.min[minAxis])
|
||||
return false;
|
||||
|
||||
dOut = d;
|
||||
normal = point - aabbPoint;
|
||||
normal.normalize();
|
||||
point -= sphere.radius * normal;
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool useNextAxis1 = inBounds1 ? nextAxis2 : nextAxis1;
|
||||
bool useNextAxis2 = inBounds1 ? nextAxis1 : nextAxis2;
|
||||
|
||||
int pointFlags = ((1 << useNextAxis1) * (inBounds1 ? inMin2 : inMin1)) | ((1 << axis) * sign);
|
||||
zeus::CVector3f aabbPoint2 = aabb.getPoint(pointFlags);
|
||||
float d;
|
||||
if (LineCircleIntersection2d(sphere.position, dir, zeus::CSphere(aabbPoint2, sphere.radius),
|
||||
axis, useNextAxis1, d) && d > 0.f && d < dOut)
|
||||
{
|
||||
point = sphere.position + d * dir;
|
||||
if (point[useNextAxis2] > aabb.max[useNextAxis2])
|
||||
{
|
||||
zeus::CVector3f aabbPoint3 = aabb.getPoint(pointFlags | (1 << useNextAxis2));
|
||||
if (point[useNextAxis2] < expAABB.max[useNextAxis2])
|
||||
{
|
||||
if (RaySphereIntersection(zeus::CSphere(aabbPoint3, sphere.radius),
|
||||
sphere.position, dir, dOut, d, point))
|
||||
{
|
||||
dOut = d;
|
||||
normal = (point - aabbPoint3).normalized();
|
||||
point -= sphere.radius * normal;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
else if (point[useNextAxis2] < aabb.min[useNextAxis2])
|
||||
{
|
||||
if (point[useNextAxis2] > expAABB.min[useNextAxis2])
|
||||
{
|
||||
if (RaySphereIntersection(zeus::CSphere(aabbPoint2, sphere.radius),
|
||||
sphere.position, dir, dOut, d, point))
|
||||
{
|
||||
dOut = d;
|
||||
normal = (point - aabbPoint2).normalized();
|
||||
point -= sphere.radius * normal;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
else
|
||||
{
|
||||
dOut = d;
|
||||
normal = point - aabbPoint2;
|
||||
normal.normalize();
|
||||
point -= sphere.radius * normal;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool AABox_AABox_Moving(const zeus::CAABox& aabb0, const zeus::CAABox& aabb1, const zeus::CVector3f& dir,
|
||||
double& d, zeus::CVector3f& point, zeus::CVector3f& normal)
|
||||
{
|
||||
zeus::CVector3d vecMin(DBL_MIN);
|
||||
zeus::CVector3d vecMax(DBL_MAX);
|
||||
|
||||
for (int i=0 ; i<3 ; ++i)
|
||||
{
|
||||
if (std::fabs(dir[i]) < FLT_EPSILON)
|
||||
{
|
||||
if (aabb0.min[i] >= aabb1.min[i] && aabb0.min[i] <= aabb1.max[i])
|
||||
continue;
|
||||
if (aabb0.max[i] >= aabb1.min[i] && aabb0.max[i] <= aabb1.max[i])
|
||||
continue;
|
||||
if (aabb0.min[i] < aabb1.min[i] && aabb0.max[i] > aabb1.max[i])
|
||||
continue;
|
||||
return false;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (aabb0.max[i] < aabb1.min[i] && dir[i] > 0.f)
|
||||
vecMin[i] = (aabb1.min[i] - aabb0.max[i]) / dir[i];
|
||||
else if (aabb1.max[i] < aabb0.min[i] && dir[i] < 0.f)
|
||||
vecMin[i] = (aabb1.max[i] - aabb0.min[i]) / dir[i];
|
||||
else if (aabb1.max[i] > aabb0.min[i] && dir[i] < 0.f)
|
||||
vecMin[i] = (aabb1.max[i] - aabb0.min[i]) / dir[i];
|
||||
else if (aabb0.max[i] > aabb1.min[i] && dir[i] > 0.f)
|
||||
vecMin[i] = (aabb1.min[i] - aabb0.max[i]) / dir[i];
|
||||
|
||||
if (aabb1.max[i] > aabb0.min[i] && dir[i] > 0.f)
|
||||
vecMax[i] = (aabb1.max[i] - aabb0.min[i]) / dir[i];
|
||||
else if (aabb0.max[i] > aabb1.min[i] && dir[i] < 0.f)
|
||||
vecMax[i] = (aabb1.min[i] - aabb0.max[i]) / dir[i];
|
||||
else if (aabb0.max[i] < aabb1.min[i] && dir[i] < 0.f)
|
||||
vecMax[i] = (aabb1.min[i] - aabb0.max[i]) / dir[i];
|
||||
else if (aabb1.max[i] < aabb0.min[i] && dir[i] > 0.f)
|
||||
vecMax[i] = (aabb1.max[i] - aabb0.min[i]) / dir[i];
|
||||
}
|
||||
}
|
||||
|
||||
int maxAxis = 0;
|
||||
if (vecMin[1] > vecMin[0])
|
||||
maxAxis = 1;
|
||||
if (vecMin[2] > vecMin[maxAxis])
|
||||
maxAxis = 2;
|
||||
|
||||
float minMax = std::min(std::min(vecMax[2], vecMax[1]), vecMax[0]);
|
||||
if (vecMin[maxAxis] > minMax)
|
||||
return false;
|
||||
d = minMax;
|
||||
|
||||
normal = zeus::CVector3f::skZero;
|
||||
normal[maxAxis] = dir[maxAxis] > 0.f ? -1.f : 1.f;
|
||||
|
||||
for (int i=0 ; i<3 ; ++i)
|
||||
point[i] = dir[i] > 0.f ? aabb0.max[i] : aabb0.min[i];
|
||||
|
||||
point += float(d) * dir;
|
||||
return true;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -36,6 +36,14 @@ bool TriSphereOverlap(const zeus::CSphere& sphere,
|
|||
bool TriSphereIntersection(const zeus::CSphere& sphere,
|
||||
const zeus::CVector3f& trivert0, const zeus::CVector3f& trivert1,
|
||||
const zeus::CVector3f& trivert2, zeus::CVector3f& point, zeus::CVector3f& normal);
|
||||
bool BoxLineTest(const zeus::CAABox& aabb, const zeus::CVector3f& point, const zeus::CVector3f& dir,
|
||||
float& tMin, float& tMax, int& axis, bool& sign);
|
||||
bool LineCircleIntersection2d(const zeus::CVector3f& point, const zeus::CVector3f& dir, const zeus::CSphere& sphere,
|
||||
int axis1, int axis2, float& d);
|
||||
bool MovingSphereAABox(const zeus::CSphere& sphere, const zeus::CAABox& aabb, const zeus::CVector3f& dir,
|
||||
double& d, zeus::CVector3f& point, zeus::CVector3f& normal);
|
||||
bool AABox_AABox_Moving(const zeus::CAABox& aabb0, const zeus::CAABox& aabb1, const zeus::CVector3f& dir,
|
||||
double& d, zeus::CVector3f& point, zeus::CVector3f& normal);
|
||||
}
|
||||
}
|
||||
#endif // __URDE_COLLISIONUTIL_HPP__
|
||||
|
|
|
|||
2
specter
2
specter
|
|
@ -1 +1 @@
|
|||
Subproject commit 79b00ed327c88c80bece7f7b58e1fc8d02ab52e1
|
||||
Subproject commit 1c17a28648e33d52a0eaea1e272da49ac6a940dd
|
||||
Loading…
Reference in New Issue