Fix frustum culling

CMakeLists.txt

@@ -17,6 +17,7 @@ set(SOURCES
     src/CMatrix3f.cpp
     src/CProjection.cpp
     src/CPlane.cpp
+    src/CFrustum.cpp
     src/CTransform.cpp
     src/CColor.cpp
     src/CVector2f.cpp

include/zeus/CFrustum.hpp

@@ -13,132 +13,12 @@ class CFrustum
     bool valid = false;
 
 public:
-    inline void updatePlanes(const CMatrix4f& modelview, const CMatrix4f& projection)
+    void updatePlanes(const CMatrix4f& viewMtx, const CMatrix4f& projection);
-    {
+    void updatePlanes(const CTransform& viewPointMtx, const CProjection& projection);
-        CMatrix4f mvp = projection * modelview;
 
-#if __SSE__
+    bool aabbFrustumTest(const CAABox& aabb) const;
-
+    bool sphereFrustumTest(const CSphere& sphere) const;
-        /* Left */
+    bool pointFrustumTest(const CVector3f& point) const;
-        planes[0].mVec128 = _mm_add_ps(mvp.vec[0].mVec128, mvp.vec[3].mVec128);
-
-        /* Right */
-        planes[1].mVec128 = _mm_add_ps(_mm_sub_ps(CVector3f::skZero.mVec128, mvp.vec[0].mVec128), mvp.vec[3].mVec128);
-
-        /* Bottom */
-        planes[2].mVec128 = _mm_add_ps(mvp.vec[1].mVec128, mvp.vec[3].mVec128);
-
-        /* Top */
-        planes[3].mVec128 = _mm_add_ps(_mm_sub_ps(CVector3f::skZero.mVec128, mvp.vec[1].mVec128), mvp.vec[3].mVec128);
-
-        /* Near */
-        planes[4].mVec128 = _mm_add_ps(mvp.vec[2].mVec128, mvp.vec[3].mVec128);
-
-        /* Far */
-        planes[5].mVec128 = _mm_add_ps(_mm_sub_ps(CVector3f::skZero.mVec128, mvp.vec[2].mVec128), mvp.vec[3].mVec128);
-
-#else
-        /* Left */
-        planes[0].a = mvp.m[0][0] + mvp.m[3][0];
-        planes[0].b = mvp.m[0][1] + mvp.m[3][1];
-        planes[0].c = mvp.m[0][2] + mvp.m[3][2];
-        planes[0].d = mvp.m[0][3] + mvp.m[3][3];
-
-        /* Right */
-        planes[1].a = -mvp.m[0][0] + mvp.m[3][0];
-        planes[1].b = -mvp.m[0][1] + mvp.m[3][1];
-        planes[1].c = -mvp.m[0][2] + mvp.m[3][2];
-        planes[1].d = -mvp.m[0][3] + mvp.m[3][3];
-
-        /* Bottom */
-        planes[2].a = mvp.m[1][0] + mvp.m[3][0];
-        planes[2].b = mvp.m[1][1] + mvp.m[3][1];
-        planes[2].c = mvp.m[1][2] + mvp.m[3][2];
-        planes[2].d = mvp.m[1][3] + mvp.m[3][3];
-
-        /* Top */
-        planes[3].a = -mvp.m[1][0] + mvp.m[3][0];
-        planes[3].b = -mvp.m[1][1] + mvp.m[3][1];
-        planes[3].c = -mvp.m[1][2] + mvp.m[3][2];
-        planes[3].d = -mvp.m[1][3] + mvp.m[3][3];
-
-        /* Near */
-        planes[4].a = mvp.m[2][0] + mvp.m[3][0];
-        planes[4].b = mvp.m[2][1] + mvp.m[3][1];
-        planes[4].c = mvp.m[2][2] + mvp.m[3][2];
-        planes[4].d = mvp.m[2][3] + mvp.m[3][3];
-
-        /* Far */
-        planes[5].a = -mvp.m[2][0] + mvp.m[3][0];
-        planes[5].b = -mvp.m[2][1] + mvp.m[3][1];
-        planes[5].c = -mvp.m[2][2] + mvp.m[3][2];
-        planes[5].d = -mvp.m[2][3] + mvp.m[3][3];
-
-#endif
-
-        planes[0].normalize();
-        planes[1].normalize();
-        planes[2].normalize();
-        planes[3].normalize();
-        planes[4].normalize();
-        planes[5].normalize();
-
-        valid = true;
-    }
-
-    inline void updatePlanes(const CTransform& modelview, const CProjection& projection)
-    {
-        updatePlanes(modelview.toMatrix4f(), projection.getCachedMatrix());
-    }
-
-    inline bool aabbFrustumTest(const CAABox& aabb) const
-    {
-        if (!valid)
-            return true;
-
-        CVector3f center = aabb.center();
-        CVector3f extents = aabb.extents();
-
-        for (uint32_t i = 0; i < 6; ++i)
-        {
-            const CPlane& plane = planes[i];
-
-            float m = plane.vec.dot(center) + plane.d;
-            float n = extents.dot({std::fabs(plane.a), std::fabs(plane.b), std::fabs(plane.c)});
-
-            if (m + n < 0)
-                return false;
-        }
-        return true;
-    }
-
-    inline bool sphereFrustumTest(const CSphere& sphere)
-    {
-        if (!valid)
-            return true;
-
-        for (uint32_t i = 0 ; i<6 ; ++i)
-        {
-            float dadot = planes[i].vec.dot(sphere.position);
-            if ((dadot + planes[i].d + sphere.radius) < 0)
-                return false;
-        }
-        return true;
-    }
-
-    inline bool pointFrustumTest(const CVector3f& point)
-    {
-        if (!valid)
-            return true;
-
-        for (uint32_t i = 0 ; i<6 ; ++i)
-        {
-            float dadot = planes[i].vec.dot(point);
-            if ((dadot + planes[i].d) < 0)
-                return false;
-        }
-        return true;
-    }
 };
 }
 #endif // CFRUSTUM_HPP

