标签：
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
标签：
原文地址：http://blog.csdn.net/silangquan/article/details/50753583
