Capsule的射线检测和Cylinder的类似,只是把上下两个面换成了两个半球,代码上稍作区别即可。
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