碰撞检测之Ray-Cylinder检测

来源：互联网发布：ubuntu安装xwindow 编辑：程序博客网时间：2024/05/23 14:09

正交基和标准正交基

首先来看定义

Let S = {v₁, v₂, ... , v_k} be a set of vectors in Rⁿ, then S is called an orthogonal if

v_i . v_j=0

for all i not equal to j. An orthogonal set of vectors is called orthonormal if all vectors in S are unit vectors.

咳咳，翻译一下

对于 Rⁿ中的一个向量集合 S={v₁, v₂, ... , v_k} ，如果存在

v_i . v_j = 0 (i != j)

那么它们就是一组正交基，如果它们都是单位向量，则它们还是标准正交基。

定理

1.给定一组正交基S = {v1, v2, ... , vk}，那么他们是线性相关的。

2.对于 Rⁿ中的一个正交基 S={v₁, v₂, ... , v_k},则 Rⁿ中的向量 v 的以S为基的第 i 个坐标为

v . v_j

例

求向量(5, 10) 在基 S = {(3/5, 4/5), (-4/5, 3/5)}中的坐标。

(5,10)^.(3/5,4/5) = 11

(5,10)^.(-4/5, 3/5) = 2

[(5,10)]_S= (11,2)

给定一个空间向量，求出以这个向量为轴的正交基

解

假设给定的空间向量为

v = (a,b,c);

则与v垂直的向量必定满足

a*x + b*y + c*z = 0

这个向量可以是u = (0;-c;b) 或者 u = (-c,0,a);

已知两个向量，第三个向量利用向量叉乘就可以得出

w = cross(u,v)

最后将u,v,w单位化，得到新的正交基。

不同正交基下的坐标变换

同一个向量，在不同的基下面的坐标是不同的。因此，可以用正交矩阵来代表坐标系（也可以看作基）从而写出在统一的参考系（全局坐标系）下同一个向量在不同基中的坐标。

已知一个基Q和向量v在它之中的坐标v’，以及另外一个基R，我们可以通过v=Qv’=Rv’’公式来计算v’’。

上面就是求v’’的公式，注意到右边需要计算基R的逆矩阵R^-1，因为基R是正交矩阵，而正交矩阵的一个重要性质就是逆等于转置。因此，我们可以把它写成

这个公式就是坐标转换公式。特别地，如果Q是和参考系相同的坐标系（3D编程中大多数情况下如此），比如世界坐标系，则Q是一个单位矩阵I，则我们可以把它写成

这个坐标转换公式可以解释为：对于世界坐标系中的向量v’，它在坐标系R中的坐标是v’’

我们还可以得到哟个有趣的结论：

假设原始的坐标系为x-(1,0,0), y-(0,1,0), z(0,0,1),这个坐标系经过一个旋转矩阵Matrix旋转之后，新的坐标系就是这个旋转矩阵的转置三个列向量。

圆柱体的射线求交

上面啰嗦到的两个地方这里都会用到。

首先还是定义圆柱体的类

 public class Cylinder : NGeometry    {        public Vector3 p0;        public Vector3 p1;        public float radius;        public Cylinder(Vector3 _p0, Vector3 _p1, float _radius)            : base(GeometryType.Cylinder)        {            p0 = _p0;            p1 = _p1;            radius = _radius;        }        public Cylinder() : base(GeometryType.Cylinder) { }        public Vector3 ComputeDirection()        {            return p1 - p0;        }    }

