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