src/CFrustum.cpp

@@ -0,0 +1,138 @@
+#include "zeus/CFrustum.hpp"
+
+namespace zeus
+{
+
+void CFrustum::updatePlanes(const CMatrix4f& viewMtx, const CMatrix4f& projection)
+{
+    CMatrix4f mvp = projection * viewMtx;
+    CMatrix4f mvp_rm = mvp.transposed();
+
+#if __SSE__
+
+    /* Left */
+    planes[0].mVec128 = _mm_add_ps(mvp_rm.vec[3].mVec128, mvp_rm.vec[0].mVec128);
+
+    /* Right */
+    planes[1].mVec128 = _mm_sub_ps(mvp_rm.vec[3].mVec128, mvp_rm.vec[0].mVec128);
+
+    /* Bottom */
+    planes[2].mVec128 = _mm_add_ps(mvp_rm.vec[3].mVec128, mvp_rm.vec[1].mVec128);
+
+    /* Top */
+    planes[3].mVec128 = _mm_sub_ps(mvp_rm.vec[3].mVec128, mvp_rm.vec[1].mVec128);
+
+    /* Near */
+    planes[4].mVec128 = _mm_add_ps(mvp_rm.vec[3].mVec128, mvp_rm.vec[2].mVec128);
+
+    /* Far */
+    planes[5].mVec128 = _mm_sub_ps(mvp_rm.vec[3].mVec128, mvp_rm.vec[2].mVec128);
+
+#else
+    /* Left */
+        planes[0].a = mvp.m[0][0] + mvp.m[3][0];
+        planes[0].b = mvp.m[0][1] + mvp.m[3][1];
+        planes[0].c = mvp.m[0][2] + mvp.m[3][2];
+        planes[0].d = mvp.m[0][3] + mvp.m[3][3];
+
+        /* Right */
+        planes[1].a = -mvp.m[0][0] + mvp.m[3][0];
+        planes[1].b = -mvp.m[0][1] + mvp.m[3][1];
+        planes[1].c = -mvp.m[0][2] + mvp.m[3][2];
+        planes[1].d = -mvp.m[0][3] + mvp.m[3][3];
+
+        /* Bottom */
+        planes[2].a = mvp.m[1][0] + mvp.m[3][0];
+        planes[2].b = mvp.m[1][1] + mvp.m[3][1];
+        planes[2].c = mvp.m[1][2] + mvp.m[3][2];
+        planes[2].d = mvp.m[1][3] + mvp.m[3][3];
+
+        /* Top */
+        planes[3].a = -mvp.m[1][0] + mvp.m[3][0];
+        planes[3].b = -mvp.m[1][1] + mvp.m[3][1];
+        planes[3].c = -mvp.m[1][2] + mvp.m[3][2];
+        planes[3].d = -mvp.m[1][3] + mvp.m[3][3];
+
+        /* Near */
+        planes[4].a = mvp.m[2][0] + mvp.m[3][0];
+        planes[4].b = mvp.m[2][1] + mvp.m[3][1];
+        planes[4].c = mvp.m[2][2] + mvp.m[3][2];
+        planes[4].d = mvp.m[2][3] + mvp.m[3][3];
+
+        /* Far */
+        planes[5].a = -mvp.m[2][0] + mvp.m[3][0];
+        planes[5].b = -mvp.m[2][1] + mvp.m[3][1];
+        planes[5].c = -mvp.m[2][2] + mvp.m[3][2];
+        planes[5].d = -mvp.m[2][3] + mvp.m[3][3];
+
+#endif
+
+    planes[0].normalize();
+    planes[1].normalize();
+    planes[2].normalize();
+    planes[3].normalize();
+    planes[4].normalize();
+    planes[5].normalize();
+
+    valid = true;
+}
+
+void CFrustum::updatePlanes(const CTransform& viewPointMtx, const CProjection& projection)
+{
+    zeus::CMatrix3f tmp(viewPointMtx.basis[0], viewPointMtx.basis[2], -viewPointMtx.basis[1]);
+    zeus::CTransform viewBasis = zeus::CTransform(tmp.transposed());
+    zeus::CTransform viewMtx = viewBasis * zeus::CTransform::Translate(-viewPointMtx.origin);
+
+    updatePlanes(viewMtx.toMatrix4f(), projection.getCachedMatrix());
+}
+
+bool CFrustum::aabbFrustumTest(const CAABox& aabb) const
+{
+    if (!valid)
+        return true;
+
+    CVector3f center = aabb.center();
+    CVector3f extents = aabb.extents();
+
+    for (uint32_t i = 0; i < 6; ++i)
+    {
+        const CPlane& plane = planes[i];
+
+        float m = plane.vec.dot(center) + plane.d;
+        float n = extents.dot({std::fabs(plane.a), std::fabs(plane.b), std::fabs(plane.c)});
+
+        if (m + n < 0)
+            return false;
+    }
+    return true;
+}
+
+bool CFrustum::sphereFrustumTest(const CSphere& sphere) const
+{
+    if (!valid)
+        return true;
+
+    for (uint32_t i = 0 ; i<6 ; ++i)
+    {
+        float dadot = planes[i].vec.dot(sphere.position);
+        if ((dadot + planes[i].d + sphere.radius) < 0)
+            return false;
+    }
+    return true;
+}
+
+bool CFrustum::pointFrustumTest(const CVector3f& point) const
+{
+    if (!valid)
+        return true;
+
+    for (uint32_t i = 0 ; i<6 ; ++i)
+    {
+        float dadot = planes[i].vec.dot(point);
+        if ((dadot + planes[i].d) < 0)
+            return false;
+    }
+    return true;
+}
+
+}