看检测的代码，有点长

 public static bool Raycast(Ray ray, float distance, Cylinder cylinder, out RaycastHitInfo hitInfo)        {            hitInfo = new RaycastHitInfo();            Vector3 cylinderDir = cylinder.ComputeDirection();            Vector3 kW = cylinderDir;            float fWLength = kW.magnitude;            kW.Normalize();            //two thin for check            if (fWLength <= 1e-6f)            {                return false;            }            //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;            Debug.Log("fDLength: " + fDLength);            kD.Normalize();            float fInvDLength = 1.0f / fDLength;            Vector3 kDiff = ray.origin - cylinder.p0;            //new Ray origin            Vector3 kP = new Vector3(Vector3.Dot(kU, kDiff), Vector3.Dot(kV, kDiff), Vector3.Dot(kW, kDiff));            float fRadiusSqr = cylinder.radius * cylinder.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, cylinderDir);                float fDiscr = fRadiusSqr - kP.x * kP.x - kP.y * kP.y;                // ray direction anti-parallel to the cylinder direction                if (fAxisDir < 0 && fDiscr >= 0.0f)                {                    if (kP.z > fWLength)                    {                        hitInfo.distance = (kP.z - fWLength) * fInvDLength;                    }                    else if (kP.z < 0)                    {                        return false;                    }                    else if (kP.z > 0 && kP.z < fWLength)                    {                        hitInfo.distance = kP.z * fInvDLength;                    }                    if (hitInfo.distance > distance)                        return false;                    hitInfo.point = hitInfo.distance * ray.direction;                    return true;                }                // ray direction parallel to the cylinder direction                else if (fAxisDir > 0 && fDiscr >= 0.0f)                {                    if (kP.z > fWLength)                    {                        return false;                    }                    else if (kP.z < 0)                    {                        hitInfo.distance = -kP.z * fInvDLength;                    }                    else if (kP.z > 0 && kP.z < fWLength)                    {                        hitInfo.distance = (fWLength - kP.z) * 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;                }                //If intersect the infinite cylinder but point not between slab, the ray may intersect cylinder's caps.                //Test intersection with caps                float deltaAngle = Vector4.Dot(ray.direction, cylinderDir);                // Ray direction anti-parallel to the capsule direction                if (deltaAngle < 0)                {                    if (kP.z > fWLength)                    {                        float deltaZ = kP.z - fWLength;                        float angle = Vector3.Angle(ray.direction, -cylinderDir);                        hitInfo.distance = (kP.z - fWLength) * fInvDLength / Mathf.Cos(angle * Mathf.Deg2Rad);                    }                    else if (kP.z < 0)                    {                        Debug.Log("No cap0");                        return false;                    }                    if (hitInfo.distance > distance)                        return false;                    hitInfo.point = ray.origin + hitInfo.distance * ray.direction;                    if (Vector3.Distance(hitInfo.point, cylinder.p1) > cylinder.radius)                    {                        return false;                    }                    return true;                }                // Direction parallel to the cylinder direction                else if (deltaAngle > 0)                {                    if (kP.z > fWLength)                    {                        Debug.Log("No cap1");                        return false;                    }                    else if (kP.z < 0)                    {                        float angle = Vector3.Angle(ray.direction, cylinderDir);                        hitInfo.distance = -kP.z * fInvDLength / Mathf.Cos(angle * Mathf.Deg2Rad);                    }                    if (hitInfo.distance > distance)                        return false;                    hitInfo.point = ray.origin + hitInfo.distance * ray.direction;                    if (Vector3.Distance(hitInfo.point, cylinder.p0) > cylinder.radius)                    {                        return false;                    }                    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;                }            }            return false;        }

简单梳理一下流程

1.将坐标系转换到了以圆柱体方向为轴的坐标系，用的就是给定一个空间向量，求出以这个向量为轴的正交基中提到的方法；

2.将Ray的origin和Direction都转到新的坐标系下，用的是上面的定理2。新的ray的origin为kP，direction为kD，射线上的点可以表示为kP+t*kD；

3.判断射线方向是否与Z轴方向平行，如果是，判断是否是圆柱体的上下面相交；

4.判断是否是圆柱相交，这里先假设圆柱是无限的，判断的方法是求解二次方程。这里详细说一下，二次函数的形式为

Q(t) = a*t^2 + 2*b*t + c

假设设想和圆柱相交，则必定存在射线上存在t，满足点kP+t*kD到Z轴的距离为圆柱的半径

(kP.x +t*kD.x)^2 +(kP.y +t*kD.y)^2 = R^2

先通过delta判断根的个数，最后求解就可以得到结果。

测试代码

using UnityEngine;using System.Collections;using NPhysX;public class RayCylinderTester : MonoBehaviour {    public GameObject cylinder;    Cylinder _cylinder;    Ray ray;    float castDistance = 10f;    // Use this for initialization    void Start () {        _cylinder = new Cylinder();    }// Update is called once per framevoid Update () {        ray = new Ray(Vector3.zero, new Vector3(1, 1, 1));        _cylinder.radius = 0.5f * cylinder.transform.localScale.x;        _cylinder.p0 = cylinder.transform.position + cylinder.transform.rotation * Vector3.down * cylinder.transform.localScale.y;        _cylinder.p1 = cylinder.transform.position + cylinder.transform.rotation * Vector3.up * cylinder.transform.localScale.y;        Debug.DrawLine(_cylinder.p0, _cylinder.p1, Color.green);        RaycastHitInfo hitinfo = new RaycastHitInfo();        if (NRaycastTests.Raycast(ray, castDistance, _cylinder, 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);        }    }}

参考

Orthonormal Bases in Rn - http://ltcconline.net/greenl/courses/203/vectors/orthonormalbases.htm

PhysX 3.3 source code

Create orthonormal basis from a given vector - http://www.mathworks.com/matlabcentral/answers/72631-create-orthonormal-basis-from-a-given-vector

推导相机变换矩阵 - http://blog.csdn.net/popy007/article/details/5120158

0 0