static bool ComputeInflatedMTD_Internal(const float MtdInflation, const PxLocationHit& PHit, FHitResult& OutResult, const PxTransform& QueryTM, const PxGeometry& Geom, const PxTransform& PShapeWorldPose)
{
PxGeometry* InflatedGeom = NULL;
PxVec3 PxMtdNormal(0.f);
PxF32 PxMtdDepth = 0.f;
const PxGeometry& POtherGeom = PHit.shape->getGeometry().any();
const bool bMtdResult = PxGeometryQuery::computePenetration(PxMtdNormal, PxMtdDepth, Geom, QueryTM, POtherGeom, PShapeWorldPose);
if (bMtdResult)
{
if (PxMtdNormal.isFinite())
{
OutResult.ImpactNormal = P2UVector(PxMtdNormal);
OutResult.PenetrationDepth = FMath::Max(FMath::Abs(PxMtdDepth) - MtdInflation, 0.f) + KINDA_SMALL_NUMBER;
return true;
}
else
{
UE_LOG(LogPhysics, Verbose, TEXT("Warning: ComputeInflatedMTD_Internal: MTD returned NaN :( normal: (X:%f, Y:%f, Z:%f)"), PxMtdNormal.x, PxMtdNormal.y, PxMtdNormal.z);
}
}
return false;
}
/** Util to convert an overlapped shape into a sweep hit result, returns whether it was a blocking hit. */
static bool ConvertOverlappedShapeToImpactHit(const PxShape* PShape, const PxRigidActor* PActor, const FVector& StartLoc, const FVector& EndLoc, FHitResult& OutResult, const PxGeometry& Geom, const PxTransform& QueryTM, const PxFilterData& QueryFilter, bool bReturnPhysMat, uint32 FaceIdx)
{
OutResult.TraceStart = StartLoc;
OutResult.TraceEnd = EndLoc;
SetHitResultFromShapeAndFaceIndex(PShape, PActor, FaceIdx, OutResult, bReturnPhysMat);
// Time of zero because initially overlapping
OutResult.Time = 0.f;
OutResult.bStartPenetrating = true;
// See if this is a 'blocking' hit
PxFilterData PShapeFilter = PShape->getQueryFilterData();
PxSceneQueryHitType::Enum HitType = FPxQueryFilterCallback::CalcQueryHitType(QueryFilter, PShapeFilter);
OutResult.bBlockingHit = (HitType == PxSceneQueryHitType::eBLOCK);
// Return start location as 'safe location'
OutResult.Location = P2UVector(QueryTM.p);
OutResult.ImpactPoint = OutResult.Location; // @todo not really sure of a better thing to do here...
const PxTransform PShapeWorldPose = PxShapeExt::getGlobalPose(*PShape, *PActor);
PxTriangleMeshGeometry PTriMeshGeom;
if(PShape->getTriangleMeshGeometry(PTriMeshGeom))
{
PxU32 HitTris[64];
bool bOverflow = false;
int32 NumTrisHit = PxMeshQuery::findOverlapTriangleMesh(Geom, QueryTM, PTriMeshGeom, PShapeWorldPose, HitTris, 64, 0, bOverflow);
#if DRAW_OVERLAPPING_TRIS
TArray<FOverlapResult> Overlaps;
DrawGeomOverlaps(World, Geom, QueryTM, Overlaps);
TArray<FBatchedLine> Lines;
const FLinearColor LineColor = FLinearColor(1.f,0.7f,0.7f);
const FLinearColor NormalColor = FLinearColor(1.f,1.f,1.f);
const float Lifetime = 5.f;
#endif // DRAW_OVERLAPPING_TRIS
// Track the best triangle plane distance
float BestPlaneDist = -BIG_NUMBER;
FVector BestPlaneNormal(0,0,1);
FVector BestPointOnPlane(0,0,0);
// Iterate over triangles
for(int32 TriIdx = 0; TriIdx<NumTrisHit; TriIdx++)
{
PxTriangle Tri;
PxMeshQuery::getTriangle(PTriMeshGeom, PShapeWorldPose, HitTris[TriIdx], Tri);
const FVector A = P2UVector(Tri.verts[0]);
const FVector B = P2UVector(Tri.verts[1]);
const FVector C = P2UVector(Tri.verts[2]);
FVector TriNormal = ((B-A) ^ (C-A));
// Use a more accurate normalization that avoids InvSqrtEst
const float TriNormalSize = TriNormal.Size();
TriNormal = (TriNormalSize >= KINDA_SMALL_NUMBER ? TriNormal/TriNormalSize : FVector::ZeroVector);
const FPlane TriPlane(A, TriNormal);
const FVector QueryCenter = P2UVector(QueryTM.p);
const float DistToPlane = TriPlane.PlaneDot(QueryCenter);
if(DistToPlane > BestPlaneDist)
{
BestPlaneDist = DistToPlane;
BestPlaneNormal = TriNormal;
BestPointOnPlane = A;
}
#if DRAW_OVERLAPPING_TRIS
Lines.Add(FBatchedLine(A, B, LineColor, Lifetime, 0.1f, SDPG_Foreground));
Lines.Add(FBatchedLine(B, C, LineColor, Lifetime, 0.1f, SDPG_Foreground));
Lines.Add(FBatchedLine(C, A, LineColor, Lifetime, 0.1f, SDPG_Foreground));
Lines.Add(FBatchedLine(A, A+(50.f*TriNormal), NormalColor, Lifetime, 0.1f, SDPG_Foreground));
#endif // DRAW_OVERLAPPING_TRIS
}
#if DRAW_OVERLAPPING_TRIS
if ( World->PersistentLineBatcher )
{
World->PersistentLineBatcher->DrawLines(Lines);
}
#endif // DRAW_OVERLAPPING_TRIS
OutResult.ImpactNormal = BestPlaneNormal;
}
else
{
// use vector center of shape to query as good direction to move in
PxGeometry& PGeom = PShape->getGeometry().any();
PxVec3 PClosestPoint;
float Distance = PxGeometryQuery::pointDistance(QueryTM.p, PGeom, PShapeWorldPose, &PClosestPoint);
if(Distance < KINDA_SMALL_NUMBER)
{
//UE_LOG(LogCollision, Warning, TEXT("ConvertOverlappedShapeToImpactHit: Query origin inside shape, giving poor MTD."));
PClosestPoint = PxShapeExt::getWorldBounds(*PShape, *PActor).getCenter();
}
OutResult.ImpactNormal = (OutResult.Location - P2UVector(PClosestPoint)).SafeNormal();
}
// Compute depenetration vector and distance if possible.
PxVec3 PxMtdNormal(0.f);
PxF32 PxMtdDepth = 0.f;
PxGeometry& POtherGeom = PShape->getGeometry().any();
const bool bMtdResult = PxGeometryQuery::computePenetration(PxMtdNormal, PxMtdDepth, Geom, QueryTM, POtherGeom, PShapeWorldPose);
if (bMtdResult)
{
const FVector MtdNormal = P2UVector(PxMtdNormal);
OutResult.Normal = MtdNormal;
OutResult.PenetrationDepth = FMath::Abs(PxMtdDepth) + KINDA_SMALL_NUMBER; // TODO: why are we getting negative values here from mtd sometimes?
}
else
{
OutResult.Normal = OutResult.ImpactNormal;
}
return OutResult.bBlockingHit;
}