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;
}
void ConvertShadowControllerToHL( const shadowcontrol_params_t &in, hlshadowcontrol_params_t &out )
{
ConvertPositionToHL( in.targetPosition, out.targetPosition );
ConvertRotationToHL( in.targetRotation, out.targetRotation );
out.teleportDistance = ConvertDistanceToHL( in.teleportDistance );
ConvertForceImpulseToHL( in.maxSpeed, out.maxSpeed);
VectorAbs( out.maxSpeed, out.maxSpeed );
ConvertAngularImpulseToHL( in.maxAngular, out.maxAngular );
VectorAbs( out.maxAngular, out.maxAngular );
out.dampFactor = in.dampFactor;
}
Vector CPhysicsObject::GetInvInertia() const {
btVector3 btvec = m_pObject->getInvInertiaDiagLocal();
Vector hlvec;
ConvertDirectionToHL(btvec, hlvec);
VectorAbs(hlvec, hlvec);
return hlvec;
}
Vector CPhysicsObject::GetInvInertia( void ) const
{
const IVP_U_Float_Point *pRI = m_pObject->get_core()->get_inv_rot_inertia();
Vector hlInvInertia;
ConvertDirectionToHL( *pRI, hlInvInertia );
VectorAbs( hlInvInertia, hlInvInertia );
return hlInvInertia;
}
TEST(DSPSingle, TestVectorAbs)
{
float out[10];
VectorAbs(out, ramp, 10);
for (unsigned i = 0; i < 10; ++i)
{
ASSERT_FLOAT_EQ(fabsf(ramp[i]), out[i]);
}
}
Vector CPhysicsObject::GetInertia() const {
btVector3 btvec = m_pObject->getInvInertiaDiagLocal();
// Invert the inverse inertia to get inertia
btvec.setX(SAFE_DIVIDE(1.0f, btvec.x()));
btvec.setY(SAFE_DIVIDE(1.0f, btvec.y()));
btvec.setZ(SAFE_DIVIDE(1.0f, btvec.z()));
Vector hlvec;
ConvertDirectionToHL(btvec, hlvec);
VectorAbs(hlvec, hlvec);
return hlvec;
}
/**
* @brief Rotates AABB around given origin point; note that it will expand the box unless all angles are multiples of 90 degrees
* @note Not fully verified so far
*/
void AABB::rotateAround (const vec3_t origin, const vec3_t angles) {
/* reject non-rotations */
if (VectorEmpty(angles))
return;
/* construct box-centered coordinates (center and corners) */
vec3_t center, halfDiagonal;
VectorInterpolation(mins, maxs, 0.5f, center);
VectorSubtract(maxs, center, halfDiagonal);
/* offset coordinate frame to rotation origin */
VectorSubtract(center, origin, center);
/* rotate center by given angles */
vec3_t m[3];
VectorCreateRotationMatrix(angles, m);
vec3_t newCenter;
VectorRotate(m, center, newCenter);
/* short-circuit calculation of the rotated box half-extents */
/* shortcut is: instead of calculating all 8 AABB corners, use the symmetry by rotating box around it's center. */
VectorAbs(m[0]);
VectorAbs(m[1]);
VectorAbs(m[2]);
vec3_t newHalfDiagonal;
VectorRotate(m, halfDiagonal, newHalfDiagonal);
/* de-offset coordinate frame from rotation origin */
VectorAdd(newCenter, origin, newCenter);
/* finally, combine results into new AABB */
VectorAdd(newCenter, newHalfDiagonal, maxs);
VectorSubtract(newCenter, newHalfDiagonal, mins);
}
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;
}
