[Python知识库]射线与OBB相交检测

在上一篇 射线与AABB相交检测
射线与OBB3D 相交检测的原理跟射线与AABB相交检测的原理相同，本篇不再讲解原理
上篇推论出：射线与平面相交点距离射线起点距离t的距离公式为 t = (d - Dot(p, n)) / Dot(rayDir, n)

OBB3D 与 AABB 属性上的区别
AABB 三个轴向量固定： (1, 0, 0), (0, 1, 0), (0, 0, 1)
OBB3D 三个轴向量随旋转变化

AABB 的表示可以使用 min, max 坐标点表示
OBB3D 则没有 min, max 坐标点

所以AABB计算逻辑中使用到的坐标点和固定轴向，需要替换为 OBB3D 的属性，
轴向分别使用 _axisX，_axisY， _axisZ
取 OBB3D 两个顶点，每个顶点分别是三个面的交点或者叫顶点

_vertexs[0] = center + (axisX * size.x + axisY * size.y + axisZ * size.z) * 0.5f;
_vertexs[5] = center + (-axisX * size.x - axisY * size.y - axisZ * size.z) * 0.5f;

逻辑代码如下

public class OBB3D
{
    /// <summary>
    ///  X 轴方向向量
    /// </summary>
    public Vector3 _axisX;

    /// <summary>
    /// Y 轴方向向量
    /// </summary>
    public Vector3 _axisY;

    /// <summary>
    /// Z 轴方向向量
    /// </summary>
    public Vector3 _axisZ;

    /// <summary>
    /// 中心点坐标
    /// </summary>
    public Vector3 _center;

    /// <summary>
    /// 三条边长度
    /// </summary>
    public Vector3 _size;

    /// <summary>
    /// 八个顶点坐标
    /// </summary>
    public Vector3[] _vertexs;

    public OBB3D(){   }

    public void Set(Vector3 axisX, Vector3 axisY, Vector3 axisZ, Vector3 center, Vector3 size)
    {
        _axisX = axisX;
        _axisY = axisY;
        _axisZ = axisZ;
        _center = center;
        _size = size;

        _vertexs = new Vector3[8];
        _vertexs[0] = center + (axisX * size.x + axisY * size.y + axisZ * size.z) * 0.5f;
        _vertexs[1] = center + (axisX * size.x - axisY * size.y + axisZ * size.z) * 0.5f;
        _vertexs[2] = center + (axisX * size.x + axisY * size.y - axisZ * size.z) * 0.5f;
        _vertexs[3] = center + (axisX * size.x - axisY * size.y - axisZ * size.z) * 0.5f;
        _vertexs[4] = center + (-axisX * size.x + axisY * size.y - axisZ * size.z) * 0.5f;
        _vertexs[5] = center + (-axisX * size.x - axisY * size.y - axisZ * size.z) * 0.5f;
        _vertexs[6] = center + (-axisX * size.x - axisY * size.y + axisZ * size.z) * 0.5f;
        _vertexs[7] = center + (-axisX * size.x + axisY * size.y + axisZ * size.z) * 0.5f;
    }
}


/// <summary>
/// 射线与AABB相交检测
/// 下面方法是 射线与 3D AABB 相交的计算
/// 如果想计算 射线与 2D AABB 相交，则将下方关于 z 坐标的部分删除即可
/// </summary>
public class RayOBBCollision
{
    /// <summary>
    /// 判断射线与AABB是否相交
    /// </summary>
    /// <param name="raySource">射线起点</param>
    /// <param name="rayDir">射线方向</param>
    /// <param name="aabb">AABB</param>
    /// <param name="point">射线与AABB交点坐标</param>
    /// <returns></returns>
    public bool IsCollision(Vector3 raySource, Vector3 rayDir, OBB3D obb3D, ref Vector3 point)
    {
        float length = 0;
        bool collision = IsCollision(raySource, rayDir, obb3D, ref length);
        point = raySource + rayDir * length;
        return collision;
    }

