FVector FindBestOverlappingNormal(const UWorld* World, const PxGeometry& Geom, const PxTransform& QueryTM, const GeomType& ShapeGeom, const PxTransform& PShapeWorldPose, PxU32* HitTris, int32 NumTrisHit, bool bCanDrawOverlaps = false)
{
#if DRAW_OVERLAPPING_TRIS
const float Lifetime = 5.f;
bCanDrawOverlaps &= World && World->IsGameWorld() && World->PersistentLineBatcher && (World->PersistentLineBatcher->BatchedLines.Num() < 2048);
if (bCanDrawOverlaps)
{
TArray<FOverlapResult> Overlaps;
DrawGeomOverlaps(World, Geom, QueryTM, Overlaps, Lifetime);
}
const FLinearColor LineColor = FLinearColor::Green;
const FLinearColor NormalColor = FLinearColor::Red;
const FLinearColor PointColor = FLinearColor::Yellow;
#endif // DRAW_OVERLAPPING_TRIS
// Track the best triangle plane distance
float BestPlaneDist = -BIG_NUMBER;
FVector BestPlaneNormal(0, 0, 1);
// Iterate over triangles
for (int32 TriIdx = 0; TriIdx < NumTrisHit; TriIdx++)
{
PxTriangle Tri;
PxMeshQuery::getTriangle(ShapeGeom, 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));
TriNormal = TriNormal.GetSafeNormal();
const FPlane TriPlane(A, TriNormal);
const FVector QueryCenter = P2UVector(QueryTM.p);
const float DistToPlane = TriPlane.PlaneDot(QueryCenter);
if (DistToPlane > BestPlaneDist)
{
BestPlaneDist = DistToPlane;
BestPlaneNormal = TriNormal;
}
#if DRAW_OVERLAPPING_TRIS
if (bCanDrawOverlaps && (World->PersistentLineBatcher->BatchedLines.Num() < 2048))
{
static const float LineThickness = 0.9f;
static const float NormalThickness = 0.75f;
static const float PointThickness = 5.0f;
World->PersistentLineBatcher->DrawLine(A, B, LineColor, SDPG_Foreground, LineThickness, Lifetime);
World->PersistentLineBatcher->DrawLine(B, C, LineColor, SDPG_Foreground, LineThickness, Lifetime);
World->PersistentLineBatcher->DrawLine(C, A, LineColor, SDPG_Foreground, LineThickness, Lifetime);
const FVector Centroid((A + B + C) / 3.f);
World->PersistentLineBatcher->DrawLine(Centroid, Centroid + (35.0f*TriNormal), NormalColor, SDPG_Foreground, NormalThickness, Lifetime);
World->PersistentLineBatcher->DrawPoint(Centroid + (35.0f*TriNormal), NormalColor, PointThickness, SDPG_Foreground, Lifetime);
World->PersistentLineBatcher->DrawPoint(A, PointColor, PointThickness, SDPG_Foreground, Lifetime);
World->PersistentLineBatcher->DrawPoint(B, PointColor, PointThickness, SDPG_Foreground, Lifetime);
World->PersistentLineBatcher->DrawPoint(C, PointColor, PointThickness, SDPG_Foreground, Lifetime);
}
#endif // DRAW_OVERLAPPING_TRIS
}
return BestPlaneNormal;
}
/** 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;
}