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