Library/Clipping: Implement FrustumRadar (#1049)

Co-authored-by: Alex Larbe <alex.larbe@outlook.com>

data/file_list.yml

@@ -230859,91 +230859,91 @@ Library/Clipping/FrustumRadar.o:
     label:
     - _ZN2al12FrustumRadarC1Ev
     - _ZN2al12FrustumRadarC2Ev
-    status: NotDecompiled
+    status: Matching
   - offset: 0x843600
     size: 296
     label: _ZN2al12FrustumRadar15calcFrustumAreaERKN4sead8Matrix34IfEEffff
-    status: NotDecompiled
+    status: Matching
   - offset: 0x843728
     size: 156
     label: _ZN2al12FrustumRadar12setLocalAxisERKN4sead8Matrix34IfEE
-    status: NotDecompiled
+    status: Matching
   - offset: 0x8437c4
     size: 148
     label: _ZN2al12FrustumRadar9setFactorEff
-    status: NotDecompiled
+    status: Matching
   - offset: 0x843858
     size: 276
     label: _ZN2al12FrustumRadar15calcFrustumAreaERKN4sead8Matrix34IfEERKNS1_8Matrix44IfEEff
-    status: NotDecompiled
+    status: Matching
   - offset: 0x84396c
     size: 132
     label: _ZN2al12FrustumRadar9setFactorERKN4sead8Matrix44IfEE
-    status: NotDecompiled
+    status: Matching
   - offset: 0x8439f0
     size: 472
     label: _ZN2al12FrustumRadar21calcFrustumAreaStereoERKN4sead8Matrix34IfEES5_RKNS1_8Matrix44IfEEff
-    status: NotDecompiled
+    status: Matching
   - offset: 0x843bc8
     size: 244
     label: _ZN2al12FrustumRadar18setLocalAxisStereoERKN4sead8Matrix34IfEES5_
-    status: NotDecompiled
+    status: Matching
   - offset: 0x843cbc
     size: 228
     label: _ZN2al12FrustumRadar15setFactorStereoERKN4sead8Matrix44IfEE
-    status: NotDecompiled
+    status: Matching
   - offset: 0x843da0
     size: 112
     label: _ZNK2al12FrustumRadar11judgeInLeftERKN4sead7Vector3IfEEf
-    status: NotDecompiled
+    status: Matching
   - offset: 0x843e10
     size: 108
     label: _ZNK2al12FrustumRadar12judgeInRightERKN4sead7Vector3IfEEf
-    status: NotDecompiled
+    status: Matching
   - offset: 0x843e7c
     size: 108
     label: _ZNK2al12FrustumRadar10judgeInTopERKN4sead7Vector3IfEEf
-    status: NotDecompiled
+    status: Matching
   - offset: 0x843ee8
     size: 112
     label: _ZNK2al12FrustumRadar13judgeInBottomERKN4sead7Vector3IfEEf
-    status: NotDecompiled
+    status: Matching
   - offset: 0x843f58
     size: 376
     label: _ZNK2al12FrustumRadar11judgeInAreaERKN4sead7Vector3IfEEfff
-    status: NotDecompiled
+    status: Matching
   - offset: 0x8440d0
     size: 8
     label: _ZNK2al12FrustumRadar11judgeInAreaERKN4sead7Vector3IfEEff
-    status: NotDecompiled
+    status: Matching
   - offset: 0x8440d8
     size: 8
     label: _ZNK2al12FrustumRadar11judgeInAreaERKN4sead7Vector3IfEEf
-    status: NotDecompiled
+    status: Matching
   - offset: 0x8440e0
     size: 12
     label: _ZNK2al12FrustumRadar16judgeInAreaNoFarERKN4sead7Vector3IfEEf
-    status: NotDecompiled
+    status: Matching
   - offset: 0x8440ec
     size: 324
     label: _ZNK2al12FrustumRadar14judgePointFlagERKN4sead7Vector3IfEEff
-    status: NotDecompiled
+    status: Matching
   - offset: 0x844230
     size: 396
     label: _ZNK2al12FrustumRadar14judgeInAreaObbEPKN4sead8Matrix34IfEERKNS1_9BoundBox3IfEEff
-    status: NotDecompiled
+    status: Matching
   - offset: 0x8443bc
     size: 8
     label: _ZNK2al12FrustumRadar14judgeInAreaObbEPKN4sead8Matrix34IfEERKNS1_9BoundBox3IfEEf
-    status: NotDecompiled
+    status: Matching
   - offset: 0x8443c4
     size: 8
     label: _ZNK2al12FrustumRadar14judgeInAreaObbEPKN4sead8Matrix34IfEERKNS1_9BoundBox3IfEE
-    status: NotDecompiled
+    status: Matching
   - offset: 0x8443cc
     size: 12
     label: _ZNK2al12FrustumRadar19judgeInAreaObbNoFarEPKN4sead8Matrix34IfEERKNS1_9BoundBox3IfEE
-    status: NotDecompiled
+    status: Matching
 Library/Collision/Collider.o:
   '.text':
   - offset: 0x8443d8

lib/al/Library/Clipping/FrustumRadar.cpp

@@ -0,0 +1,245 @@
+#include "Library/Clipping/FrustumRadar.h"
+
+#include <math/seadMathCalcCommon.h>
+
+#include "Library/Math/MathUtil.h"
+#include "Library/Matrix/MatrixUtil.h"
+
+namespace al {
+
+FrustumRadar::FrustumRadar() = default;
+
+void FrustumRadar::calcFrustumArea(const sead::Matrix34f& orthoMtx, f32 fovyAngle, f32 aspectRatio,
+                                   f32 near, f32 far) {
+    setLocalAxis(orthoMtx);
+
+    setFactor(fovyAngle, aspectRatio);
+
+    mNear = near;
+    mFar = far;
+}
+
+void FrustumRadar::setLocalAxis(const sead::Matrix34f& orthoMtx) {
+    sead::Matrix34f mtxInvertOrtho;
+    calcMxtInvertOrtho(&mtxInvertOrtho, orthoMtx);
+
+    mtxInvertOrtho.getBase(mOrthoSide, 0);
+    mtxInvertOrtho.getBase(mOrthoUp, 1);
+    mtxInvertOrtho.getBase(mOrthoFront, 2);
+    mOrthoFront.negate();
+    mtxInvertOrtho.getTranslation(mOrthoTrans);
+
+    mStereoEyeOffset = 0.0f;
+}
+
+void FrustumRadar::setFactor(f32 fovyAngle, f32 aspectRatio) {
+    mVerticalSlope = sead::Mathf::tan(sead::Mathf::deg2rad(fovyAngle * 0.5f));
+    mVerticalNormFactor = sead::Mathf::sqrt(mVerticalSlope * mVerticalSlope + 1.0f);
+
+    mHorizontalSlope = mVerticalSlope * aspectRatio;
+    mHorizontalNormFactor = sead::Mathf::sqrt(mHorizontalSlope * mHorizontalSlope + 1.0f);
+}
+
+void FrustumRadar::calcFrustumArea(const sead::Matrix34f& orthoMtx,
+                                   const sead::Matrix44f& projectionMtx, f32 near, f32 far) {
+    setLocalAxis(orthoMtx);
+    setFactor(projectionMtx);
+    mNear = near;
+    mFar = far;
+}
+
+void FrustumRadar::setFactor(const sead::Matrix44f& projectionMtx) {
+    mVerticalSlope = 1.0f / projectionMtx(1, 1);
+    mVerticalNormFactor = sead::Mathf::sqrt(mVerticalSlope * mVerticalSlope + 1.0f);
+
+    mHorizontalSlope = 1.0f / projectionMtx(0, 0);
+    mHorizontalNormFactor = sead::Mathf::sqrt(mHorizontalSlope * mHorizontalSlope + 1.0f);
+}
+
+void FrustumRadar::calcFrustumAreaStereo(const sead::Matrix34f& orthoMtxLeft,
+                                         const sead::Matrix34f& orthoMtxRight,
+                                         const sead::Matrix44f& projectionMtx, f32 near, f32 far) {
+    setLocalAxisStereo(orthoMtxLeft, orthoMtxRight);
+    setFactorStereo(projectionMtx);
+    mNear = near;
+    mFar = far;
+}
+
+void FrustumRadar::setLocalAxisStereo(const sead::Matrix34f& orthoMtxLeft,
+                                      const sead::Matrix34f& orthoMtxRight) {
+    sead::Matrix34f mtxInvertOrthoLeft;
+    sead::Matrix34f mtxInvertRight;
+    calcMxtInvertOrtho(&mtxInvertOrthoLeft, orthoMtxLeft);
+
+    mtxInvertOrthoLeft.getBase(mOrthoSide, 0);
+    mtxInvertOrthoLeft.getBase(mOrthoUp, 1);
+    mtxInvertOrthoLeft.getBase(mOrthoFront, 2);
+    mOrthoFront.negate();
+
+    calcMxtInvertOrtho(&mtxInvertRight, orthoMtxRight);
+    mOrthoTrans = (mtxInvertOrthoLeft.getTranslation() + mtxInvertRight.getTranslation()) * 0.5f;
+
+    mStereoEyeOffset = mOrthoSide.dot(mOrthoTrans - mtxInvertOrthoLeft.getTranslation());
+}
+
+void FrustumRadar::setFactorStereo(const sead::Matrix44f& projectionMtx) {
+    setFactor(projectionMtx);
+    f32 centerOffset = projectionMtx(0, 2);
+
+    mStereoSlopeLeft = mHorizontalSlope * (1.0f - centerOffset);
+    mStereoNormFactorLeft = sead::Mathf::sqrt(mStereoSlopeLeft * mStereoSlopeLeft + 1.0f);
+
+    mStereoSlopeRight = mHorizontalSlope * (1.0f + centerOffset);
+    mStereoNormFactorRight = sead::Mathf::sqrt(mStereoSlopeRight * mStereoSlopeRight + 1.0f);
+}
+
+bool FrustumRadar::judgeInLeft(const sead::Vector3f& pos, f32 radius) const {
+    f32 dotFront = mOrthoFront.dot(pos - mOrthoTrans);
+    f32 dotSide = mOrthoSide.dot(pos - mOrthoTrans);
+
+    return !(dotSide < -(dotFront * mHorizontalSlope + mHorizontalNormFactor * radius));
+}
+
+bool FrustumRadar::judgeInRight(const sead::Vector3f& pos, f32 radius) const {
+    f32 dotFront = mOrthoFront.dot(pos - mOrthoTrans);
+    f32 dotSide = mOrthoSide.dot(pos - mOrthoTrans);
+
+    return !(dotFront * mHorizontalSlope + mHorizontalNormFactor * radius < dotSide);
+}
+
+bool FrustumRadar::judgeInTop(const sead::Vector3f& pos, f32 radius) const {
+    f32 dotFront = mOrthoFront.dot(pos - mOrthoTrans);
+    f32 dotUp = mOrthoUp.dot(pos - mOrthoTrans);
+
+    return !(dotFront * mVerticalSlope + mVerticalNormFactor * radius < dotUp);
+}
+
+bool FrustumRadar::judgeInBottom(const sead::Vector3f& pos, f32 radius) const {
+    f32 dotFront = mOrthoFront.dot(pos - mOrthoTrans);
+    f32 dotUp = mOrthoUp.dot(pos - mOrthoTrans);
+
+    return !(dotUp < -(dotFront * mVerticalSlope + mVerticalNormFactor * radius));
+}
+
+bool FrustumRadar::judgeInArea(const sead::Vector3f& pos, f32 radius, f32 near, f32 far) const {
+    f32 dotFront = mOrthoFront.dot(pos - mOrthoTrans);
+
+    if (dotFront < near - radius)
+        return false;
+
+    if (far > 0.0f && radius + far < dotFront)
+        return false;
+
+    f32 dotUpAbs = sead::Mathf::abs(mOrthoUp.dot(pos - mOrthoTrans));
+    if (dotFront * mVerticalSlope + mVerticalNormFactor * radius < dotUpAbs)
+        return false;
+
+    f32 dotSide = mOrthoSide.dot(pos - mOrthoTrans);
+    if (isNearZero(mStereoEyeOffset)) {
+        if (dotFront * mHorizontalSlope + mHorizontalNormFactor * radius <
+            sead::Mathf::abs(dotSide))
+            return false;
+    } else {
+        f32 limitRight = dotFront * mStereoSlopeRight + mStereoNormFactorRight * radius;
+        f32 limitLeft = dotFront * mStereoSlopeLeft + mStereoNormFactorLeft * radius;
+
+        f32 relSideLeft = dotSide - mStereoEyeOffset;
+        f32 relSideRight = dotSide + mStereoEyeOffset;
+
+        if (relSideLeft > limitLeft && relSideRight > limitRight)
+            return false;
+
+        if (relSideLeft < -limitRight && relSideRight < -limitLeft)
+            return false;
+    }
+    return true;
+}
+
+bool FrustumRadar::judgeInArea(const sead::Vector3f& pos, f32 radius, f32 near) const {
+    return judgeInArea(pos, radius, near, mFar);
+}
+
+bool FrustumRadar::judgeInArea(const sead::Vector3f& pos, f32 radius) const {
+    return judgeInArea(pos, radius, mNear, mFar);
+}
+
+bool FrustumRadar::judgeInAreaNoFar(const sead::Vector3f& pos, f32 radius) const {
+    return judgeInArea(pos, radius, mNear, -1.0f);
+}
+
+FrustumRadar::PointFlag FrustumRadar::judgePointFlag(const sead::Vector3f& pos, f32 near,
+                                                     f32 far) const {
+    u32 flag = PointFlag::None;
+
+    sead::Vector3f relPos = pos - mOrthoTrans;
+    f32 dotFront = mOrthoFront.dot(relPos);
+
+    if (dotFront < near)
+        flag |= PointFlag::Near;
+
+    if (far > 0.0f && dotFront > far)
+        flag |= PointFlag::Far;
+
+    f32 dotUp = mOrthoUp.dot(relPos);
+    if (dotUp < -(dotFront * mVerticalSlope))
+        flag |= PointFlag::Bottom;
+
+    if (dotFront * mVerticalSlope < dotUp)
+        flag |= PointFlag::Top;
+
+    f32 dotSide = mOrthoSide.dot(relPos);
+    if (isNearZero(mStereoEyeOffset)) {
+        if (dotSide < -(dotFront * mHorizontalSlope))
+            flag |= PointFlag::Left;
+        if (dotFront * mHorizontalSlope < dotSide)
+            flag |= PointFlag::Right;
+        return (PointFlag)flag;
+    }
+
+    f32 limitRight = dotFront * mStereoSlopeRight;
+    f32 limitLeft = dotFront * mStereoSlopeLeft;
+    f32 distSideLeftEye = dotSide - mStereoEyeOffset;
+    f32 distSideRightEye = dotSide + mStereoEyeOffset;
+
+    if (distSideLeftEye > limitLeft && distSideRightEye > limitRight)
+        flag |= PointFlag::Right;
+
+    if (distSideLeftEye < -limitRight && distSideRightEye < -limitLeft)
+        flag |= PointFlag::Left;
+
+    return (PointFlag)flag;
+}
+
+bool FrustumRadar::judgeInAreaObb(const sead::Matrix34f* mtx, const sead::BoundBox3f& boundBox,
+                                  f32 near, f32 far) const {
+    sead::Vector3f corners[8];
+    calcObbCorners(corners, *mtx, boundBox);
+
+    s32 mask = ~PointFlag::None;
+    for (s32 i = 0; i < 8; ++i) {
+        s32 pointFlags = judgePointFlag(corners[i], near, far);
+        if (pointFlags == PointFlag::None)
+            return true;
+
+        mask &= pointFlags;
+    }
+
+    return mask == PointFlag::None;
+}
+
+bool FrustumRadar::judgeInAreaObb(const sead::Matrix34f* mtx, const sead::BoundBox3f& boundBox,
+                                  f32 near) const {
+    return judgeInAreaObb(mtx, boundBox, near, mFar);
+}
+
+bool FrustumRadar::judgeInAreaObb(const sead::Matrix34f* mtx,
+                                  const sead::BoundBox3f& boundBox) const {
+    return judgeInAreaObb(mtx, boundBox, mNear, mFar);
+}
+
+bool FrustumRadar::judgeInAreaObbNoFar(const sead::Matrix34f* mtx,
+                                       const sead::BoundBox3f& boundBox) const {
+    return judgeInAreaObb(mtx, boundBox, mNear, -1.0f);
+}
+
+}  // namespace al

lib/al/Library/Clipping/FrustumRadar.h

@@ -3,34 +3,68 @@
 #include <math/seadBoundBox.h>
 #include <math/seadMatrix.h>
 
+#include "Library/Matrix/MatrixUtil.h"
+
 namespace al {
 
 class FrustumRadar {
 public:
+    enum PointFlag : u32 {
+        None = 0,
+        Near = 1,
+        Far = 2,
+        Bottom = 4,
+        Top = 8,
+        Left = 16,
+        Right = 32,
+    };
+
     FrustumRadar();
-    void calcFrustumArea(const sead::Matrix34f&, f32, f32, f32, f32);
-    void setLocalAxis(const sead::Matrix34f&);
-    void setFactor(f32, f32);
-    void calcFrustumArea(const sead::Matrix34f&, const sead::Matrix44f&, f32, f32);
-    void setFactor(sead::Matrix44f);
-    void calcFrustumAreaStereo(const sead::Matrix34f&, const sead::Matrix34f&,
-                               const sead::Matrix44f&, f32, f32);
-    void setLocalAxisStereo(const sead::Matrix34f&, const sead::Matrix34f&);
-    void setFactorStereo(const sead::Matrix44f&);
-    bool judgeInLeft(const sead::Vector3f&, f32) const;
-    bool judgeInRight(const sead::Vector3f&, f32) const;
-    bool judgeInTop(const sead::Vector3f&, f32) const;
-    bool judgeInBottom(const sead::Vector3f&, f32) const;
-    bool judgeInArea(const sead::Vector3f&, f32, f32, f32) const;
-    bool judgeInArea(const sead::Vector3f&, f32, f32) const;
-    bool judgeInAreaNoFar(const sead::Vector3f&, f32) const;
-    bool judgePointFlag(const sead::Vector3f&, f32, f32) const;
-    bool judgeInAreaObb(const sead::Matrix34f*, const sead::BoundBox3f&, f32, f32) const;
-    bool judgeInAreaObb(const sead::Matrix34f*, const sead::BoundBox3f&, f32) const;
-    bool judgeInAreaObbNoFar(const sead::Matrix34f*, const sead::BoundBox3f&) const;
+    void calcFrustumArea(const sead::Matrix34f& orthoMtx, f32 fovyAngle, f32 aspectRatio, f32 near,
+                         f32 far);
+    void setLocalAxis(const sead::Matrix34f& orthoMtx);
+    void setFactor(f32 fovyAngle, f32 aspectRatio);
+    void calcFrustumArea(const sead::Matrix34f& orthoMtx, const sead::Matrix44f& projectionMtx,
+                         f32 near, f32 far);
+    void setFactor(const sead::Matrix44f& projectionMtx);
+    void calcFrustumAreaStereo(const sead::Matrix34f& orthoMtxLeft,
+                               const sead::Matrix34f& orthoMtxRight,
+                               const sead::Matrix44f& projectionMtx, f32 near, f32 far);
+    void setLocalAxisStereo(const sead::Matrix34f& orthoMtxLeft,
+                            const sead::Matrix34f& orthoMtxRight);
+    void setFactorStereo(const sead::Matrix44f& projectionMtx);
+    bool judgeInLeft(const sead::Vector3f& pos, f32 radius) const;
+    bool judgeInRight(const sead::Vector3f& pos, f32 radius) const;
+    bool judgeInTop(const sead::Vector3f& pos, f32 radius) const;
+    bool judgeInBottom(const sead::Vector3f& pos, f32 radius) const;
+    bool judgeInArea(const sead::Vector3f& pos, f32 radius, f32 near, f32 far) const;
+    bool judgeInArea(const sead::Vector3f& pos, f32 radius, f32 near) const;
+    bool judgeInArea(const sead::Vector3f& pos, f32 radius) const;
+    bool judgeInAreaNoFar(const sead::Vector3f& pos, f32 radius) const;
+    PointFlag judgePointFlag(const sead::Vector3f& pos, f32 near, f32 far) const;
+    bool judgeInAreaObb(const sead::Matrix34f* mtx, const sead::BoundBox3f& boundBox, f32 near,
+                        f32 far) const;
+    bool judgeInAreaObb(const sead::Matrix34f* mtx, const sead::BoundBox3f& boundBox,
+                        f32 near) const;
+    bool judgeInAreaObb(const sead::Matrix34f* mtx, const sead::BoundBox3f& boundBox) const;
+    bool judgeInAreaObbNoFar(const sead::Matrix34f* mtx, const sead::BoundBox3f& boundBox) const;
 
 private:
-    unsigned char filler[0x5c];
+    sead::Vector3f mOrthoSide = sead::Vector3f::ex;
+    sead::Vector3f mOrthoUp = sead::Vector3f::ey;
+    sead::Vector3f mOrthoFront = sead::Vector3f::ez;
+    sead::Vector3f mOrthoTrans = sead::Vector3f::zero;
+    f32 mHorizontalSlope = 0.3f;
+    f32 mHorizontalNormFactor = 1.04403f;
+    f32 mVerticalSlope = 0.2f;
+    f32 mVerticalNormFactor = 1.0098f;
+    f32 mStereoEyeOffset = 0.0f;
+    f32 mStereoSlopeLeft = 0.3f;
+    f32 mStereoNormFactorLeft = 1.04403f;
+    f32 mStereoSlopeRight = 0.3f;
+    f32 mStereoNormFactorRight = 1.04403f;
+    f32 mNear = 100.0f;
+    f32 mFar = 10000.0f;
 };
 
 }  // namespace al