/// <summary>
/// Initializes a new instance of cone with the given parameters.
/// </summary>
/// <param name="origin">The position of the apex of the cone.</param>
/// <param name="orientation">The direction of the cone's axis, and its length.</param>
/// <param name="angle">The angle of the cone.</param>
public Cone(Vector3 <TScalar> origin, Vector3 <TScalar> orientation, TScalar angle)
{
Axis = orientation;
_start = origin;
_end = origin + orientation;
Length = Vector3 <TScalar> .Length(orientation);
var two = NumberValues.Two <TScalar>();
var halfHeight = Length / two;
var normalAxis = Vector3 <TScalar> .Normalize(orientation);
Position = origin + (normalAxis * halfHeight);
Radius = TScalar.Tan(angle / two) * Length;
var r2 = Radius * Radius;
ContainingRadius = TScalar.Sqrt((halfHeight * halfHeight) + r2);
var pr = Vector3 <TScalar> .UnitY - (normalAxis * Vector3 <TScalar> .Dot(Vector3 <TScalar> .UnitY, normalAxis));
var h = Vector3 <TScalar> .Normalize(pr) * Radius;
var endTop = _end + h;
var endBottom = _end - h;
HighestPoint = endTop.Y >= _start.Y ? endTop : _start;
LowestPoint = endBottom.Y < _start.Y ? endBottom : _start;
SmallestDimension = TScalar.Min(Length, Radius * two);
Volume = TScalar.Pi * r2 * (Length / NumberValues.Three <TScalar>());
}
public Cone(Vector3 <TScalar> axis, TScalar radius, Vector3 <TScalar> position)
{
Axis = axis;
Position = position;
Radius = radius;
Length = Vector3 <TScalar> .Length(axis);
var two = NumberValues.Two <TScalar>();
var halfHeight = Length / two;
var r2 = Radius * Radius;
ContainingRadius = TScalar.Sqrt((halfHeight * halfHeight) + r2);
var _halfPath = Vector3 <TScalar> .Normalize(axis) * halfHeight;
_start = Position - _halfPath;
_end = _start + axis;
var pl = Vector3 <TScalar> .Normalize(Axis);
var pr = Vector3 <TScalar> .UnitY - (pl * Vector3 <TScalar> .Dot(Vector3 <TScalar> .UnitY, pl));
var h = Vector3 <TScalar> .Normalize(pr) * Radius;
var endTop = _end + h;
var endBottom = _end - h;
HighestPoint = endTop.Y >= _start.Y ? endTop : _start;
LowestPoint = endBottom.Y < _start.Y ? endBottom : _start;
SmallestDimension = TScalar.Min(Length, Radius * two);
Volume = TScalar.Pi * r2 * (Length / NumberValues.Three <TScalar>());
}
public Cuboid(TScalar axisX, TScalar axisY, TScalar axisZ, Vector3 <TScalar> position, Quaternion <TScalar> rotation)
{
AxisX = axisX;
AxisY = axisY;
AxisZ = axisZ;
Position = position;
Rotation = rotation;
var two = NumberValues.Two <TScalar>();
ContainingRadius = TScalar.Sqrt((AxisX * AxisX) + (AxisY * AxisY) + (AxisZ * AxisZ)) / two;
SmallestDimension = TScalar.Min(AxisX, TScalar.Min(AxisY, AxisZ));
Volume = AxisX * AxisY * AxisZ;
var x = Vector3 <TScalar> .Transform(Vector3 <TScalar> .UnitX *(AxisX / two), Rotation);
var y = Vector3 <TScalar> .Transform(Vector3 <TScalar> .UnitY *(AxisY / two), Rotation);
var z = Vector3 <TScalar> .Transform(Vector3 <TScalar> .UnitZ *(AxisZ / two), Rotation);
Corners = new Vector3 <TScalar>[8]
{
Position + x + y + z,
Position + x - y + z,
Position + x - y - z,
Position + x + y - z,
Position - x + y - z,
Position - x - y - z,
Position - x - y + z,
Position - x + y + z
};
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];
}
}
}
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 Capsule(Vector3 <TScalar> axis, TScalar radius, Vector3 <TScalar> position)
{
Axis = axis;
Position = position;
Radius = radius;
var two = NumberValues.Two <TScalar>();
_pathLength = Vector3 <TScalar> .Length(axis);
var twoR = Radius * two;
Length = _pathLength + twoR;
var halfHeight = _pathLength / two;
ContainingRadius = halfHeight + Radius;
var _halfPath = Vector3 <TScalar> .Normalize(axis) * halfHeight;
_start = Position - _halfPath;
_end = _start + axis;
var y = Vector3 <TScalar> .UnitY * Radius;
if (_end.Y >= _start.Y)
{
HighestPoint = _end + y;
LowestPoint = _start - y;
}
else
{
HighestPoint = _start + y;
LowestPoint = _end - y;
}
SmallestDimension = TScalar.Min(Length, twoR);
Volume = (TScalar.Pi * Radius * Radius * _pathLength)
+ (NumberValues.FourThirdsPi <TScalar>() * Radius.Cube());
}
public Cylinder(Vector3 <TScalar> axis, TScalar radius, Vector3 <TScalar> position)
{
Axis = axis;
Position = position;
Radius = radius;
var two = NumberValues.Two <TScalar>();
var axisLength = Vector3 <TScalar> .Length(axis);
var halfHeight = axisLength / two;
var r2 = Radius * Radius;
ContainingRadius = TScalar.Sqrt((halfHeight * halfHeight) + r2);
var _halfPath = Vector3 <TScalar> .Normalize(axis) * halfHeight;
_start = Position - _halfPath;
_end = _start + axis;
var pl = Vector3 <TScalar> .Normalize(Axis);
var pr = Vector3 <TScalar> .UnitY - (pl * Vector3 <TScalar> .Dot(Vector3 <TScalar> .UnitY, pl));
var h = Vector3 <TScalar> .Normalize(pr) * Radius;
if (_end.Y >= _start.Y)
{
HighestPoint = _end + h;
LowestPoint = _start - h;
}
else
{
HighestPoint = _start + h;
LowestPoint = _end - h;
}
SmallestDimension = TScalar.Min(axisLength, Radius * two);
Volume = TScalar.Pi * r2 * axisLength;
}
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]),
};
}