763 lines
23 KiB
C++
763 lines
23 KiB
C++
// Copyright Epic Games, Inc. All Rights Reserved.
|
|
|
|
#include "BaseGizmos/GizmoMath.h"
|
|
|
|
|
|
|
|
|
|
FVector GizmoMath::ProjectPointOntoLine(
|
|
const FVector& Point,
|
|
const FVector& LineOrigin, const FVector& LineDirection)
|
|
{
|
|
double ProjectionParam = FVector::DotProduct((Point - LineOrigin), LineDirection);
|
|
return LineOrigin + ProjectionParam * LineDirection;
|
|
}
|
|
|
|
|
|
void GizmoMath::NearestPointOnLine(
|
|
const FVector& LineOrigin, const FVector& LineDirection,
|
|
const FVector& QueryPoint,
|
|
FVector& NearestPointOut, float& LineParameterOut)
|
|
{
|
|
check(LineDirection.IsNormalized());
|
|
double LineParameter = FVector::DotProduct( (QueryPoint - LineOrigin), LineDirection);
|
|
NearestPointOut = LineOrigin + LineParameter * LineDirection;
|
|
LineParameterOut = (float)LineParameter;
|
|
}
|
|
|
|
|
|
void GizmoMath::NearestPointOnLineToRay(
|
|
const FVector& LineOrigin, const FVector& LineDirection,
|
|
const FVector& RayOrigin, const FVector& RayDirection,
|
|
FVector& NearestLinePointOut, float& LineParameterOut,
|
|
FVector& NearestRayPointOut, float& RayParameterOut)
|
|
{
|
|
FVector kDiff = LineOrigin - RayOrigin;
|
|
double a01 = -FVector::DotProduct(LineDirection, RayDirection);
|
|
double b0 = FVector::DotProduct(kDiff, LineDirection);
|
|
double c = kDiff.SizeSquared();
|
|
double det = FMath::Abs((double)1 - a01 * a01);
|
|
double b1, s0, s1;
|
|
|
|
if (det >= SMALL_NUMBER)
|
|
{
|
|
b1 = -FVector::DotProduct(kDiff, RayDirection);
|
|
s1 = a01 * b0 - b1;
|
|
|
|
if (s1 >= (double)0)
|
|
{
|
|
// Two interior points are closest, one on Line and one on Ray
|
|
double invDet = ((double)1) / det;
|
|
s0 = (a01 * b1 - b0) * invDet;
|
|
s1 *= invDet;
|
|
}
|
|
else
|
|
{
|
|
// Origin of Ray and interior point of Line are closest.
|
|
s0 = -b0;
|
|
s1 = (double)0;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// Lines are parallel, closest pair with one point at Ray origin.
|
|
s0 = -b0;
|
|
s1 = (double)0;
|
|
}
|
|
|
|
NearestLinePointOut = LineOrigin + s0 * LineDirection;
|
|
NearestRayPointOut = RayOrigin + s1 * RayDirection;
|
|
LineParameterOut = (float)s0;
|
|
RayParameterOut = (float)s1;
|
|
}
|
|
|
|
|
|
|
|
|
|
void GizmoMath::RayPlaneIntersectionPoint(
|
|
const FVector& PlaneOrigin, const FVector& PlaneNormal,
|
|
const FVector& RayOrigin, const FVector& RayDirection,
|
|
bool& bIntersectsOut, FVector& PlaneIntersectionPointOut)
|
|
{
|
|
bIntersectsOut = false;
|
|
PlaneIntersectionPointOut = PlaneOrigin;
|
|
|
|
double PlaneEquationD = -FVector::DotProduct(PlaneOrigin, PlaneNormal);
|
|
double NormalDot = FVector::DotProduct(RayDirection, PlaneNormal);
|
|
|
|
if (FMath::Abs(NormalDot) < SMALL_NUMBER)
|
|
{
|
|
return;
|
|
}
|
|
|
|
double RayParam = -( FVector::DotProduct(RayOrigin, PlaneNormal) + PlaneEquationD) / NormalDot;
|
|
if (RayParam < 0)
|
|
{
|
|
return;
|
|
}
|
|
|
|
PlaneIntersectionPointOut = RayOrigin + RayParam * RayDirection;
|
|
bIntersectsOut = true;
|
|
}
|
|
|
|
|
|
void GizmoMath::RaySphereIntersection(
|
|
const FVector& SphereOrigin, const float SphereRadius,
|
|
const FVector& RayOrigin, const FVector& RayDirection,
|
|
bool& bIntersectsOut, FVector& SphereIntersectionPointOut)
|
|
{
|
|
bIntersectsOut = false;
|
|
SphereIntersectionPointOut = RayOrigin;
|
|
|
|
FVector DeltaPos = RayOrigin - SphereOrigin;
|
|
double a0 = DeltaPos.SizeSquared() - (double)SphereRadius*(double)SphereRadius;
|
|
double a1 = FVector::DotProduct(RayDirection, DeltaPos);
|
|
double discr = a1 * a1 - a0;
|
|
if (discr > 0) // intersection only when roots are real
|
|
{
|
|
bIntersectsOut = true;
|
|
double root = FMath::Sqrt(discr);
|
|
double NearRayParam = -a1 + root; // isn't it always this one?
|
|
double NearRayParam2 = -a1 - root;
|
|
double UseRayParam = FMath::Min(NearRayParam, NearRayParam2);
|
|
SphereIntersectionPointOut = RayOrigin + UseRayParam * RayDirection;
|
|
}
|
|
}
|
|
|
|
|
|
template<typename RealType>
|
|
void GizmoMath::RayCylinderIntersection(
|
|
const FVector& CylinderCenter, const FVector& CylinderAxis, RealType CylinderRadius, RealType CylinderHeight,
|
|
const FVector& RayOrigin, const FVector& RayDirection,
|
|
bool& bIntersectsOut, RealType& OutRayParam)
|
|
{
|
|
// adapted from GeometricTools GTEngine
|
|
// https://www.geometrictools.com/GTE/Mathematics/IntrRay3Cylinder3.h and
|
|
// https://www.geometrictools.com/GTE/Mathematics/IntrLine3Cylinder3.h
|
|
// (Engine/Plugins/Runtime/GeometryProcessing/Source/GeometryAlgorithms/Private/ThirdParty/GTEngine/Mathematics/GteIntrRay3Cylinder3.h and
|
|
// Engine/Plugins/Runtime/GeometryProcessing/Source/GeometryAlgorithms/Private/ThirdParty/GTEngine/Mathematics/GteIntrLine3Cylinder3.h)
|
|
|
|
// The cylinder axis is a line. The origin of the cylinder is chosen to be
|
|
// the line origin. The cylinder wall is at a distance R units from the axis.
|
|
// An infinite cylinder has infinite height. A finite cylinder has center C
|
|
// at the line origin and has a finite height H. The segment for the finite
|
|
// cylinder has endpoints C-(H/2)*D and C+(H/2)*D where D is a unit-length
|
|
// direction of the line.
|
|
|
|
// Initialize the result as if there is no intersection. If we discover
|
|
// an intersection, these values will be modified accordingly.
|
|
bIntersectsOut = false;
|
|
int NumIntersections = 0;
|
|
RealType RayParam[2];
|
|
|
|
// Create a coordinate system for the cylinder. In this system, the
|
|
// cylinder segment center C is the origin and the cylinder axis direction
|
|
// W is the z-axis. U and V are the other coordinate axis directions.
|
|
// If P = x*U+y*V+z*W, the cylinder is x^2 + y^2 = r^2, where r is the
|
|
// cylinder radius. The end caps are |z| = h/2, where h is the cylinder
|
|
// height.
|
|
FVector Basis[3]; // {W, U, V}
|
|
Basis[0] = CylinderAxis.GetSafeNormal();
|
|
// @todo: replace with MakePerpVectors() once it has been moved out of plugins
|
|
Basis[1] = GetOrthogonalVector(Basis[0]).GetSafeNormal();
|
|
Basis[2] = (Basis[0] ^ Basis[1]).GetSafeNormal();
|
|
|
|
RealType HalfHeight = RealType(0.5) * CylinderHeight;
|
|
RealType RadiusSquared = CylinderRadius * CylinderRadius;
|
|
|
|
// Convert incoming line origin to capsule coordinates.
|
|
FVector Diff = RayOrigin - CylinderCenter;
|
|
FVector P(Basis[1] | Diff, Basis[2] | Diff, Basis[0] | Diff);
|
|
|
|
// Get the z-value, in cylinder coordinates, of the incoming line's
|
|
// unit-length direction.
|
|
RealType Dz = static_cast<RealType>( Basis[0] | RayDirection );
|
|
if (FMath::IsNearlyEqual(Dz, 1.0, SMALL_NUMBER))
|
|
{
|
|
// The line is parallel to the cylinder axis. Determine whether the
|
|
// line intersects the cylinder end disks.
|
|
RealType RadialSquaredDist = static_cast<RealType>( RadiusSquared - P[0] * P[0] - P[1] * P[1]);
|
|
if (RadialSquaredDist >= 0.0)
|
|
{
|
|
// The line intersects the cylinder end disks.
|
|
NumIntersections = 2;
|
|
if (Dz > 0.0)
|
|
{
|
|
RayParam[0] = static_cast<RealType>( -P[2] - HalfHeight );
|
|
RayParam[1] = static_cast<RealType>( -P[2] + HalfHeight );
|
|
}
|
|
else
|
|
{
|
|
RayParam[0] = static_cast<RealType>( P[2] - HalfHeight );
|
|
RayParam[1] = static_cast<RealType>( P[2] + HalfHeight );
|
|
}
|
|
}
|
|
// else: The line is outside the cylinder, no intersection.
|
|
}
|
|
else
|
|
{
|
|
// Convert the incoming line unit-length direction to cylinder
|
|
// coordinates.
|
|
FVector D((Basis[1] | RayDirection), (Basis[2] | RayDirection), Dz);
|
|
|
|
RealType A0, A1, A2, Discr, Root, Inv, TValue;
|
|
|
|
if (FMath::IsNearlyZero(D[2]))
|
|
{
|
|
// The line is perpendicular to the cylinder axis.
|
|
if (FMath::Abs(P[2]) <= HalfHeight)
|
|
{
|
|
// Test intersection of line P+t*D with infinite cylinder
|
|
// x^2+y^2 = r^2. This reduces to computing the Roots of a
|
|
// quadratic equation. If P = (px,py,pz) and D = (dx,dy,dz),
|
|
// then the quadratic equation is
|
|
// (dx^2+dy^2)*t^2 + 2*(px*dx+py*dy)*t + (px^2+py^2-r^2) = 0
|
|
A0 = static_cast<RealType>( P[0] * P[0] + P[1] * P[1] - RadiusSquared );
|
|
A1 = static_cast<RealType>( P[0] * D[0] + P[1] * D[1] );
|
|
A2 = static_cast<RealType>( D[0] * D[0] + D[1] * D[1] );
|
|
Discr = A1 * A1 - A0 * A2;
|
|
if (FMath::IsNearlyZero(Discr))
|
|
{
|
|
// The line is tangent to the cylinder.
|
|
NumIntersections = 1;
|
|
RayParam[0] = -A1 / A2;
|
|
RayParam[1] = RayParam[0];
|
|
}
|
|
else if (Discr > 0.0)
|
|
{
|
|
// The line intersects the cylinder in two places.
|
|
NumIntersections = 2;
|
|
Root = FMath::Sqrt(Discr);
|
|
Inv = 1.0f / A2;
|
|
RayParam[0] = (-A1 - Root) * Inv;
|
|
RayParam[1] = (-A1 + Root) * Inv;
|
|
}
|
|
// else: The line does not intersect the cylinder.
|
|
}
|
|
// else: The line is outside the planes of the cylinder end disks.
|
|
}
|
|
else
|
|
{
|
|
// Test for intersections with the planes of the end disks.
|
|
Inv = static_cast<RealType>( 1.0 / D[2] );
|
|
|
|
RealType T0 = static_cast<RealType>( (-HalfHeight - P[2]) * Inv );
|
|
RealType TmpX = static_cast<RealType>( P[0] + T0 * D[0] );
|
|
RealType TmpY = static_cast<RealType>( P[1] + T0 * D[1] );
|
|
if (TmpX * TmpX + TmpY * TmpY <= RadiusSquared)
|
|
{
|
|
// Plane intersection inside the top cylinder end disk.
|
|
RayParam[NumIntersections++] = T0;
|
|
}
|
|
|
|
RealType T1 = static_cast<RealType>( (+HalfHeight - P[2]) * Inv );
|
|
TmpX = static_cast<RealType>( P[0] + T1 * D[0] );
|
|
TmpY = static_cast<RealType>( P[1] + T1 * D[1] );
|
|
if (TmpX * TmpX + TmpY * TmpY <= RadiusSquared)
|
|
{
|
|
// Plane intersection inside the bottom cylinder end disk.
|
|
RayParam[NumIntersections++] = T1;
|
|
}
|
|
|
|
if (NumIntersections < 2)
|
|
{
|
|
// Test for intersection with the cylinder wall.
|
|
A0 = static_cast<RealType>( P[0] * P[0] + P[1] * P[1] - RadiusSquared );
|
|
A1 = static_cast<RealType>( P[0] * D[0] + P[1] * D[1] );
|
|
A2 = static_cast<RealType>( D[0] * D[0] + D[1] * D[1] );
|
|
Discr = A1 * A1 - A0 * A2;
|
|
if (FMath::IsNearlyZero(Discr))
|
|
{
|
|
TValue = -A1 / A2;
|
|
if (T0 <= T1)
|
|
{
|
|
if (T0 <= TValue && TValue <= T1)
|
|
{
|
|
RayParam[NumIntersections++] = TValue;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (T1 <= TValue && TValue <= T0)
|
|
{
|
|
RayParam[NumIntersections++] = TValue;
|
|
}
|
|
}
|
|
}
|
|
else if (Discr > 0.0)
|
|
{
|
|
Root = FMath::Sqrt(Discr);
|
|
Inv = (1.0f) / A2;
|
|
TValue = (-A1 - Root) * Inv;
|
|
if (T0 <= T1)
|
|
{
|
|
if (T0 <= TValue && TValue <= T1)
|
|
{
|
|
RayParam[NumIntersections++] = TValue;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (T1 <= TValue && TValue <= T0)
|
|
{
|
|
RayParam[NumIntersections++] = TValue;
|
|
}
|
|
}
|
|
|
|
if (NumIntersections < 2)
|
|
{
|
|
TValue = (-A1 + Root) * Inv;
|
|
if (T0 <= T1)
|
|
{
|
|
if (T0 <= TValue && TValue <= T1)
|
|
{
|
|
RayParam[NumIntersections++] = TValue;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (T1 <= TValue && TValue <= T0)
|
|
{
|
|
RayParam[NumIntersections++] = TValue;
|
|
}
|
|
}
|
|
}
|
|
// else: Line intersects end disk and cylinder wall.
|
|
}
|
|
// else: Line does not intersect cylinder wall.
|
|
}
|
|
// else: Line intersects both top and bottom cylinder end disks.
|
|
|
|
if (NumIntersections == 2)
|
|
{
|
|
if (RayParam[0] > RayParam[1])
|
|
{
|
|
RealType TmpT = RayParam[0];
|
|
RayParam[0] = RayParam[1];
|
|
RayParam[1] = TmpT;
|
|
}
|
|
}
|
|
else if (NumIntersections == 1)
|
|
{
|
|
RayParam[1] = RayParam[0];
|
|
}
|
|
}
|
|
}
|
|
|
|
// Get rid of hits before ray origin
|
|
if (NumIntersections > 0 && RayParam[0] >= RealType(0))
|
|
{
|
|
bIntersectsOut = true;
|
|
OutRayParam = RayParam[0];
|
|
}
|
|
else if (NumIntersections == 2 && RayParam[1] >= RealType(0))
|
|
{
|
|
bIntersectsOut = true;
|
|
OutRayParam = RayParam[1];
|
|
}
|
|
else
|
|
{
|
|
bIntersectsOut = false;
|
|
}
|
|
}
|
|
|
|
template<typename RealType>
|
|
void GizmoMath::RayConeIntersection(
|
|
const FVector& ConeCenter, const FVector& ConeDirection, RealType ConeCosAngle, RealType ConeHeight,
|
|
const FVector& RayOrigin, const FVector& RayDirection,
|
|
bool& bIntersectsOut, RealType& OutRayParam)
|
|
{
|
|
// adapted from GeometricTools GTEngine
|
|
// https://www.geometrictools.com/GTE/Mathematics/IntrRay3Cone3.h and
|
|
// https://www.geometrictools.com/GTE/Mathematics/IntrLine3Cone3.h
|
|
// (Engine/Plugins/Runtime/GeometryProcessing/Source/GeometryAlgorithms/Private/ThirdParty/GTEngine/Mathematics/GteIntrRay3Cone3.h and
|
|
// Engine/Plugins/Runtime/GeometryProcessing/Source/GeometryAlgorithms/Private/ThirdParty/GTEngine/Mathematics/GteIntrLine3Cone3.h)
|
|
|
|
// The cone has vertex V, unit-length axis direction D, angle theta in
|
|
// (0,pi/2), and height h in (0,+infinity). The line is P + t*U, where U
|
|
// is a unit-length direction vector. Define g = cos(theta). The cone
|
|
// is represented by
|
|
// (X-V)^T * (D*D^T - g^2*I) * (X-V) = 0, 0 <= Dot(D,X-V) <= h
|
|
// The first equation defines a double-sided cone. The first inequality
|
|
// in the second equation limits this to a single-sided cone containing
|
|
// the ray V + s*D with s >= 0. We will call this the 'positive cone'.
|
|
// The single-sided cone containing ray V + s * t with s <= 0 is called
|
|
// the 'negative cone'. The double-sided cone is the union of the
|
|
// positive cone and negative cone. The second inequality in the second
|
|
// equation limits the single-sided cone to the region bounded by the
|
|
// height. Setting X(t) = P + t*U, the equations are
|
|
// C2*t^2 + 2*C1*t + C0 = 0, 0 <= Dot(D,U)*t + Dot(D,P-V) <= h
|
|
// where
|
|
// C2 = Dot(D,U)^2 - g^2
|
|
// C1 = Dot(D,U)*Dot(D,P-V) - g^2*Dot(U,P-V)
|
|
// C0 = Dot(D,P-V)^2 - g^2*Dot(P-V,P-V)
|
|
// The following code computes the t-interval that satisfies the quadratic
|
|
// equation subject to the linear inequality constraints.
|
|
|
|
FVector PmV = RayOrigin - ConeCenter;
|
|
RealType DdU = static_cast<RealType>( FVector::DotProduct(ConeDirection, RayDirection) );
|
|
RealType DdPmV = static_cast<RealType>( FVector::DotProduct(ConeDirection, PmV) );
|
|
RealType UdPmV = static_cast<RealType>( FVector::DotProduct(RayDirection, PmV) );
|
|
RealType PmVdPmV = static_cast<RealType>( FVector::DotProduct(PmV, PmV) );
|
|
RealType CosAngleSqr = ConeCosAngle * ConeCosAngle;
|
|
RealType C2 = DdU * DdU - CosAngleSqr;
|
|
RealType C1 = DdU * DdPmV - CosAngleSqr * UdPmV;
|
|
RealType C0 = DdPmV * DdPmV - CosAngleSqr * PmVdPmV;
|
|
RealType T;
|
|
RealType RayParam[2];
|
|
|
|
if (!FMath::IsNearlyZero(C2))
|
|
{
|
|
RealType Discr = C1 * C1 - C0 * C2;
|
|
if (FMath::IsNearlyZero(Discr))
|
|
{
|
|
// One repeated real Root; the line is tangent to the double-sided
|
|
// cone at a single point. Report only the point if it is on the
|
|
// positive cone.
|
|
T = -C1 / C2;
|
|
if (DdU * T + DdPmV >= 0.0)
|
|
{
|
|
RayParam[0] = T;
|
|
RayParam[1] = T;
|
|
}
|
|
else
|
|
{
|
|
bIntersectsOut = false;
|
|
return;
|
|
}
|
|
}
|
|
else if (Discr < 0.0)
|
|
{
|
|
// The quadratic has no real-valued Roots. The line does not
|
|
// intersect the double-sided cone.
|
|
bIntersectsOut = false;
|
|
return;
|
|
}
|
|
else // (Discr > 0.0)
|
|
{
|
|
// The quadratic has two distinct real-valued Roots. However, one
|
|
// or both of them might intersect the negative cone. We are
|
|
// interested only in those intersections with the positive cone.
|
|
RealType Root = FMath::Sqrt(Discr);
|
|
RealType InvC2 = RealType(1) / C2;
|
|
int NumIntersections = 0;
|
|
|
|
T = (-C1 - Root) * InvC2;
|
|
if (DdU * T + DdPmV >= 0.0)
|
|
{
|
|
RayParam[NumIntersections++] = T;
|
|
}
|
|
|
|
T = (-C1 + Root) * InvC2;
|
|
if (DdU * T + DdPmV >= 0.0)
|
|
{
|
|
RayParam[NumIntersections++] = T;
|
|
}
|
|
|
|
if (NumIntersections == 2)
|
|
{
|
|
// The line intersects the positive cone in two distinct
|
|
// points.
|
|
if (RayParam[0] > RayParam[1])
|
|
{
|
|
RealType TmpT = RayParam[0];
|
|
RayParam[0] = RayParam[1];
|
|
RayParam[1] = TmpT;
|
|
}
|
|
}
|
|
else if (NumIntersections == 1)
|
|
{
|
|
// The line intersects the positive cone in a single point and
|
|
// the negative cone in a single point. We report only the
|
|
// intersection with the positive cone.
|
|
if (DdU > 0.0)
|
|
{
|
|
RayParam[1] = TNumericLimits<RealType>::Max();
|
|
}
|
|
else
|
|
{
|
|
bIntersectsOut = false;
|
|
return;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
bIntersectsOut = false;
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
bIntersectsOut = false;
|
|
return;
|
|
}
|
|
|
|
if (!FMath::IsNearlyZero(DdU))
|
|
{
|
|
// Clamp the intersection to the height of the cone.
|
|
RealType InvDdU = (1.0f) / DdU;
|
|
RealType hInterval[2];
|
|
if (DdU > 0.0)
|
|
{
|
|
hInterval[0] = -DdPmV * InvDdU;
|
|
hInterval[1] = (ConeHeight - DdPmV) * InvDdU;
|
|
}
|
|
else // (DdU < 0.0)
|
|
{
|
|
hInterval[0] = (ConeHeight - DdPmV) * InvDdU;
|
|
hInterval[1] = -DdPmV * InvDdU;
|
|
}
|
|
|
|
RealType Result0, Result1;
|
|
int NumIntersections;
|
|
GizmoMath::IntervalIntervalIntersection<RealType>(RayParam, hInterval, NumIntersections, Result0, Result1);
|
|
|
|
if (NumIntersections > 0)
|
|
{
|
|
RayParam[0] = Result0;
|
|
RayParam[1] = NumIntersections == 2 ? Result1 : Result0;
|
|
}
|
|
else
|
|
{
|
|
bIntersectsOut = false;
|
|
return;
|
|
}
|
|
}
|
|
else if (DdPmV > ConeHeight)
|
|
{
|
|
bIntersectsOut = false;
|
|
return;
|
|
}
|
|
|
|
// Get rid of hits before ray origin
|
|
if (RayParam[0] >= RealType(0))
|
|
{
|
|
bIntersectsOut = true;
|
|
OutRayParam = RayParam[0];
|
|
}
|
|
else if (RayParam[1] >= RealType(0))
|
|
{
|
|
bIntersectsOut = true;
|
|
OutRayParam = RayParam[1];
|
|
}
|
|
else
|
|
{
|
|
bIntersectsOut = false;
|
|
}
|
|
|
|
}
|
|
|
|
template <typename RealType>
|
|
void GizmoMath::IntervalIntervalIntersection(
|
|
const RealType Interval0[2], const RealType Interval1[2],
|
|
int& OutNumIntersections, RealType& OutResult0, RealType& OutResult1)
|
|
{
|
|
// adapted from GeometricTools GTEngine
|
|
// https://www.geometrictools.com/GTE/Mathematics/IntrIntervals.h
|
|
// (Engine/Plugins/Runtime/GeometryProcessing/Source/GeometryAlgorithms/Private/ThirdParty/GTEngine/Mathematics/GteIntrIntervals.h)
|
|
// Determines the intersection between two floating point intervals in which
|
|
// the input intervals' values are sorted in increasing order.
|
|
// Used by RayConeIntersection.
|
|
|
|
if (Interval0[1] < Interval1[0] || Interval0[0] > Interval1[1])
|
|
{
|
|
OutNumIntersections = 0;
|
|
OutResult0 = TNumericLimits<RealType>::Max();
|
|
OutResult1 = -TNumericLimits<RealType>::Max();
|
|
}
|
|
else if (Interval0[1] > Interval1[0])
|
|
{
|
|
if (Interval0[0] < Interval1[1])
|
|
{
|
|
OutNumIntersections = 2;
|
|
OutResult0 =
|
|
(Interval0[0] < Interval1[0] ? Interval1[0] : Interval0[0]);
|
|
OutResult1 =
|
|
(Interval0[1] > Interval1[1] ? Interval1[1] : Interval0[1]);
|
|
if (OutResult0 == OutResult1)
|
|
{
|
|
OutNumIntersections = 1;
|
|
}
|
|
}
|
|
else // Interval0[0] == Interval1[1]
|
|
{
|
|
OutNumIntersections = 1;
|
|
OutResult0 = Interval0[0];
|
|
OutResult1 = OutResult0;
|
|
}
|
|
}
|
|
else // Interval0[1] == Interval1[0]
|
|
{
|
|
OutNumIntersections = 1;
|
|
OutResult0 = Interval0[1];
|
|
OutResult1 = OutResult0;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|
|
void GizmoMath::ClosetPointOnCircle(
|
|
const FVector& QueryPoint,
|
|
const FVector& CircleOrigin, const FVector& CircleNormal, float CircleRadius,
|
|
FVector& ClosestPointOut)
|
|
{
|
|
FVector PointDelta = QueryPoint - CircleOrigin;
|
|
FVector DeltaInPlane = PointDelta - FVector::DotProduct(CircleNormal,PointDelta)*CircleNormal;
|
|
double OriginDist = DeltaInPlane.Size();
|
|
if (OriginDist > 0.0f)
|
|
{
|
|
ClosestPointOut = CircleOrigin + ((double)CircleRadius / OriginDist) * DeltaInPlane;
|
|
}
|
|
else // all points equidistant, use any one
|
|
{
|
|
FVector PlaneX, PlaneY;
|
|
MakeNormalPlaneBasis(CircleNormal, PlaneX, PlaneY);
|
|
ClosestPointOut = CircleOrigin + (double)CircleRadius * PlaneX;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
void GizmoMath::MakeNormalPlaneBasis(
|
|
const FVector& PlaneNormal,
|
|
FVector& BasisAxis1Out, FVector& BasisAxis2Out)
|
|
{
|
|
// Duff et al method, from https://graphics.pixar.com/library/OrthonormalB/paper.pdf
|
|
if (PlaneNormal.Z < 0)
|
|
{
|
|
double A = 1.0f / (1.0f - PlaneNormal.Z);
|
|
double B = PlaneNormal.X * PlaneNormal.Y * A;
|
|
BasisAxis1Out.X = 1.0f - PlaneNormal.X * PlaneNormal.X * A;
|
|
BasisAxis1Out.Y = -B;
|
|
BasisAxis1Out.Z = PlaneNormal.X;
|
|
BasisAxis2Out.X = B;
|
|
BasisAxis2Out.Y = PlaneNormal.Y * PlaneNormal.Y * A - 1.0f;
|
|
BasisAxis2Out.Z = -PlaneNormal.Y;
|
|
}
|
|
else
|
|
{
|
|
double A = 1.0f / (1.0f + PlaneNormal.Z);
|
|
double B = -PlaneNormal.X * PlaneNormal.Y * A;
|
|
BasisAxis1Out.X = 1.0f - PlaneNormal.X * PlaneNormal.X * A;
|
|
BasisAxis1Out.Y = B;
|
|
BasisAxis1Out.Z = -PlaneNormal.X;
|
|
BasisAxis2Out.X = B;
|
|
BasisAxis2Out.Y = 1.0f - PlaneNormal.Y * PlaneNormal.Y * A;
|
|
BasisAxis2Out.Z = -PlaneNormal.Y;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
float GizmoMath::ComputeAngleInPlane(
|
|
const FVector& Point,
|
|
const FVector& PlaneOrigin, const FVector& PlaneNormal,
|
|
const FVector& PlaneAxis1, const FVector& PlaneAxis2)
|
|
{
|
|
// project point into plane
|
|
FVector LocalPoint = Point - PlaneOrigin;
|
|
|
|
double X = FVector::DotProduct(LocalPoint, PlaneAxis1);
|
|
double Y = FVector::DotProduct(LocalPoint, PlaneAxis2);
|
|
|
|
float SignedAngle = (float)atan2(Y, X);
|
|
return SignedAngle;
|
|
}
|
|
|
|
|
|
|
|
|
|
FVector2D GizmoMath::ComputeCoordinatesInPlane(
|
|
const FVector& Point,
|
|
const FVector& PlaneOrigin, const FVector& PlaneNormal,
|
|
const FVector& PlaneAxis1, const FVector& PlaneAxis2)
|
|
{
|
|
FVector LocalPoint = Point - PlaneOrigin;
|
|
double X = FVector::DotProduct(LocalPoint, PlaneAxis1);
|
|
double Y = FVector::DotProduct(LocalPoint, PlaneAxis2);
|
|
return FVector2D(X, Y);
|
|
}
|
|
|
|
|
|
FVector GizmoMath::ProjectPointOntoPlane(
|
|
const FVector& Point,
|
|
const FVector& PlaneOrigin, const FVector& PlaneNormal)
|
|
{
|
|
FVector LocalPoint = Point - PlaneOrigin;
|
|
double NormalDot = FVector::DotProduct(LocalPoint, PlaneNormal);
|
|
return Point - NormalDot * PlaneNormal;
|
|
}
|
|
|
|
|
|
template <typename RealType>
|
|
RealType GizmoMath::SnapToIncrement(RealType Value, RealType Increment)
|
|
{
|
|
if (!FMath::IsFinite(Value))
|
|
{
|
|
return 0;
|
|
}
|
|
RealType Sign = FMath::Sign(Value);
|
|
Value = FMath::Abs(Value);
|
|
int IntIncrement = (int)(Value / Increment);
|
|
RealType Remainder = (RealType)fmod(Value, Increment);
|
|
if (Remainder > Increment / 2)
|
|
{
|
|
++IntIncrement;
|
|
}
|
|
return Sign * (RealType)IntIncrement * Increment;
|
|
}
|
|
|
|
// @todo: Remove this and replace calls to it with MakePerpVectors() once
|
|
// Engine\Plugins\Experimental\GeometryProcessing\Source\GeometricObjects\Public\VectorUtil.h has been moved out of Plugins dir.
|
|
FVector GizmoMath::GetOrthogonalVector(const FVector& V)
|
|
{
|
|
FVector AbsVector(FMath::Abs(V.X), FMath::Abs(V.Y), FMath::Abs(V.Z));
|
|
if ((AbsVector.X <= AbsVector.Y) && (AbsVector.X <= AbsVector.Z))
|
|
{
|
|
// X is the smallest component
|
|
return FVector(0, V.Z, -V.Y);
|
|
}
|
|
if ((AbsVector.Z <= AbsVector.X) && (AbsVector.Z <= AbsVector.Y))
|
|
{
|
|
// Z is the smallest component
|
|
return FVector(V.Y, -V.X, 0);
|
|
}
|
|
// Y is the smallest component
|
|
return FVector(-V.Z, 0, V.X);
|
|
}
|
|
|
|
namespace GizmoMath
|
|
{
|
|
template
|
|
void INTERACTIVETOOLSFRAMEWORK_API GizmoMath::RayCylinderIntersection<float>(
|
|
const FVector& CylinderCenter, const FVector& CylinderAxis, float CylinderRadius, float CylinderHeight,
|
|
const FVector& RayOrigin, const FVector& RayDirection,
|
|
bool& bIntersectsOut, float& OutRayParam);
|
|
|
|
template
|
|
void INTERACTIVETOOLSFRAMEWORK_API GizmoMath::RayCylinderIntersection<double>(
|
|
const FVector& CylinderCenter, const FVector& CylinderAxis, double CylinderRadius, double CylinderHeight,
|
|
const FVector& RayOrigin, const FVector& RayDirection,
|
|
bool& bIntersectsOut, double& OutRayParam);
|
|
|
|
template
|
|
void INTERACTIVETOOLSFRAMEWORK_API GizmoMath::RayConeIntersection<float>(
|
|
const FVector& ConeCenter, const FVector& ConeDirection, float ConeAngle, float ConeHeight,
|
|
const FVector& RayOrigin, const FVector& RayDirection,
|
|
bool& bIntersectsOut, float& OutHitDepth);
|
|
|
|
template
|
|
void INTERACTIVETOOLSFRAMEWORK_API GizmoMath::RayConeIntersection<double>(
|
|
const FVector& ConeCenter, const FVector& ConeDirection, double ConeAngle, double ConeHeight,
|
|
const FVector& RayOrigin, const FVector& RayDirection,
|
|
bool& bIntersectsOut, double& OutHitDepth);
|
|
|
|
template
|
|
double INTERACTIVETOOLSFRAMEWORK_API GizmoMath::SnapToIncrement(double Value, double Increment);
|
|
|
|
template
|
|
float INTERACTIVETOOLSFRAMEWORK_API GizmoMath::SnapToIncrement(float Value, float Increment);
|
|
} // end namespace GizmoMath
|