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;
}