private static bool Quadratic(TScalar a, TScalar b, TScalar c, out TScalar root1, out TScalar root2)
{
var q = (b * b) - (NumberValues.Four <TScalar>() * a * c);
if (q >= TScalar.Zero)
{
var sq = TScalar.Sqrt(q);
var d = TScalar.One / (NumberValues.Two <TScalar>() * a);
root1 = (-b + sq) * d;
root2 = (-b - sq) * d;
return(true);
}
else
{
root1 = TScalar.NaN;
root2 = TScalar.NaN;
return(false);
}
}
public Frustum(
TScalar aspectRatio,
Vector3 <TScalar> axis,
TScalar fieldOfViewAngle,
TScalar nearPlaneDistance,
Vector3 <TScalar> position,
Quaternion <TScalar> rotation)
{
AspectRatio = aspectRatio;
Axis = axis;
FieldOfViewAngle = fieldOfViewAngle;
NearPlaneDistance = nearPlaneDistance;
Position = position;
Rotation = rotation;
var farPlaneDistSq = Vector3 <TScalar> .LengthSquared(Axis);
FarPlaneDistance = TScalar.Sqrt(farPlaneDistSq);
var two = NumberValues.Two <TScalar>();
var tan = TScalar.Tan(fieldOfViewAngle);
var tanSq4 = tan.Square() * NumberValues.Four <TScalar>();
var tanAR = tan * aspectRatio;
Volume = tanSq4
* aspectRatio
* ((farPlaneDistSq * FarPlaneDistance) - nearPlaneDistance.Cube())
/ NumberValues.Three <TScalar>();
SmallestDimension = aspectRatio <= TScalar.One
? two * tanAR * nearPlaneDistance
: two * tan * nearPlaneDistance;
ContainingRadius = TScalar.Sqrt(
(FarPlaneDistance - nearPlaneDistance).Square()
+ (tanSq4 * farPlaneDistSq * (TScalar.One + aspectRatio.Square())))
/ two;
axis = position - (axis / two);
var basis1 = Vector3 <TScalar> .Transform(Vector3 <TScalar> .Normalize(axis), rotation);
Vector3 <TScalar> basis2, basis3;
if (basis1.Z.IsNearlyEqualTo(TScalar.NegativeOne))
{
basis2 = new Vector3 <TScalar>(TScalar.Zero, TScalar.NegativeOne, TScalar.Zero);
basis3 = new Vector3 <TScalar>(TScalar.NegativeOne, TScalar.Zero, TScalar.Zero);
}
else
{
var a = TScalar.One / (TScalar.One + basis1.Z);
var b = -basis1.X * basis1.Y * a;
basis2 = new Vector3 <TScalar>(TScalar.One - (basis1.X.Square() * a), b, -basis1.X);
basis3 = new Vector3 <TScalar>(b, TScalar.One - (basis1.Y.Square() * a), -basis1.Y);
}
var farY = basis2 * farPlaneDistSq * tanAR;
var farZ = basis3 * tan * FarPlaneDistance;
var nearX = basis1 * nearPlaneDistance;
var nearY = basis2 * nearPlaneDistance.Square() * tanAR;
var nearZ = basis3 * tan * nearPlaneDistance;
Corners = new Vector3 <TScalar>[8]
{
Position + axis - farY + farZ,
Position + axis + farY + farZ,
Position + axis - farY - farZ,
Position + axis + farY - farZ,
Position + nearX + nearY - nearZ,
Position + nearX - nearY - nearZ,
Position + nearX - nearY + nearZ,
Position + nearX + nearY + nearZ
};
HighestPoint = Corners[0];
LowestPoint = Corners[0];
for (var i = 1; i < 8; i++)
{
if (Corners[i].Y > HighestPoint.Y)
{
HighestPoint = Corners[i];
}
if (Corners[i].Y < LowestPoint.Y)
{
LowestPoint = Corners[i];
}
}
Planes = new Plane <TScalar>[6]
{
Plane <TScalar> .CreateFromVertices(Corners[0], Corners[1], Corners[2]),
Plane <TScalar> .CreateFromVertices(Corners[0], Corners[1], Corners[4]),
Plane <TScalar> .CreateFromVertices(Corners[0], Corners[1], Corners[5]),
Plane <TScalar> .CreateFromVertices(Corners[0], Corners[1], Corners[6]),
Plane <TScalar> .CreateFromVertices(Corners[0], Corners[1], Corners[7]),
Plane <TScalar> .CreateFromVertices(Corners[4], Corners[5], Corners[6]),
};
}