游戏编程精粹学习 - 一种快速的圆柱棱台相交测试算法

Posted 2020-10-29 HONT blog

 

在Unity中挂载Renderer的对象可以使用OnBecameVisible/OnBecameInvisible来接收剔除事件。

但是非Renderer对象则要自己处理相交检测。

 

文中的方法测试结果比Unity的GeometryUtility效率要高一倍左右，且没有GC。不过只支持圆柱

 

下面是直接从书上C++版本转换的C#实现

using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class Frustum
{
    // Near and far plane distances
    float mNearDistance;
    float mFarDistance;

    // Precalculated normal components
    float mLeftRightX;
    float mLeftRightZ;
    float mTopBottomY;
    float mTopBottomZ;


    public Frustum(float focusLength, float aspect, float nearDistance, float farDistance)
    {
        // Save off near plane and far plane distances
        mNearDistance = nearDistance;
        mFarDistance = farDistance;

        // Precalculate side plane normal components
        float d = 1.0f / Mathf.Sqrt(1 * 1 + 1.0F);
        mLeftRightX = focusLength * d;
        mLeftRightZ = d;

        d = 1.0F / Mathf.Sqrt(focusLength * focusLength + aspect * aspect);
        mTopBottomY = focusLength * d;
        mTopBottomZ = aspect * d;
    }

    public bool CylinderVisible(Vector3 p1, Vector3 p2, float radius)
    {
        // Calculate unit vector representing cylinder`s axis
        var dp = p2 - p1;
        dp = dp.normalized;

        // Visit near plane first, N = (0,0,-1)
        var dot1 = -p1.z;
        var dot2 = -p2.z;

        // Calculate effective radius for near and far planes
        var effectiveRadius = radius * Mathf.Sqrt(1.0F - dp.z * dp.z);

        // Test endpoints against adjusted near plane
        var d = mNearDistance - effectiveRadius;
        var interior1 = (dot1 > d);
        var interior2 = (dot2 > d);

        if (!interior1)
        {
            // If neither endpoint is interior,
            // cylinder is not visible
            if (!interior2) return false;

            // p1 was outisde, so move it to the near plane
            var t = (d + p1.z) / dp.z;
            p1.x -= t * dp.x;
            p1.y -= t * dp.y;
            p1.z = -d;
        }
        else if (!interior2)
        {
            // p2 was outside, so move it to the near plane
            var t = (d + p1.z) / dp.z;
            p2.x = p1.x - t * dp.x;
            p2.y = p1.y - t * dp.y;
            p2.z = -d;
        }

        // Test endpoints against adjusted far plane
        d = mFarDistance + effectiveRadius;
        interior1 = (dot1 < d);
        interior2 = (dot2 < d);

        if (!interior1)
        {
            // If neither endpoint is interior,
            //cylinder is not visible
            if (!interior2) return false;

            // p1 was outside, so move it to the far plane
            var t = (d + p1.z) / (p2.z - p1.z);
            p1.x -= t * (p2.x - p1.x);
            p1.y -= t * (p2.y - p1.y);
            p1.z = -d;
        }
        else if (!interior2)
        {
            // p2 was outside, so move it to the far plane
            var t = (d + p1.z) / (p2.z - p1.z);
            p2.x = p1.x - t * (p2.x - p1.x);
            p2.y = p1.y - t * (p2.y - p1.y);
            p2.z = -d;
        }

        // Visit left side plane next.
        // The normal components have been precalculated
        var nx = mLeftRightX;
        var nz = mLeftRightZ;

        // Compute p1 * N and p2 * N
        dot1 = nx * p1.x - nz * p1.z;
        dot2 = nx * p2.x - nz * p2.z;

        // Calculate effective radius for this plane
        var s = nx * dp.x - nz * dp.z;
        effectiveRadius = -radius * Mathf.Sqrt(1.0F - s * s);

        // Test endpoints against adjusted plane
        interior1 = (dot1 > effectiveRadius);
        interior2 = (dot2 > effectiveRadius);

        if (!interior1)
        {
            // If neither endpoint is interior,
            // cylinder is not visible
            if (!interior2) return false;

            // p1 was outside, so move it to the plane
            var t = (effectiveRadius - dot1) / (dot2 - dot1);
            p1.x += t * (p2.x - p1.x);
            p1.y += t * (p2.y - p1.y);
            p1.z += t * (p2.z - p1.z);
        }
        else if (!interior2)
        {
            // p2 was outside, so move it to the plane
            var t = (effectiveRadius - dot1) / (dot2 - dot1);
            p2.x = p1.x + t * (p2.x - p1.x);
            p2.y = p1.y + t * (p2.y - p1.y);
            p2.z = p1.z + t * (p1.z - p1.z);
        }

        // Visit right side plane next
        dot1 = -nx * p1.x - nz * p1.z;
        dot2 = -nx * p2.x - nz * p2.z;

        s = -nx * dp.x - nz * dp.z;
        effectiveRadius = -radius * Mathf.Sqrt(1.0F - s * s);

        interior1 = (dot1 > effectiveRadius);
        interior2 = (dot2 > effectiveRadius);

        if (!interior1)
        {
            if (!interior2) return false;

            var t = (effectiveRadius - dot1) / (dot2 - dot1);
            p1.x += t * (p2.x - p1.x);
            p1.y += t * (p2.y - p1.y);
            p1.z += t * (p2.z - p1.z);
        }
        else if (!interior2)
        {
            var t = (effectiveRadius - dot1) / (dot2 - dot1);

            p2.x = p1.x + t * (p2.x - p1.x);
            p2.y = p1.y + t * (p2.y - p1.y);
            p2.z = p1.z + t * (p2.z - p1.z);
        }

        // Visit top side plane next
        // The normal components have been precalculated
        var ny = mTopBottomY;
        nz = mTopBottomZ;

        dot1 = -ny * p1.y - nz * p1.z;
        dot2 = -ny * p2.y - nz * p2.z;

        s = -ny * dp.y - nz * dp.z;
        effectiveRadius = -radius * Mathf.Sqrt(1.0F - s * s);

        interior1 = (dot1 > effectiveRadius);
        interior2 = (dot2 > effectiveRadius);

        if (!interior1)
        {
            if (!interior2) return false;

            var t = (effectiveRadius - dot1) / (dot2 - dot1);

            p1.x += t * (p2.x - p1.x);
            p1.y += t * (p2.y - p1.y);
            p1.z += t * (p2.z - p1.z);
        }
        else if (!interior2)
        {
            var t = (effectiveRadius - dot1) / (dot2 - dot1);

            p2.x = p1.x + t * (p2.x - p1.x);
            p2.y = p1.y + t * (p2.y - p1.y);
            p2.z = p1.z + t * (p2.z - p1.z);
        }

        // Finally, visit bottom side plane
        dot1 = ny * p1.y - nz * p1.z;
        dot2 = ny * p2.y - nz * p2.z;

        s = ny * dp.y - nz * dp.z;
        effectiveRadius = -radius * Mathf.Sqrt(1.0F - s * s);

        interior1 = (dot1 > effectiveRadius);
        interior2 = (dot2 > effectiveRadius);

        // At least one endpoint must be interior
        // or cylinder is not visible;
        return (interior1 | interior2);
    }
}

