碰撞检测之Ray-Capsule检测

Posted 2020-06-17 拳四郎

Capsule的射线检测和Cylinder的类似，只是把上下两个面换成了两个半球，代码上稍作区别即可。

Capsule类型定义

 public class Capsule : NGeometry
    {
        public Vector3 p0;
        public Vector3 p1;
        public float radius;

        public Capsule(Vector3 _p0, Vector3 _p1, float _radius)
            : base(GeometryType.Capsule)
        {
            p0 = _p0;
            p1 = _p1;
            radius = _radius;
        }
        public Capsule() : base(GeometryType.Capsule) { }

        public Vector3 ComputeDirection()
        {
            return p1 - p0;
        }
    }

射线检测

  public static bool Raycast(Ray ray, float distance, Capsule capsule, out RaycastHitInfo hitInfo)
        {
            hitInfo = new RaycastHitInfo();
            Vector3 capsuleDir = capsule.ComputeDirection();
            Vector3 kW = capsuleDir;
            float fWLength = kW.magnitude;
            kW.Normalize();

            // PT: if the capsule is in fact a circle, switch back to dedicated plane code.
            // This is not just an optimization, the rest of the code fails otherwise.
            if (fWLength <= 1e-6f)
            {
                return Raycast(ray, distance, new Sphere(capsule.p0, capsule.radius), out hitInfo);
            }

            // generate orthonormal basis
            //cylinder along the z direction
            Vector3 kU = Vector3.zero;
            if (fWLength > 0.0f)
            {
                float fInvLength;
                if (Mathf.Abs(kW.x) >= Mathf.Abs(kW.y))
                {
                    fInvLength = 1.0f / Mathf.Sqrt(kW.x * kW.x + kW.z * kW.z);
                    kU.x = -kW.z * fInvLength;
                    kU.y = 0.0f;
                    kU.z = kW.x * fInvLength;
                }
                else
                {
                    // W.y or W.z is the largest magnitude component, swap them
                    fInvLength = 1.0f / Mathf.Sqrt(kW.y * kW.y + kW.z * kW.z);
                    kU.x = 0.0f;
                    kU.y = kW.z * fInvLength;
                    kU.z = -kW.y * fInvLength;
                }
            }
            Vector3 kV = Vector3.Cross(kW, kU);
            kV.Normalize();

            // compute intersection
            //Transform the ray to the cylinder‘s local coordinate
            //new Ray direction
            Vector3 kD = new Vector3(Vector3.Dot(kU, ray.direction), Vector3.Dot(kV, ray.direction), Vector3.Dot(kW, ray.direction));
            float fDLength = kD.magnitude;
            kD.Normalize();

            float fInvDLength = 1.0f / fDLength;
            Vector3 kDiff = ray.origin - capsule.p0;
            //new Ray origin
            Vector3 kP = new Vector3(Vector3.Dot(kU, kDiff), Vector3.Dot(kV, kDiff), Vector3.Dot(kW, kDiff));

            float fRadiusSqr = capsule.radius * capsule.radius;

            // Is the ray direction parallel to the cylinder direction? (or zero)
            if (Mathf.Abs(kD.z) >= 1.0f - Mathf.Epsilon || fDLength < Mathf.Epsilon)
            {
                float fAxisDir = Vector4.Dot(ray.direction, capsuleDir);

                float fDiscr = fRadiusSqr - kP.x * kP.x - kP.y * kP.y;
                // direction anti-parallel to the capsule direction
                if (fAxisDir < 0 && fDiscr >= 0.0f)
                {
                    float fRoot = Mathf.Sqrt(fDiscr);
                    //Ray origin in the top of capsule.
                    if (kP.z > fWLength + fRoot)
                    {
                        hitInfo.distance = (kP.z - fWLength - fRoot) * fInvDLength;
                    }
                    //Ray origin in the bottom of capsule.
                    else if (kP.z < -fRoot)
                    {
                        return false;
                    }
                    //Ray origin one the capsule.
                    else if (kP.z > -fRoot && kP.z < fWLength + fRoot)
                    {
                        hitInfo.distance = (kP.z + fRoot) * fInvDLength;
                    }

                    if (hitInfo.distance > distance)
                        return false;
                    hitInfo.point = hitInfo.distance * ray.direction;
                    return true;
                }
                // direction parallel to the capsule direction
                else if (fAxisDir > 0 && fDiscr >= 0.0f)
                {
                    float fRoot = Mathf.Sqrt(fDiscr);
                    if (kP.z > fWLength + fRoot)
                    {
                        return false;
                    }
                    else if (kP.z < -fRoot)
                    {
                        hitInfo.distance = (-kP.z - fRoot) * fInvDLength;
                    }
                    else if (kP.z > -fRoot && kP.z < fWLength + fRoot)
                    {
                        hitInfo.distance = (fWLength - kP.z + fRoot) * fInvDLength;
                    }
                    if (hitInfo.distance > distance)
                        return false;
                    hitInfo.point = hitInfo.distance * ray.direction;
                    return true;
                }
                else
                {
                    //ray origin out of the circle
                    return false;
                }
            }

            // Test intersection with infinite cylinder
            // set up quadratic Q(t) = a*t^2 + 2*b*t + c
            float fA = kD.x * kD.x + kD.y * kD.y;
            float fB = kP.x * kD.x + kP.y * kD.y;
            float fC = kP.x * kP.x + kP.y * kP.y - fRadiusSqr;
            float delta = fB * fB - fA * fC;
            // line does not intersect infinite cylinder
            if (delta < 0.0f)
            {
                return false;
            }

            // line intersects infinite cylinder in two points
            if (delta > 0.0f)
            {
                float fRoot = Mathf.Sqrt(delta);
                float fInv = 1.0f / fA;
                float fT = (-fB - fRoot) * fInv;
                float fTmp = kP.z + fT * kD.z;
                float dist0 = 0f, dist1 = 0f;

                float fT1 = (-fB + fRoot) * fInv;
                float fTmp1 = kP.z + fT * kD.z;

                //cast two point
                //fTmp <= fWLength to check intersect point between slab.
                if ((0.0f <= fTmp && fTmp <= fWLength) && (0.0f <= fTmp1 && fTmp1 <= fWLength))
                {
                    dist0 = fT * fInvDLength;
                    dist1 = fT1 * fInvDLength;
                    hitInfo.distance = Mathf.Min(dist0, dist1);
                    return true;
                }
                else if ((0.0f <= fTmp && fTmp <= fWLength))
                {
                    dist0 = fT * fInvDLength;
                    hitInfo.distance = dist0;
                    return true;
                }
                else if ((0.0f <= fTmp1 && fTmp1 <= fWLength))
                {
                    dist1 = fT1 * fInvDLength;
                    hitInfo.distance = dist1;
                    return true;
                }
            }
            // line is tangent to infinite cylinder
            else
            {
                float fT = -fB / fA;
                float fTmp = kP.z + fT * kD.z;
                if (0.0f <= fTmp && fTmp <= fWLength)
                {
                    hitInfo.distance = fT * fInvDLength;
                    return true;
                }
            }

            // test intersection with bottom hemisphere
            // fA = 1
            fB += kP.z * kD.z;
            fC += kP.z * kP.z;
            float distanceSqrt = fB * fB - fC;
            if (distanceSqrt > 0.0f)
            {
                float fRoot = Mathf.Sqrt(distanceSqrt);
                float fT = -fB - fRoot;
                float fTmp = kP.z + fT * kD.z;
                if (fTmp <= 0.0f)
                {
                    hitInfo.distance = fT * fInvDLength;
                    if (hitInfo.distance > distance)
                        return false;
                    hitInfo.point = hitInfo.distance * ray.direction;
                    return true;
                }

            }
            else if (distanceSqrt == 0.0f)
            {
                float fT = -fB;
                float fTmp = kP.z + fT * kD.z;
                if (fTmp <= 0.0f)
                {
                    hitInfo.distance = fT * fInvDLength;
                    if (hitInfo.distance > distance)
                        return false;
                    hitInfo.point = hitInfo.distance * ray.direction;
                    return true;

                }
            }

            // test intersection with top hemisphere
            // fA = 1
            fB -= kD.z * fWLength;
            fC += fWLength * (fWLength - 2.0f * kP.z);

            distanceSqrt = fB * fB - fC;
            if (distanceSqrt > 0.0f)
            {
                float fRoot = Mathf.Sqrt(distanceSqrt);
                float fT = -fB - fRoot;
                float fTmp = kP.z + fT * kD.z;
                if (fTmp >= fWLength)
                {
                    hitInfo.distance = fT * fInvDLength;
                    if (hitInfo.distance > distance)
                        return false;
                    hitInfo.point = hitInfo.distance * ray.direction;

                    return true;
                }

                fT = -fB + fRoot;
                fTmp = kP.z + fT * kD.z;
                if (fTmp >= fWLength)
                {
                    hitInfo.distance = fT * fInvDLength;
                    if (hitInfo.distance > distance)
                        return false;
                    hitInfo.point = hitInfo.distance * ray.direction;
                    return true;

                }
            }
            else if (distanceSqrt == 0.0f)
            {
                float fT = -fB;
                float fTmp = kP.z + fT * kD.z;
                if (fTmp >= fWLength)
                {
                    hitInfo.distance = fT * fInvDLength;
                    if (hitInfo.distance > distance)
                        return false;
                    hitInfo.point = hitInfo.distance * ray.direction;
                    return true;
                }
            }

            return false;
        }

