bool FConvexVolume::IntersectSphere(const FVector& Origin,const float& Radius, bool& bOutFullyContained) const
{
bool Result = true;
//Assume fully contained
bOutFullyContained = true;
checkSlow(PermutedPlanes.Num() % 4 == 0);
// Load the origin & radius
VectorRegister Orig = VectorLoadFloat3(&Origin);
VectorRegister VRadius = VectorLoadFloat1(&Radius);
VectorRegister NegativeVRadius = VectorNegate(VRadius);
// Splat origin into 3 vectors
VectorRegister OrigX = VectorReplicate(Orig, 0);
VectorRegister OrigY = VectorReplicate(Orig, 1);
VectorRegister OrigZ = VectorReplicate(Orig, 2);
// Since we are moving straight through get a pointer to the data
const FPlane* RESTRICT PermutedPlanePtr = (FPlane*)PermutedPlanes.GetData();
// Process four planes at a time until we have < 4 left
for (int32 Count = 0; Count < PermutedPlanes.Num(); Count += 4)
{
// Load 4 planes that are already all Xs, Ys, ...
VectorRegister PlanesX = VectorLoadAligned(PermutedPlanePtr);
PermutedPlanePtr++;
VectorRegister PlanesY = VectorLoadAligned(PermutedPlanePtr);
PermutedPlanePtr++;
VectorRegister PlanesZ = VectorLoadAligned(PermutedPlanePtr);
PermutedPlanePtr++;
VectorRegister PlanesW = VectorLoadAligned(PermutedPlanePtr);
PermutedPlanePtr++;
// Calculate the distance (x * x) + (y * y) + (z * z) - w
VectorRegister DistX = VectorMultiply(OrigX,PlanesX);
VectorRegister DistY = VectorMultiplyAdd(OrigY,PlanesY,DistX);
VectorRegister DistZ = VectorMultiplyAdd(OrigZ,PlanesZ,DistY);
VectorRegister Distance = VectorSubtract(DistZ,PlanesW);
// Check for completely outside
int32 Mask = VectorAnyGreaterThan(Distance,VRadius);
if (Mask)
{
Result = false;
bOutFullyContained = false;
break;
}
//the sphere is definitely inside the frustums, but let's check if it's FULLY contained by checking the NEGATIVE radius (on the inside of each frustum plane)
Mask = VectorAnyGreaterThan(Distance,NegativeVRadius);
if (Mask)
{
bOutFullyContained = false;
}
}
return Result;
}
static FORCEINLINE bool IntersectBoxWithPermutedPlanes(
const FConvexVolume::FPermutedPlaneArray& PermutedPlanes,
const VectorRegister BoxOrigin,
const VectorRegister BoxExtent )
{
bool Result = true;
checkSlow(PermutedPlanes.Num() % 4 == 0);
// Splat origin into 3 vectors
VectorRegister OrigX = VectorReplicate(BoxOrigin, 0);
VectorRegister OrigY = VectorReplicate(BoxOrigin, 1);
VectorRegister OrigZ = VectorReplicate(BoxOrigin, 2);
// Splat extent into 3 vectors
VectorRegister ExtentX = VectorReplicate(BoxExtent, 0);
VectorRegister ExtentY = VectorReplicate(BoxExtent, 1);
VectorRegister ExtentZ = VectorReplicate(BoxExtent, 2);
// Splat the abs for the pushout calculation
VectorRegister AbsExt = VectorAbs(BoxExtent);
VectorRegister AbsExtentX = VectorReplicate(AbsExt, 0);
VectorRegister AbsExtentY = VectorReplicate(AbsExt, 1);
VectorRegister AbsExtentZ = VectorReplicate(AbsExt, 2);
// Since we are moving straight through get a pointer to the data
const FPlane* RESTRICT PermutedPlanePtr = (FPlane*)PermutedPlanes.GetData();
// Process four planes at a time until we have < 4 left
for ( int32 Count = 0, Num = PermutedPlanes.Num(); Count < Num; Count += 4 )
{
// Load 4 planes that are already all Xs, Ys, ...
VectorRegister PlanesX = VectorLoadAligned(PermutedPlanePtr);
PermutedPlanePtr++;
VectorRegister PlanesY = VectorLoadAligned(PermutedPlanePtr);
PermutedPlanePtr++;
VectorRegister PlanesZ = VectorLoadAligned(PermutedPlanePtr);
PermutedPlanePtr++;
VectorRegister PlanesW = VectorLoadAligned(PermutedPlanePtr);
PermutedPlanePtr++;
// Calculate the distance (x * x) + (y * y) + (z * z) - w
VectorRegister DistX = VectorMultiply(OrigX,PlanesX);
VectorRegister DistY = VectorMultiplyAdd(OrigY,PlanesY,DistX);
VectorRegister DistZ = VectorMultiplyAdd(OrigZ,PlanesZ,DistY);
VectorRegister Distance = VectorSubtract(DistZ,PlanesW);
// Now do the push out FMath::Abs(x * x) + FMath::Abs(y * y) + FMath::Abs(z * z)
VectorRegister PushX = VectorMultiply(AbsExtentX,VectorAbs(PlanesX));
VectorRegister PushY = VectorMultiplyAdd(AbsExtentY,VectorAbs(PlanesY),PushX);
VectorRegister PushOut = VectorMultiplyAdd(AbsExtentZ,VectorAbs(PlanesZ),PushY);
// Check for completely outside
if ( VectorAnyGreaterThan(Distance,PushOut) )
{
Result = false;
break;
}
}
return Result;
}
bool FConvexVolume::IntersectSphere(const FVector& Origin,const float& Radius) const
{
bool Result = true;
checkSlow(PermutedPlanes.Num() % 4 == 0);
// Load the origin & radius
VectorRegister Orig = VectorLoadFloat3(&Origin);
VectorRegister VRadius = VectorLoadFloat1(&Radius);
// Splat origin into 3 vectors
VectorRegister OrigX = VectorReplicate(Orig, 0);
VectorRegister OrigY = VectorReplicate(Orig, 1);
VectorRegister OrigZ = VectorReplicate(Orig, 2);
// Since we are moving straight through get a pointer to the data
const FPlane* RESTRICT PermutedPlanePtr = (FPlane*)PermutedPlanes.GetData();
// Process four planes at a time until we have < 4 left
for (int32 Count = 0; Count < PermutedPlanes.Num(); Count += 4)
{
// Load 4 planes that are already all Xs, Ys, ...
VectorRegister PlanesX = VectorLoadAligned(PermutedPlanePtr);
PermutedPlanePtr++;
VectorRegister PlanesY = VectorLoadAligned(PermutedPlanePtr);
PermutedPlanePtr++;
VectorRegister PlanesZ = VectorLoadAligned(PermutedPlanePtr);
PermutedPlanePtr++;
VectorRegister PlanesW = VectorLoadAligned(PermutedPlanePtr);
PermutedPlanePtr++;
// Calculate the distance (x * x) + (y * y) + (z * z) - w
VectorRegister DistX = VectorMultiply(OrigX,PlanesX);
VectorRegister DistY = VectorMultiplyAdd(OrigY,PlanesY,DistX);
VectorRegister DistZ = VectorMultiplyAdd(OrigZ,PlanesZ,DistY);
VectorRegister Distance = VectorSubtract(DistZ,PlanesW);
// Check for completely outside
if (VectorAnyGreaterThan(Distance,VRadius))
{
Result = false;
break;
}
}
return Result;
}
FBox FBox::TransformBy(const FMatrix& M) const
{
// if we are not valid, return another invalid box.
if (!IsValid)
{
return FBox(0);
}
VectorRegister Vertices[8];
VectorRegister m0 = VectorLoadAligned(M.M[0]);
VectorRegister m1 = VectorLoadAligned(M.M[1]);
VectorRegister m2 = VectorLoadAligned(M.M[2]);
VectorRegister m3 = VectorLoadAligned(M.M[3]);
Vertices[0] = VectorLoadFloat3(&Min);
Vertices[1] = VectorSetFloat3(Min.X, Min.Y, Max.Z);
Vertices[2] = VectorSetFloat3(Min.X, Max.Y, Min.Z);
Vertices[3] = VectorSetFloat3(Max.X, Min.Y, Min.Z);
Vertices[4] = VectorSetFloat3(Max.X, Max.Y, Min.Z);
Vertices[5] = VectorSetFloat3(Max.X, Min.Y, Max.Z);
Vertices[6] = VectorSetFloat3(Min.X, Max.Y, Max.Z);
Vertices[7] = VectorLoadFloat3(&Max);
VectorRegister r0 = VectorMultiply(VectorReplicate(Vertices[0],0), m0);
VectorRegister r1 = VectorMultiply(VectorReplicate(Vertices[1],0), m0);
VectorRegister r2 = VectorMultiply(VectorReplicate(Vertices[2],0), m0);
VectorRegister r3 = VectorMultiply(VectorReplicate(Vertices[3],0), m0);
VectorRegister r4 = VectorMultiply(VectorReplicate(Vertices[4],0), m0);
VectorRegister r5 = VectorMultiply(VectorReplicate(Vertices[5],0), m0);
VectorRegister r6 = VectorMultiply(VectorReplicate(Vertices[6],0), m0);
VectorRegister r7 = VectorMultiply(VectorReplicate(Vertices[7],0), m0);
r0 = VectorMultiplyAdd( VectorReplicate(Vertices[0],1), m1, r0);
r1 = VectorMultiplyAdd( VectorReplicate(Vertices[1],1), m1, r1);
r2 = VectorMultiplyAdd( VectorReplicate(Vertices[2],1), m1, r2);
r3 = VectorMultiplyAdd( VectorReplicate(Vertices[3],1), m1, r3);
r4 = VectorMultiplyAdd( VectorReplicate(Vertices[4],1), m1, r4);
r5 = VectorMultiplyAdd( VectorReplicate(Vertices[5],1), m1, r5);
r6 = VectorMultiplyAdd( VectorReplicate(Vertices[6],1), m1, r6);
r7 = VectorMultiplyAdd( VectorReplicate(Vertices[7],1), m1, r7);
r0 = VectorMultiplyAdd( VectorReplicate(Vertices[0],2), m2, r0);
r1 = VectorMultiplyAdd( VectorReplicate(Vertices[1],2), m2, r1);
r2 = VectorMultiplyAdd( VectorReplicate(Vertices[2],2), m2, r2);
r3 = VectorMultiplyAdd( VectorReplicate(Vertices[3],2), m2, r3);
r4 = VectorMultiplyAdd( VectorReplicate(Vertices[4],2), m2, r4);
r5 = VectorMultiplyAdd( VectorReplicate(Vertices[5],2), m2, r5);
r6 = VectorMultiplyAdd( VectorReplicate(Vertices[6],2), m2, r6);
r7 = VectorMultiplyAdd( VectorReplicate(Vertices[7],2), m2, r7);
r0 = VectorAdd(r0, m3);
r1 = VectorAdd(r1, m3);
r2 = VectorAdd(r2, m3);
r3 = VectorAdd(r3, m3);
r4 = VectorAdd(r4, m3);
r5 = VectorAdd(r5, m3);
r6 = VectorAdd(r6, m3);
r7 = VectorAdd(r7, m3);
FBox NewBox;
VectorRegister min0 = VectorMin(r0, r1);
VectorRegister min1 = VectorMin(r2, r3);
VectorRegister min2 = VectorMin(r4, r5);
VectorRegister min3 = VectorMin(r6, r7);
VectorRegister max0 = VectorMax(r0, r1);
VectorRegister max1 = VectorMax(r2, r3);
VectorRegister max2 = VectorMax(r4, r5);
VectorRegister max3 = VectorMax(r6, r7);
min0 = VectorMin(min0, min1);
min1 = VectorMin(min2, min3);
max0 = VectorMax(max0, max1);
max1 = VectorMax(max2, max3);
min0 = VectorMin(min0, min1);
max0 = VectorMax(max0, max1);
VectorStoreFloat3(min0, &NewBox.Min);
VectorStoreFloat3(max0, &NewBox.Max);
NewBox.IsValid = 1;
return NewBox;
}
FOutcode FConvexVolume::GetBoxIntersectionOutcode(const FVector& Origin,const FVector& Extent) const
{
FOutcode Result(1,0);
checkSlow(PermutedPlanes.Num() % 4 == 0);
// Load the origin & extent
VectorRegister Orig = VectorLoadFloat3(&Origin);
VectorRegister Ext = VectorLoadFloat3(&Extent);
// Splat origin into 3 vectors
VectorRegister OrigX = VectorReplicate(Orig, 0);
VectorRegister OrigY = VectorReplicate(Orig, 1);
VectorRegister OrigZ = VectorReplicate(Orig, 2);
// Splat extent into 3 vectors
VectorRegister ExtentX = VectorReplicate(Ext, 0);
VectorRegister ExtentY = VectorReplicate(Ext, 1);
VectorRegister ExtentZ = VectorReplicate(Ext, 2);
// Splat the abs for the pushout calculation
VectorRegister AbsExt = VectorAbs(Ext);
VectorRegister AbsExtentX = VectorReplicate(AbsExt, 0);
VectorRegister AbsExtentY = VectorReplicate(AbsExt, 1);
VectorRegister AbsExtentZ = VectorReplicate(AbsExt, 2);
// Since we are moving straight through get a pointer to the data
const FPlane* RESTRICT PermutedPlanePtr = (FPlane*)PermutedPlanes.GetData();
// Process four planes at a time until we have < 4 left
for (int32 Count = 0; Count < PermutedPlanes.Num(); Count += 4)
{
// Load 4 planes that are already all Xs, Ys, ...
VectorRegister PlanesX = VectorLoadAligned(PermutedPlanePtr);
PermutedPlanePtr++;
VectorRegister PlanesY = VectorLoadAligned(PermutedPlanePtr);
PermutedPlanePtr++;
VectorRegister PlanesZ = VectorLoadAligned(PermutedPlanePtr);
PermutedPlanePtr++;
VectorRegister PlanesW = VectorLoadAligned(PermutedPlanePtr);
PermutedPlanePtr++;
// Calculate the distance (x * x) + (y * y) + (z * z) - w
VectorRegister DistX = VectorMultiply(OrigX,PlanesX);
VectorRegister DistY = VectorMultiplyAdd(OrigY,PlanesY,DistX);
VectorRegister DistZ = VectorMultiplyAdd(OrigZ,PlanesZ,DistY);
VectorRegister Distance = VectorSubtract(DistZ,PlanesW);
// Now do the push out FMath::Abs(x * x) + FMath::Abs(y * y) + FMath::Abs(z * z)
VectorRegister PushX = VectorMultiply(AbsExtentX,VectorAbs(PlanesX));
VectorRegister PushY = VectorMultiplyAdd(AbsExtentY,VectorAbs(PlanesY),PushX);
VectorRegister PushOut = VectorMultiplyAdd(AbsExtentZ,VectorAbs(PlanesZ),PushY);
// Check for completely outside
if (VectorAnyGreaterThan(Distance,PushOut))
{
Result.SetInside(0);
Result.SetOutside(1);
break;
}
// See if any part is outside
if (VectorAnyGreaterThan(Distance,VectorNegate(PushOut)))
{
Result.SetOutside(1);
}
}
return Result;
}
bool FConvexVolume::IntersectBox(const FVector& Origin,const FVector& Extent, bool& bOutFullyContained) const
{
bool Result = true;
// Assume fully contained
bOutFullyContained = true;
checkSlow(PermutedPlanes.Num() % 4 == 0);
// Load the origin & extent
VectorRegister Orig = VectorLoadFloat3(&Origin);
VectorRegister Ext = VectorLoadFloat3(&Extent);
// Splat origin into 3 vectors
VectorRegister OrigX = VectorReplicate(Orig, 0);
VectorRegister OrigY = VectorReplicate(Orig, 1);
VectorRegister OrigZ = VectorReplicate(Orig, 2);
// Splat extent into 3 vectors
VectorRegister ExtentX = VectorReplicate(Ext, 0);
VectorRegister ExtentY = VectorReplicate(Ext, 1);
VectorRegister ExtentZ = VectorReplicate(Ext, 2);
// Splat the abs for the pushout calculation
VectorRegister AbsExt = VectorAbs(Ext);
VectorRegister AbsExtentX = VectorReplicate(AbsExt, 0);
VectorRegister AbsExtentY = VectorReplicate(AbsExt, 1);
VectorRegister AbsExtentZ = VectorReplicate(AbsExt, 2);
// Since we are moving straight through get a pointer to the data
const FPlane* RESTRICT PermutedPlanePtr = (FPlane*)PermutedPlanes.GetData();
// Process four planes at a time until we have < 4 left
for (int32 Count = 0; Count < PermutedPlanes.Num(); Count += 4)
{
// Load 4 planes that are already all Xs, Ys, ...
VectorRegister PlanesX = VectorLoadAligned(PermutedPlanePtr);
PermutedPlanePtr++;
VectorRegister PlanesY = VectorLoadAligned(PermutedPlanePtr);
PermutedPlanePtr++;
VectorRegister PlanesZ = VectorLoadAligned(PermutedPlanePtr);
PermutedPlanePtr++;
VectorRegister PlanesW = VectorLoadAligned(PermutedPlanePtr);
PermutedPlanePtr++;
// Calculate the distance (x * x) + (y * y) + (z * z) - w
VectorRegister DistX = VectorMultiply(OrigX,PlanesX);
VectorRegister DistY = VectorMultiplyAdd(OrigY,PlanesY,DistX);
VectorRegister DistZ = VectorMultiplyAdd(OrigZ,PlanesZ,DistY);
VectorRegister Distance = VectorSubtract(DistZ,PlanesW);
// Now do the push out FMath::Abs(x * x) + FMath::Abs(y * y) + FMath::Abs(z * z)
VectorRegister PushX = VectorMultiply(AbsExtentX,VectorAbs(PlanesX));
VectorRegister PushY = VectorMultiplyAdd(AbsExtentY,VectorAbs(PlanesY),PushX);
VectorRegister PushOut = VectorMultiplyAdd(AbsExtentZ,VectorAbs(PlanesZ),PushY);
VectorRegister PushOutNegative = VectorNegate(PushOut);
// Check for completely outside
if (VectorAnyGreaterThan(Distance,PushOut))
{
Result = false;
bOutFullyContained = false;
break;
}
// Definitely inside frustums, but check to see if it's fully contained
if (VectorAnyGreaterThan(Distance,PushOutNegative))
{
bOutFullyContained = false;
}
}
return Result;
}
