public static bool Raycast(Ray ray, out float t, Vector3 coneBaseCenter, float coneBaseRadius, float coneHeight, Quaternion coneRotation, ConeEpsilon epsilon = new ConeEpsilon())
{
t = 0.0f;
Ray coneSpaceRay = ray.InverseTransform(Matrix4x4.TRS(coneBaseCenter, coneRotation, Vector3.one));
float xzAABBSize = coneBaseRadius * 2.0f;
Vector3 aabbSize = new Vector3(xzAABBSize, coneHeight + epsilon.VertEps * 2.0f, xzAABBSize);
if (!BoxMath.Raycast(coneSpaceRay, Vector3.up * coneHeight * 0.5f, aabbSize, Quaternion.identity))
{
return(false);
}
// We will first perform a preliminary check to see if the ray intersects the bottom cap of the cone.
// This is necessary because the cone equation views the cone as infinite (i.e. no bottom cap), and
// if we didn't perform this check, we would never be able to tell when the bottom cap was hit.
float rayEnter;
Plane bottomCapPlane = new Plane(-Vector3.up, Vector3.zero);
if (bottomCapPlane.Raycast(coneSpaceRay, out rayEnter))
{
// If the ray intersects the plane of the bottom cap, we will calculate the intersection point
// and if it lies inside the cone's bottom cap area, it means we have a valid intersection. We
// store the t value and then return true.
Vector3 intersectionPoint = coneSpaceRay.origin + coneSpaceRay.direction * rayEnter;
if (intersectionPoint.magnitude <= coneBaseRadius)
{
t = rayEnter;
return(true);
}
}
// We need this for the calculation of the quadratic coefficients
float ratioSquared = coneBaseRadius / coneHeight;
ratioSquared *= ratioSquared;
// Calculate the coefficients.
// Note: The cone equation which was used is: (X^2 + Z^2) / ratioSquared = (Y - coneHeight)^2.
// Where X, Y and Z are the coordinates of the point along the ray: (Origin + Direction * t).xyz
float a = coneSpaceRay.direction.x * coneSpaceRay.direction.x + coneSpaceRay.direction.z * coneSpaceRay.direction.z - ratioSquared * coneSpaceRay.direction.y * coneSpaceRay.direction.y;
float b = 2.0f * (coneSpaceRay.origin.x * coneSpaceRay.direction.x + coneSpaceRay.origin.z * coneSpaceRay.direction.z - ratioSquared * coneSpaceRay.direction.y * (coneSpaceRay.origin.y - coneHeight));
float c = coneSpaceRay.origin.x * coneSpaceRay.origin.x + coneSpaceRay.origin.z * coneSpaceRay.origin.z - ratioSquared * (coneSpaceRay.origin.y - coneHeight) * (coneSpaceRay.origin.y - coneHeight);
// The intersection happnes only if the quadratic equation has solutions
float t1, t2;
if (MathEx.SolveQuadratic(a, b, c, out t1, out t2))
{
// Make sure the ray does not intersect the cone only from behind
if (t1 < 0.0f && t2 < 0.0f)
{
return(false);
}
// Make sure we are using the smallest positive t value
if (t1 < 0.0f)
{
float temp = t1;
t1 = t2;
t2 = temp;
}
t = t1;
// Make sure the intersection point does not sit below the cone's bottom cap or above the cone's cap
Vector3 intersectionPoint = coneSpaceRay.origin + coneSpaceRay.direction * t;
if (intersectionPoint.y < -epsilon.VertEps || intersectionPoint.y > coneHeight + epsilon.VertEps)
{
t = 0.0f;
return(false);
}
// The intersection point is valid
return(true);
}
// If we reached this point, it means the ray does not intersect the cone in any way
return(false);
}
public static bool ContainsPoint(Vector3 point, Vector3 coneBaseCenter, float coneBaseRadius, float coneHeight, Quaternion coneRotation, ConeEpsilon epsilon = new ConeEpsilon())
{
Matrix4x4 coneTransform = Matrix4x4.TRS(coneBaseCenter, coneRotation, Vector3.one);
point = coneTransform.inverse.MultiplyPoint(point);
float dotConeAxis = Vector3.Dot(Vector3.up, point);
if (dotConeAxis < -epsilon.VertEps || dotConeAxis > coneHeight + epsilon.VertEps)
{
return(false);
}
float tan = coneHeight / coneBaseRadius;
float coneRadius = dotConeAxis * tan;
return(point.GetDistanceToSegment(Vector3.zero, Vector3.up * coneHeight) <= (coneRadius + epsilon.HrzEps));
}