src/CProjection.cpp

@@ -26,7 +26,7 @@ void CProjection::_updateCachedMatrix()
         tmp = 1.0f / (m_ortho.zfar - m_ortho.znear);
         m_mtx.m[0][2] = 0.0f;
         m_mtx.m[1][2] = 0.0f;
-        m_mtx.m[2][2] = -(1.0f) * tmp;
+        m_mtx.m[2][2] = -tmp;
         m_mtx.m[3][2] = -m_ortho.zfar * tmp;
 
         m_mtx.m[0][3] = 0.0f;
@@ -36,26 +36,33 @@ void CProjection::_updateCachedMatrix()
     }
     else if (m_projType == EProjType::Perspective)
     {
-        float cot, tmp;
+        float tfov = std::tan(m_persp.fov * 0.5f);
-        float t_fovy = std::tan(m_persp.fov / 2.0f);
+        float top = m_persp.znear * tfov;
+        float bottom = -top;
+        float right = m_persp.aspect * m_persp.znear * tfov;
+        float left = -right;
 
-        cot = 1.0f / t_fovy;
+        float rml = right - left;
+        float rpl = right + left;
+        float tmb = top - bottom;
+        float tpb = top + bottom;
+        float fpn = m_persp.zfar + m_persp.znear;
+        float fmn = m_persp.zfar - m_persp.znear;
 
-        m_mtx.m[0][0] = cot / m_persp.aspect;
+        m_mtx.m[0][0] = 2.f * m_persp.znear / rml;
         m_mtx.m[1][0] = 0.0f;
-        m_mtx.m[2][0] = 0.0f;
+        m_mtx.m[2][0] = rpl / rml;
         m_mtx.m[3][0] = 0.0f;
 
         m_mtx.m[0][1] = 0.0f;
-        m_mtx.m[1][1] = cot;
+        m_mtx.m[1][1] = 2.f * m_persp.znear / tmb;
-        m_mtx.m[2][1] = 0.0f;
+        m_mtx.m[2][1] = tpb / tmb;
         m_mtx.m[3][1] = 0.0f;
 
-        tmp = 1.0f / (m_persp.zfar - m_persp.znear);
         m_mtx.m[0][2] = 0.0f;
         m_mtx.m[1][2] = 0.0f;
-        m_mtx.m[2][2] = -m_persp.zfar * tmp;
+        m_mtx.m[2][2] = -fpn / fmn;
-        m_mtx.m[3][2] = -(m_persp.zfar * m_persp.znear) * tmp;
+        m_mtx.m[3][2] = -2.f * m_persp.zfar * m_persp.znear / fmn;
 
         m_mtx.m[0][3] = 0.0f;
         m_mtx.m[1][3] = 0.0f;