Ray-capsule Detection of collision detection

Capsule ray detection and cylinder similar, only the upper and lower two faces replaced by two hemispheres, the code is slightly different.

Capsule type definition

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;
   }    }

Radiographic Inspection

  public static bool Raycast (Ray Ray, float distance, Capsule Capsule, out Raycasthitinfo hitinfo) {hit            Info = 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 isn't 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.zer            O                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.dir            ection));            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.dista                    NCE = (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 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//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; }}//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 * Finvdlengt                    H                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 * F            Wlength;            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; }

Test code

public class Raycapsuletester:monobehaviour {public Capsulecollider capsule;    Capsule _capsule;    Ray Ray;    float castdistance = 10f;        Use the 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.or        Igin + ray.direction * hitinfo.distance, color.red, 0, true); } else {debug.drawline (ray.origin, Ray.origin + ray.direction * castdistance, Color.Blue, 0, Tru        e); }    }}

Run results

Reference

PhysX 3.3 Source Code

Ray-capsule Detection of collision detection