AxioDL
/
metaforce
mirror of https://github.com/AxioDL/metaforce.git
2
0
Fork 0
Code Issues Projects Releases Wiki Activity
metaforce/Runtime/Collision/CCollidableSphere.cpp

317 lines
11 KiB
C++
Raw Blame History

#include "CCollidableSphere.hpp"
#include "CCollisionInfoList.hpp"
#include "CCollidableAABox.hpp"
#include "CollisionUtil.hpp"
#include "CInternalRayCastStructure.hpp"
namespace urde
{
const CCollisionPrimitive::Type CCollidableSphere::sType(CCollidableSphere::SetStaticTableIndex, "CCollidableSphere");
u32 CCollidableSphere::sTableIndex = -1;
namespace Collide
{
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());
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;
}
}
}
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;
}
}
CCollidableSphere::CCollidableSphere(const zeus::CSphere& sphere, const CMaterialList& list)
: CCollisionPrimitive(list), x10_sphere(sphere)
{
}
zeus::CSphere CCollidableSphere::Transform(const zeus::CTransform& xf) const
{
return zeus::CSphere(xf * x10_sphere.position, x10_sphere.radius);
}
u32 CCollidableSphere::GetTableIndex() const { return sTableIndex; }
zeus::CAABox CCollidableSphere::CalculateAABox(const zeus::CTransform& xf) const
{
zeus::CVector3f xfPos = xf * x10_sphere.position;
return {xfPos - x10_sphere.radius, xfPos + x10_sphere.radius};
}
zeus::CAABox CCollidableSphere::CalculateLocalAABox() const
{
return {x10_sphere.position - x10_sphere.radius, x10_sphere.position + x10_sphere.radius};
}
FourCC CCollidableSphere::GetPrimType() const { return SBIG('SPHR'); }
CRayCastResult CCollidableSphere::CastRayInternal(const CInternalRayCastStructure& rayCast) const
{
if (!rayCast.GetFilter().Passes(GetMaterial()))
return {};
zeus::CSphere xfSphere = Transform(rayCast.GetTransform());
float t = 0.f;
zeus::CVector3f point;
if (CollisionUtil::RaySphereIntersection(xfSphere, rayCast.GetRay().start, rayCast.GetRay().dir,
rayCast.GetMaxTime(), t, point))
{
zeus::CVector3f delta = point - xfSphere.position;
float deltaMag = delta.magnitude();
zeus::CUnitVector3f planeNormal =
(deltaMag > 0.01f) ? delta * (1.f / deltaMag) : rayCast.GetRay().dir;
float planeD = point.dot(planeNormal);
return CRayCastResult(t, point, zeus::CPlane(planeNormal, planeD), GetMaterial());
}
return {};
}
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& 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;
}
bool CCollidableSphere::Sphere_AABox_Bool(const zeus::CSphere& sphere, const zeus::CAABox& aabb)
{
float mag = 0.f;
for (int i=0 ; i<3 ; ++i)
{
if (sphere.position[i] < aabb.min[i])
{
float tmp = sphere.position[i] - aabb.min[i];
mag += tmp * tmp;
}
else if (sphere.position[i] > aabb.max[i])
{
float tmp = sphere.position[i] - aabb.max[i];
mag += tmp * tmp;
}
}
return mag <= sphere.radius * sphere.radius;
}
}
Reference in New Issue View Git Blame Copy Permalink