Frustum.cs

 

 

测试脚本

using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class FrustumTest : MonoBehaviour
{
    public float cylinderRadius = 1f;
    public float cylinderHeight = 1f;
    Frustum mFrustum;

    bool mCylinderVisible;


    void OnEnable()
    {
        var a = Screen.currentResolution.height / (float)Screen.currentResolution.width;
        mFrustum = new Frustum(Camera.main.fieldOfView * Mathf.Deg2Rad, a, Camera.main.nearClipPlane, Camera.main.farClipPlane);
    }

    void Update()
    {
        UnityEngine.Profiling.Profiler.BeginSample("Frustum Test");

        for (int i = 0; i < 1000; i++)
        {
            var worldToLocalMatrix = Camera.main.transform.worldToLocalMatrix;
            var p1 = -worldToLocalMatrix.MultiplyPoint3x4(transform.position + Vector3.up * cylinderHeight * 0.5f);
            var p2 = -worldToLocalMatrix.MultiplyPoint3x4(transform.position - Vector3.up * cylinderHeight * 0.5f);
            mCylinderVisible = mFrustum.CylinderVisible(p1, p2, cylinderRadius);

            //var planes = GeometryUtility.CalculateFrustumPlanes(Camera.main);
            //mCylinderVisible = GeometryUtility.TestPlanesAABB(planes, new Bounds(transform.position, cylinderHeight * Vector3.one));
        }

        UnityEngine.Profiling.Profiler.EndSample();
    }

    void OnDrawGizmos()
    {
        var oldColor = Gizmos.color;

        Gizmos.color = Color.blue;

        var cacheMatrix = Gizmos.matrix;
        Gizmos.matrix = Camera.main.transform.localToWorldMatrix;
        var a = Screen.currentResolution.width / (float)Screen.currentResolution.height;
        Gizmos.DrawFrustum(Vector3.zero, Camera.main.fieldOfView, Camera.main.farClipPlane, Camera.main.nearClipPlane, a);
        Gizmos.color = Color.white;
        Gizmos.matrix = cacheMatrix;

        if (mCylinderVisible)
            Gizmos.color = Color.red;

        DrawCylinder(transform.position, cylinderRadius, cylinderHeight);

        Gizmos.color = oldColor;
    }

    void DrawCylinder(Vector3 center, float radius, float height)
    {
        const float SEGMENT = 16f;
        var topCenter = center + height * 0.5f * Vector3.up;
        var bottomCenter = center - height * 0.5f * Vector3.up;

        var angle = 360 / SEGMENT;
        for (int i = 1; i <= SEGMENT + 1; i++)
        {
            var quat1 = Quaternion.AngleAxis(angle * (i - 1), Vector3.up);
            var quat2 = Quaternion.AngleAxis(angle * i, Vector3.up);

            var topEnd1 = quat1 * Vector3.forward * radius;
            var topEnd2 = quat2 * Vector3.forward * radius;
            topEnd1 += topCenter;
            topEnd2 += topCenter;
            var bottomEnd1 = quat1 * Vector3.forward * radius;
            var bottomEnd2 = quat2 * Vector3.forward * radius;
            bottomEnd1 += bottomCenter;
            bottomEnd2 += bottomCenter;

            Gizmos.DrawLine(topCenter, topEnd2);
            Gizmos.DrawLine(bottomCenter, bottomEnd2);

            Gizmos.DrawLine(topEnd1, topEnd2);
            Gizmos.DrawLine(bottomEnd1, bottomEnd2);

            Gizmos.DrawLine(topEnd2, bottomEnd2);
        }
    }
}