测试代码

public class RayCapsuleTester : MonoBehaviour {
    public CapsuleCollider capsule;
    Capsule _capsule;
    Ray ray;
    float castDistance = 10f;
    // Use this for initialization
    void Start()
    {
        _capsule = new Capsule();
        ray = new Ray(Vector3.zero, new Vector3(1, 1, 1));
    }

    // Update is called once per frame
    void Update()
    {
        _capsule.radius = capsule.radius;
        if (capsule.height < 2.0f * capsule.radius)
        {
            _capsule.p0 = capsule.transform.position;
            _capsule.p1 = capsule.transform.position;
        }
        else
        {
            //In unity capsule collider height include hemisphere height.
            float realHeight = capsule.height - 2.0f * capsule.radius;
            _capsule.p0 = capsule.transform.position + capsule.transform.rotation * Vector3.down * realHeight * 0.5f;
            _capsule.p1 = capsule.transform.position + capsule.transform.rotation * Vector3.up * realHeight * 0.5f;
        }

        Debug.DrawLine(_capsule.p0, _capsule.p1, Color.black);


        RaycastHitInfo hitinfo = new RaycastHitInfo();

        if (NRaycastTests.Raycast(ray, castDistance, _capsule, out hitinfo))
        {
            Debug.DrawLine(ray.origin, ray.origin + ray.direction * hitinfo.distance, Color.red, 0, true);
        }
        else
        {
            Debug.DrawLine(ray.origin, ray.origin + ray.direction * castDistance, Color.blue, 0, true);
        }
    }
}

运行结果

参考

PhysX 3.3 source code