public static bool IsPlaneIntersectingCone(Vect3 planeNormal, Vect3 planePoint, Vect3 conePosition,
Vect3 coneAxis, double coneAxisLength, double coneCosPhi)
{
double len;
//try "bending" axis towards plane normal
Vect3 q = conePosition + coneAxis * coneAxisLength;
Vect3 qx = planeNormal.ProjectOn(coneAxis);
Vect3 p = qx - planeNormal;
if (System.Math.Abs(p.QuadLength() - 0) < Constants.EPS)
{
// axis equals plane normal
p = coneAxis;
len = coneAxisLength;
}
else
{
//bend axis towards plane normal as far as sinPhi allows
p = p.Normalize();
q = q + (p * coneAxisLength * System.Math.Sin(System.Math.Acos(coneCosPhi)));
q -= conePosition;
len = q.Length();
p = q / len;
}
double d = RayPlane.GetHitPointRayPlaneDistance(conePosition, p, planePoint, planeNormal);
return(d < len);
}
public static bool IsAreaIntersectingCone(Vect3 planeNormal, Vect3 planePoint, double areaWidthHalf,
Vect3 conePosition, Vect3 coneAxis, double coneAxisLength,
double coneCosPhi)
{
double len;
//try "bending" axis towards plane normal
Vect3 q = conePosition + coneAxis * coneAxisLength;
Vect3 qx = planeNormal.ProjectOn(coneAxis);
Vect3 p = qx - planeNormal;
if (System.Math.Abs(p.QuadLength() - 0) < Constants.EPS)
{
// axis equals plane normal
p = coneAxis;
len = coneAxisLength;
}
else
{
//bend axis towards plane normal as far as sinPhi allows
p = p.Normalize();
q = q + (p * coneAxisLength * System.Math.Sin(System.Math.Acos(coneCosPhi)));
q -= conePosition;
len = q.Length();
p = q / len;
}
double d = RayPlane.GetHitPointRayPlaneDistance(conePosition, p, planePoint, planeNormal);
if (d < len)
{
//check if Hitpoint is in the +/-width/2 - area of the plane
p = conePosition + p * d;
return
(System.Math.Abs(p.X - planePoint.X) < areaWidthHalf * 2 &&
System.Math.Abs(p.Y - planePoint.Y) < areaWidthHalf * 2 &&
System.Math.Abs(p.Z - planePoint.Z) < areaWidthHalf * 2);
}
return(false);
}
public static double GetHitPointRayTriangleDistance(Vect3 rayPosition, Vect3 rayDirection, Vect3 trianglePos,
Vect3 triangleVect1, Vect3 triangleVect2)
{
Vect3 tmp = triangleVect1.CrossProduct(triangleVect2);
double ret = RayPlane.GetHitPointRayPlaneDistance(rayPosition, rayDirection, trianglePos, tmp);
if (double.IsPositiveInfinity(ret) || ret < Constants.EPS)
{
return(double.PositiveInfinity);
}
tmp = rayPosition + rayDirection * ret;
tmp -= trianglePos;
double uu = triangleVect1 * triangleVect1;
double uv = triangleVect1 * triangleVect2;
double vv = triangleVect2 * triangleVect2;
double wu = triangleVect1 * tmp;
double wv = triangleVect2 * tmp;
double d = uv * uv - uu * vv;
double s = (uv * wv - vv * wu) / d;
if (s < -TriEPS || s > 1 + TriEPS)
{
return(double.PositiveInfinity);
}
double t = (uv * wu - uu * wv) / d;
if (t < -TriEPS || s + t > 1 + TriEPS)
{
return(double.PositiveInfinity);
}
return(ret);
}