    /// <summary>
    /// 判断射线与AABB是否相交
    /// </summary>
    /// <param name="raySource">射线起点</param>
    /// <param name="rayDir">射线方向向量</param>
    /// <param name="obb3D">OBB3D</param>
    /// <param name="length">射线起点到相交点距离</param>
    /// <returns></returns>
    public bool IsCollision(Vector3 raySource, Vector3 rayDir, OBB3D obb3D, ref float length)
    {
        float t1 = 0;
        float t2 = 0;
        bool collision = Calculate(raySource, rayDir, obb3D, obb3D._axisX, ref t1, ref t2);
        if (!collision || !CheckValue(ref t1, ref t2))
        {
            return false;
        }

        float t3 = 0;
        float t4 = 0;
        collision = Calculate(raySource, rayDir, obb3D, obb3D._axisY, ref t3, ref t4);
        if (!collision || !CheckValue(ref t3, ref t4))
        {
            return false;
        }

        float t5 = 0;
        float t6 = 0;
        collision = Calculate(raySource, rayDir, obb3D, obb3D._axisZ, ref t5, ref t6);
        if (!collision || !CheckValue(ref t5, ref t6))
        {
            return false;
        }

        float entryT1 = Math.Max(Math.Max(t1, t3), t5);
        float entryT2 = Math.Min(Math.Min(t2, t4), t6);
        length = entryT1;
        return (entryT1 < entryT2);
    }

    /// <summary>
    /// 射线与平面相交计算
    /// </summary>
    /// <param name="raySource">射线起点</param>
    /// <param name="rayDir">射线方向向量</param>
    /// <param name="obb3D">aabb</param>
    /// <param name="normal">平面法向量</param>
    /// t = (d - Dot(raySource, normal)) / Dot(rayDir, normal)
    /// d = Dot(planePoint, normal)
    /// t = (Dot(planePoint, normal)  - Dot(raySource, normal)) / Dot(rayDir, normal)
    /// t = Dot((planePoint - raySource), normal) / Dot(rayDir, normal)
    private bool Calculate(Vector3 raySource, Vector3 rayDir, OBB3D obb3D, Vector3 normal, ref float t1, ref float t2)
    {
        float p_sub_r_dot_n1 = Vector3.Dot(obb3D._vertexs[0] - raySource, normal);
        float p_sub_r_dot_n2 = Vector3.Dot(obb3D._vertexs[5] - raySource, normal);
        float r_dot_n = Vector3.Dot(rayDir, normal);

        if (Math.Abs(r_dot_n) <= float.Epsilon)  // 射线垂直于平面法向量，所以射线与平面平行
        {
            float dot1 = Vector3.Dot(obb3D._vertexs[0] - raySource, normal);
            float dot2 = Vector3.Dot(obb3D._vertexs[5] - raySource, normal);
            if (dot1 * dot2 > 0)
            {
                return false;
            }
        }

        t1 = p_sub_r_dot_n1 / r_dot_n;
        t2 = p_sub_r_dot_n2 / r_dot_n;
        return true;
    }

    private bool CheckValue(ref float value1, ref float value2)
    {
        if (value1 < 0 && value2 < 0)
        {
            return false;
        }
        value1 = Mathf.Clamp(value1, 0, value1);
        value2 = Mathf.Clamp(value2, 0, value2);
        if (value1 > value2)
        {
            float temp = value1;
            value1 = value2;
            value2 = temp;
        }
        return true;
    }
}

测试代码如下

using System;
using UnityEngine;

public class RayOBBController : MonoBehaviour
{
    // Cube 立方体 A
    public Transform cube;

    // 定义两个 OBB3D 立方体
    private OBB3D obb3D;

    // 相交检测逻辑
    private RayOBBCollision rayOBBCollision;

    void Start()
    {
        // 实例化
        obb3D = new OBB3D();
        rayOBBCollision = new RayOBBCollision();
    }

    private bool result = false;
    // 创建一个小球，当射线与AABB相交时，将交点坐标设置给小球
    private GameObject go;
    void Update()
    {
        // 将两个Cube 的数据分别赋值给 OBB3D
        SetOBB(cube, obb3D);

        Vector3 point = Vector3.zero;
        // 判断是否相交
        bool isCollision = rayOBBCollision.IsCollision(transform.position, transform.forward, obb3D, ref point);
        if (result != isCollision)
        {
            result = isCollision;
            // 如果设想与AABB相交，将AABB物体设置为红色
            cube.GetComponent<Renderer>().material.color = result ? Color.red : Color.gray;
        }

        if (result)
        {
            // 如果射线与AABB相交，将小球坐标设置为相交点
            if (!go)
            {
                go = GameObject.CreatePrimitive(PrimitiveType.Sphere);
                go.transform.localScale = Vector3.one * 0.2f;
            }
            go.transform.position = point;
        }
    }

    private void SetOBB(Transform tr, OBB3D obb)
    {
        // OBB3D 的三个轴分别使用 Cube.transform：right、up、forward
        // OBB3D 的坐标使用 Cube.transform.position
        // OBB3D 的size 使用 Cube.transform.localScale
        obb.Set(tr.right, tr.up, tr.forward, tr.position, tr.localScale);
    }
}